Completed main implementation and debugging

3 files changed, 110 insertions, 88 deletions

diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c
index 827cb862a..4318244f8 100644
--- a/src/drivers/hott_telemetry/hott_telemetry_main.c
+++ b/src/drivers/hott_telemetry/hott_telemetry_main.c
@@ -138,6 +138,7 @@ recv_req_id(int uart, uint8_t *id)
 
 		/* if we have a binary mode request */
 		if (mode != BINARY_MODE_REQUEST_ID) {
+			warnx("Non binary request ID detected: %d", mode);
 			return ERROR;
 		}
 
@@ -232,6 +233,8 @@ hott_telemetry_thread_main(int argc, char *argv[])
 			}
 
 			send_data(uart, buffer, size);
+		} else {
+			warnx("NOK");
 		}
 	}
 
diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
index feec8998c..1d746506e 100644
--- a/src/drivers/hott_telemetry/messages.c
+++ b/src/drivers/hott_telemetry/messages.c
@@ -40,8 +40,9 @@
 #include "messages.h"
 
 #include <math.h>
+#include <stdio.h>
 #include <string.h>
-#include <systemlib/systemlib.h>
+#include <systemlib/geo/geo.h>
 #include <unistd.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/home_position.h>
@@ -51,11 +52,17 @@
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5
 
+#define ALT_OFFSET 500.0f
+
 static int battery_sub = -1;
 static int gps_sub = -1;
 static int home_sub = -1;
 static int sensor_sub = -1;
 
+static bool home_position_set = false;
+static double home_lat = 0.0d;
+static double home_lon = 0.0d;
+
 void 
 messages_init(void)
 {
@@ -69,15 +76,18 @@ void
 build_eam_response(uint8_t *buffer, size_t *size)
 {
 	/* get a local copy of the current sensor values */
-	struct sensor_combined_s raw = { 0 };
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
 	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
 	/* get a local copy of the battery data */
-	struct battery_status_s battery = { 0 };
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
 	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
-	struct eam_module_msg msg = { 0 };
+	struct eam_module_msg msg;
 	*size = sizeof(msg);
+	memset(&msg, 0, *size);
 
 	msg.start = START_BYTE;
 	msg.eam_sensor_id = EAM_SENSOR_ID;
@@ -92,12 +102,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
-	// TODO: flight time
-	// TODO: climb rate
-	
-
 	msg.stop = STOP_BYTE;
-
 	memcpy(buffer, &msg, *size);
 }
 
@@ -105,102 +110,113 @@ void
 build_gps_response(uint8_t *buffer, size_t *size)
 {
 	/* get a local copy of the current sensor values */
-	struct sensor_combined_s raw = { 0 };
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
 	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
  	/* get a local copy of the battery data */
-	struct vehicle_gps_position_s gps = { 0 };
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
 	orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
 
 	struct gps_module_msg msg = { 0 };
 	*size = sizeof(msg);
+	memset(&msg, 0, *size);
 
 	msg.start = START_BYTE;
 	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;
-	
-	/* 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;
 
 	/* 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);
 
-	msg.stop = STOP_BYTE;
+	/* No point collecting more data if we don't have a 3D fix yet */
+	if (gps.fix_type > 2) {
+		/* 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;
+		
+		/* Get latitude in degrees, minutes and seconds */
+		double lat = ((double)(gps.lat))*1e-7d;
+
+		/* Prepend N or S specifier */		
+		msg.latitude_ns = 0;
+		if (lat < 0) {
+			msg.latitude_ns = 1;
+			lat = abs(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;
+
+		/* Prepend E or W specifier */
+		msg.longitude_ew = 0;
+		if (lon < 0) {
+			msg.longitude_ew = 1;
+			lon = abs(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 + ALT_OFFSET	);
+		msg.altitude_L = (uint8_t)alt & 0xff;
+		msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+
+		/* Get any (and probably only ever one) home_sub postion report */
+		bool updated;
+		orb_check(home_sub, &updated);
+		if (updated) {
+		    /* get a local copy of the home position data */
+			struct home_position_s home;
+			memset(&home, 0, sizeof(home));
+			orb_copy(ORB_ID(home_position), home_sub, &home);
+
+			home_lat = ((double)(home.lat))*1e-7d;
+			home_lon = ((double)(home.lon))*1e-7d;
+			home_position_set = true;
+		}
+
+		/* Distance from home */
+		if (home_position_set) {
+		    uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, 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(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
+			msg.home_direction = (uint8_t)bearing >> 1;
+	    }
+	}
 
+	msg.stop = STOP_BYTE;
 	memcpy(buffer, &msg, *size);
 }
 
@@ -210,6 +226,8 @@ 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);
+    const double min_d = delta * 60.0d;
+    *min = (int)min_d;
+    delta = min_d - *min;
+    *sec = (int)(delta * 10000.0d);
 }
diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h
index 5f7dd5c14..e6d5cc723 100644
--- a/src/drivers/hott_telemetry/messages.h
+++ b/src/drivers/hott_telemetry/messages.h
@@ -189,6 +189,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);
+float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);