GPS support - mostly working.

3 files changed, 87 insertions, 6 deletions

diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c
index 33b721962..f419c95de 100644
--- a/apps/hott_telemetry/hott_telemetry_main.c
+++ b/apps/hott_telemetry/hott_telemetry_main.c
@@ -230,7 +230,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
 				break;
 
 			case GPS_SENSOR_ID:
-				build_eam_response(buffer, &size);
+				build_gps_response(buffer, &size);
 				break;
 
 			default:
diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c
index 2e1a5c064..feec8998c 100644
--- a/apps/hott_telemetry/messages.c
+++ b/apps/hott_telemetry/messages.c
@@ -39,10 +39,12 @@
 
 #include "messages.h"
 
+#include <math.h>
 #include <string.h>
 #include <systemlib/systemlib.h>
 #include <unistd.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/home_position.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
@@ -51,6 +53,7 @@
 
 static int battery_sub = -1;
 static int gps_sub = -1;
+static int home_sub = -1;
 static int sensor_sub = -1;
 
 void 
@@ -58,6 +61,7 @@ messages_init(void)
 {
 	battery_sub = orb_subscribe(ORB_ID(battery_status));
 	gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	home_sub = orb_subscribe(ORB_ID(home_position));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 }
 
@@ -115,22 +119,97 @@ build_gps_response(uint8_t *buffer, size_t *size)
 	msg.sensor_id = GPS_SENSOR_ID;
 	msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
 
+	/* Current flight direction */
+	msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
+
+	/* GPS speed */
 	uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
 	msg.gps_speed_L = (uint8_t)speed & 0xff;
 	msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
 	
-	uint16_t alt = (uint16_t)(gps.alt);
+	/* Get latitude in degrees, minutes and seconds */
+	double lat = ((double)(gps.lat)) * 1e-7d;
+
+	msg.latitude_ns = 0;
+	if (lat < 0) {
+		msg.latitude_ns = 1;
+		lat = -lat;
+	}
+
+	int deg;
+	int min;
+	int sec;
+	convert_to_degrees_minutes_seconds(lat, &deg, &min, &sec);
+
+	uint16_t lat_min = (uint16_t)(deg * 100 + min);
+	msg.latitude_min_L = (uint8_t)lat_min & 0xff;
+	msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
+	uint16_t lat_sec = (uint16_t)(sec);
+	msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
+	msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
+
+
+	/* Get longitude in degrees, minutes and seconds */
+	double lon = ((double)(gps.lon)) * 1e-7d;
+
+	msg.longitude_ew = 0;
+	if (lon < 0) {
+		msg.longitude_ew = 1;
+		lon = -lon;
+	}
+
+	convert_to_degrees_minutes_seconds(lon, &deg, &min, &sec);
+	
+	uint16_t lon_min = (uint16_t)(deg * 100 + min);
+	msg.longitude_min_L = (uint8_t)lon_min & 0xff;
+	msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
+	uint16_t lon_sec = (uint16_t)(sec);
+	msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
+	msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
+	
+	/* Altitude */
+	uint16_t alt = (uint16_t)(gps.alt * 1e-3 + 500.0f);
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
+	/* Distance from home */
+	bool updated;
+	orb_check(home_sub, &updated);
+	if (updated) {
+	    /* get a local copy of the home position data */
+		struct home_position_s home = { 0 };
+		orb_copy(ORB_ID(home_position), home_sub, &home);
+
+	    uint16_t dist = (uint16_t)get_distance_to_next_waypoint(
+	    	(double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon);
+	    warnx("dist %d home.lat %3.6f home.lon %3.6f lat %3.6f lon %3.6f ", 
+	    	  dist, (double)home.lat*1e-7d, (double)home.lon*1e-7d, lat, lon);
+		msg.distance_L = (uint8_t)dist & 0xff;
+		msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
+
+		/* Direction back to home */
+		uint16_t bearing = (uint16_t)get_bearing_to_next_waypoint(
+	    	(double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon) * M_RAD_TO_DEG_F;
+		msg.home_direction = (uint8_t)bearing >> 1;
+    }
+
 	msg.gps_num_sat = gps.satellites_visible;
 
-	/* Display the fix type: 0 = none, 2 = 2D, 3 = 3D */
+	/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
+	msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
 	msg.gps_fix = (uint8_t)(gps.fix_type + 48);
 
-	// TODO(sjwilks): Complete.
-
 	msg.stop = STOP_BYTE;
 
 	memcpy(buffer, &msg, *size);
 }
+
+void
+convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
+{
+    *deg = (int)val;
+
+    double delta = val - *deg;
+    *min = (int)(delta * 60.0);
+    *sec = (int)(delta * 3600.0);
+}
diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h
index c83672c08..4ccc1b311 100644
--- a/apps/hott_telemetry/messages.h
+++ b/apps/hott_telemetry/messages.h
@@ -145,7 +145,7 @@ struct gps_module_msg {
 	uint8_t latitude_sec_H;		/**< Byte 14: 016 3 = 0x03 */
 	 
 	uint8_t longitude_ew;		/**< Byte 15: 000  = E= 9° 25’9360 */
-	uint8_t longitude_min_L;		/**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
+	uint8_t longitude_min_L;	/**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
 	uint8_t longitude_min_H;	/**< Byte 17: 003 3 = 0x03 */
 	uint8_t longitude_sec_L;	/**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
 	uint8_t longitude_sec_H;	/**< Byte 19: 004 36 = 0x24 */
@@ -188,5 +188,7 @@ struct gps_module_msg {
 void messages_init(void);
 void build_eam_response(uint8_t *buffer, size_t *size);
 void build_gps_response(uint8_t *buffer, size_t *size);
+void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
+
 
 #endif /* MESSAGES_H_ */