1 files changed, 61 insertions, 2 deletions

diff --git a/Mode/Mode.cpp b/Mode/Mode.cpp
index 5ae3528..a30ac6e 100644
--- a/Mode/Mode.cpp
+++ b/Mode/Mode.cpp
@@ -3,6 +3,16 @@
 
 const char* Mode::_file_name = "SurveyData.txt";
 
+/*
+ * union for converting from 16- bit to 2 8-bit values
+ */
+union convert16 {
+    int16_t f_s;		// convert from signed 16 bit int
+    uint16_t f_u;		// convert from unsigned 16 bit int
+    uint8_t t_u[2];		// convert to 8 bit unsigned array
+} convertS;
+
+
 Mode::Mode(DOGS102* lcd, ButtonHandler* buttons, mDot* dot, LoRaHandler* lora)
   : _lcd(lcd),
     _buttons(buttons),
@@ -15,7 +25,9 @@ Mode::Mode(DOGS102* lcd, ButtonHandler* buttons, mDot* dot, LoRaHandler* lora)
     _data_rate(mDot::SF_7),
     _power(2),
     _next_tx(0),
-    _send_data(false)
+    _send_data(false),
+	_gpsUART(PA_2, PA_3),
+	_mdot_gps(&_gpsUART)
 {}
 
 Mode::~Mode() {}
@@ -121,7 +133,9 @@ void Mode::updateData(DataItem& data, DataType type, bool status) {
     data.index = _index;
     data.status = status;
     data.lock = 0;
-    // TODO add GPS info
+	data.gps_longitude = _mdot_gps.getLongitude();
+	data.gps_latitude = _mdot_gps.getLatitude();
+	data.gps_altitude = _mdot_gps.getAltitude();
     data.ping = _ping_result;
     data.data_rate = _data_rate;
     data.power = _power;
@@ -129,6 +143,51 @@ void Mode::updateData(DataItem& data, DataType type, bool status) {
 
 std::vector<uint8_t> Mode::formatSurveyData(DataItem& data) {
     std::vector<uint8_t> send_data;
+	uint8_t satfix;
+
+	send_data.clear();
+	send_data.push_back(0x1D);			// key for start of data structure
+	send_data.push_back(0x1A);			// key for uplink QOS + RF Pwr
+	convertS.f_s = data.ping.up.rssi;
+	send_data.push_back(convertS.t_u[1]);
+	send_data.push_back(convertS.t_u[0]);
+	send_data.push_back((data.ping.up.snr/10) & 0xFF);
+	send_data.push_back(data.power);
+
+	send_data.push_back(0x1B);			// key for downlink QOS
+	convertS.f_s=data.ping.down.rssi;
+	send_data.push_back(convertS.t_u[1]);
+	send_data.push_back(convertS.t_u[0]);
+	send_data.push_back(data.ping.down.snr);
+
+	// collect GPS data if GPS device detected
+	if (_mdot_gps.gpsDetected() && ((_data_rate != mDot::SF_10) || (_band == mDot::FB_868))){
+		send_data.push_back(0x19);			// key for GPS Lock Status
+		satfix = (_mdot_gps.getNumSatellites() << 4 ) | (_mdot_gps.getFixStatus() & 0x0F );
+		send_data.push_back(satfix);
+
+		if (_mdot_gps.getLockStatus()){			// if gps has a lock
+   			// Send GPS data if GPS device locked
+			send_data.push_back(0x15);			// key for GPS Latitude
+			send_data.push_back(data.gps_latitude.degrees);
+			send_data.push_back(data.gps_latitude.minutes);
+			convertS.f_s = data.gps_latitude.seconds;
+			send_data.push_back(convertS.t_u[1]);
+			send_data.push_back(convertS.t_u[0]);
+
+			send_data.push_back(0x16);			// key for GPS Longitude
+			convertS.f_s = data.gps_longitude.degrees;
+			send_data.push_back(convertS.t_u[1]);
+			send_data.push_back(convertS.t_u[0]);
+
+			send_data.push_back(data.gps_longitude.minutes);
+			convertS.f_s = data.gps_longitude.seconds;
+			send_data.push_back(convertS.t_u[1]);
+			send_data.push_back(convertS.t_u[0]);
+		}
+	}
+	// key for end of data structure		
+	send_data.push_back(0x1D);					
 
     return send_data;
 }