altos: Extend GPS altitudes to at least 24 bits everywhere

Telemetry gets a special 'mode' flag indicating that 24-bit data is
present; log files get new data and log readers are expected to detect
that via the firmware version number.

Signed-off-by: Keith Packard <keithp@keithp.com>

10 files changed, 70 insertions, 34 deletions

diff --git a/src/kernel/ao_gps_report.c b/src/kernel/ao_gps_report.c
index 07201ac2..7ef98a97 100644
--- a/src/kernel/ao_gps_report.c
+++ b/src/kernel/ao_gps_report.c
@@ -52,8 +52,12 @@ ao_gps_report(void)
 			gps_log.u.gps_longitude = gps_data.longitude;
 			ao_log_data(&gps_log);
 			gps_log.type = AO_LOG_GPS_ALT;
-			gps_log.u.gps_altitude.altitude = gps_data.altitude;
-			gps_log.u.gps_altitude.unused = 0xffff;
+			gps_log.u.gps_altitude.altitude_low = gps_data.altitude_low;
+#if HAS_WIDE_GPS
+			gps_log.u.gps_altitude.altitude_high = gps_data.altitude_high;
+#else
+			gps_log.u.gps_altitude.altitude_high = 0xffff;
+#endif
 			ao_log_data(&gps_log);
 			if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) {
 				gps_log.type = AO_LOG_GPS_DATE;
diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c
index cb0c0fd9..f3711fb1 100644
--- a/src/kernel/ao_gps_report_mega.c
+++ b/src/kernel/ao_gps_report_mega.c
@@ -78,7 +78,8 @@ ao_gps_report_mega(void)
 #if GPS_SPARSE_LOG
 		/* Don't log data if GPS has a fix and hasn't moved for a while */
 		if ((gps_data.flags & AO_GPS_VALID) &&
-		    !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude, gps_data.altitude))
+		    !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude,
+					      AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data))
 			continue;
 #endif
 		if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
@@ -87,8 +88,8 @@ ao_gps_report_mega(void)
 			gps_log.type = AO_LOG_GPS_TIME;
 			gps_log.u.gps.latitude = gps_data.latitude;
 			gps_log.u.gps.longitude = gps_data.longitude;
-			gps_log.u.gps.altitude = gps_data.altitude;
-
+			gps_log.u.gps.altitude_low = gps_data.altitude_low;
+			gps_log.u.gps.altitude_high = gps_data.altitude_high;
 			gps_log.u.gps.hour = gps_data.hour;
 			gps_log.u.gps.minute = gps_data.minute;
 			gps_log.u.gps.second = gps_data.second;
diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c
index 696a833b..31939385 100644
--- a/src/kernel/ao_gps_report_metrum.c
+++ b/src/kernel/ao_gps_report_metrum.c
@@ -44,7 +44,8 @@ ao_gps_report_metrum(void)
 			gps_log.type = AO_LOG_GPS_POS;
 			gps_log.u.gps.latitude = gps_data.latitude;
 			gps_log.u.gps.longitude = gps_data.longitude;
-			gps_log.u.gps.altitude = gps_data.altitude;
+			gps_log.u.gps.altitude_low = gps_data.altitude_low;
+			gps_log.u.gps.altitude_high = gps_data.altitude_high;
 			ao_log_metrum(&gps_log);
 
 			gps_log.type = AO_LOG_GPS_TIME;
diff --git a/src/kernel/ao_gps_show.c b/src/kernel/ao_gps_show.c
index 3a05e35a..e45cd795 100644
--- a/src/kernel/ao_gps_show.c
+++ b/src/kernel/ao_gps_show.c
@@ -19,6 +19,8 @@
 #include <ao.h>
 #endif
 
+#include <ao_data.h>
+
 void
 ao_gps_show(void) __reentrant
 {
@@ -27,7 +29,11 @@ ao_gps_show(void) __reentrant
 	printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
 	printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
 	printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude);
-	printf ("Alt: %d\n", ao_gps_data.altitude);
+#if HAS_WIDE_GPS
+	printf ("Alt: %ld\n", (long) AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data));
+#else
+	printf ("Alt: %d\n", AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data));
+#endif
 	printf ("Flags: 0x%x\n", ao_gps_data.flags);
 	printf ("Sats: %d", ao_gps_tracking_data.channels);
 	for (i = 0; i < ao_gps_tracking_data.channels; i++)
diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h
index 33cda3eb..080cfb02 100644
--- a/src/kernel/ao_log.h
+++ b/src/kernel/ao_log.h
@@ -176,8 +176,8 @@ struct ao_log_record {
 		int32_t		gps_latitude;
 		int32_t		gps_longitude;
 		struct {
-			int16_t		altitude;
-			uint16_t	unused;
+			uint16_t	altitude_low;
+			int16_t		altitude_high;
 		} gps_altitude;
 		struct {
 			uint16_t	svid;
@@ -246,7 +246,7 @@ struct ao_log_mega {
 		struct {
 			int32_t		latitude;	/* 4 */
 			int32_t		longitude;	/* 8 */
-			int16_t		altitude;	/* 12 */
+			uint16_t	altitude_low;	/* 12 */
 			uint8_t		hour;		/* 14 */
 			uint8_t		minute;		/* 15 */
 			uint8_t		second;		/* 16 */
@@ -261,7 +261,8 @@ struct ao_log_mega {
 			uint8_t		hdop;		/* 27 */
 			uint8_t		vdop;		/* 28 */
 			uint8_t		mode;		/* 29 */
-		} gps;	/* 30 */
+			int16_t		altitude_high;	/* 30 */
+		} gps;	/* 32 */
 		/* AO_LOG_GPS_SAT */
 		struct {
 			uint16_t	channels;	/* 4 */
@@ -273,6 +274,11 @@ struct ao_log_mega {
 	} u;
 };
 
+#define AO_LOG_MEGA_GPS_ALTITUDE(l)	((int32_t) ((l)->u.gps.altitude_high << 16) | ((l)->u.gps.altitude_low))
+#define AO_LOG_MEGA_SET_GPS_ALTITUDE(l,a)	(((l)->u.gps.mode |= AO_GPS_MODE_ALTITUDE_24), \
+						 ((l)->u.gps.altitude_high = (a) >> 16), \
+						 (l)->u.gps.altitude_low = (a))
+
 struct ao_log_metrum {
 	char			type;			/* 0 */
 	uint8_t			csum;			/* 1 */
@@ -306,8 +312,9 @@ struct ao_log_metrum {
 		struct {
 			int32_t		latitude;	/* 4 */
 			int32_t		longitude;	/* 8 */
-			int16_t		altitude;	/* 12 */
-		} gps;		/* 14 */
+			uint16_t	altitude_low;	/* 12 */
+			int16_t		altitude_high;	/* 14 */
+		} gps;		/* 16 */
 		/* AO_LOG_GPS_TIME */
 		struct {
 			uint8_t		hour;		/* 4 */
@@ -381,7 +388,7 @@ struct ao_log_gps {
 		struct {
 			int32_t		latitude;	/* 4 */
 			int32_t		longitude;	/* 8 */
-			int16_t		altitude;	/* 12 */
+			uint16_t	altitude_low;	/* 12 */
 			uint8_t		hour;		/* 14 */
 			uint8_t		minute;		/* 15 */
 			uint8_t		second;		/* 16 */
@@ -396,7 +403,7 @@ struct ao_log_gps {
 			uint8_t		hdop;		/* 27 */
 			uint8_t		vdop;		/* 28 */
 			uint8_t		mode;		/* 29 */
-			uint8_t		state;		/* 30 */
+			int16_t		altitude_high;	/* 30 */
 		} gps;	/* 31 */
 		/* AO_LOG_GPS_SAT */
 		struct {
diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c
index 3b728c25..a5a6358b 100644
--- a/src/kernel/ao_log_gps.c
+++ b/src/kernel/ao_log_gps.c
@@ -75,7 +75,8 @@ ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data)
 	log.type = AO_LOG_GPS_TIME;
 	log.u.gps.latitude = gps_data->latitude;
 	log.u.gps.longitude = gps_data->longitude;
-	log.u.gps.altitude = gps_data->altitude;
+	log.u.gps.altitude_low = gps_data->altitude_low;
+	log.u.gps.altitude_high = gps_data->altitude_high;
 
 	log.u.gps.hour = gps_data->hour;
 	log.u.gps.minute = gps_data->minute;
diff --git a/src/kernel/ao_log_gps.h b/src/kernel/ao_log_gps.h
index 5851f4d1..a9e8c831 100644
--- a/src/kernel/ao_log_gps.h
+++ b/src/kernel/ao_log_gps.h
@@ -21,9 +21,6 @@
 #ifndef _AO_LOG_GPS_H_
 #define _AO_LOG_GPS_H_
 
-uint8_t
-ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt);
-
 void
 ao_log_gps_flight(void);
 
diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c
index d321c8ff..56bd715e 100644
--- a/src/kernel/ao_telemetry.c
+++ b/src/kernel/ao_telemetry.c
@@ -344,7 +344,7 @@ ao_send_location(void)
 		ao_mutex_get(&ao_gps_mutex);
 		ao_xmemcpy(&telemetry.location.flags,
 		       &ao_gps_data.flags,
-		       26);
+		       27);
 		telemetry.location.tick = ao_gps_tick;
 		ao_mutex_put(&ao_gps_mutex);
 		ao_radio_send(&telemetry, sizeof (telemetry));
diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h
index be7d0340..83d432cf 100644
--- a/src/kernel/ao_telemetry.h
+++ b/src/kernel/ao_telemetry.h
@@ -86,12 +86,9 @@ struct ao_telemetry_configuration {
 
 #define AO_TELEMETRY_LOCATION		0x05
 
-#define AO_GPS_MODE_NOT_VALID		'N'
-#define AO_GPS_MODE_AUTONOMOUS		'A'
-#define AO_GPS_MODE_DIFFERENTIAL	'D'
-#define AO_GPS_MODE_ESTIMATED		'E'
-#define AO_GPS_MODE_MANUAL		'M'
-#define AO_GPS_MODE_SIMULATED		'S'
+/* Mode bits */
+
+#define AO_GPS_MODE_ALTITUDE_24		(1 << 0)	/* reports 24-bits of altitude */
 
 struct ao_telemetry_location {
 	uint16_t	serial;		/*  0 */
@@ -99,7 +96,7 @@ struct ao_telemetry_location {
 	uint8_t		type;		/*  4 */
 
 	uint8_t         flags;		/*  5 Number of sats and other flags */
-	int16_t         altitude;	/*  6 GPS reported altitude (m) */
+	uint16_t        altitude_low;	/*  6 GPS reported altitude (m) */
 	int32_t         latitude;	/*  8 latitude (degrees * 10⁷) */
 	int32_t         longitude;	/* 12 longitude (degrees * 10⁷) */
 	uint8_t         year;		/* 16 (- 2000) */
@@ -115,10 +112,31 @@ struct ao_telemetry_location {
 	uint16_t	ground_speed;	/* 26 cm/s */
 	int16_t		climb_rate;	/* 28 cm/s */
 	uint8_t		course;		/* 30 degrees / 2 */
-	uint8_t		unused;		/* 31 unused */
+	int8_t		altitude_high;	/* 31 high byte of altitude */
 	/* 32 */
 };
 
+#if HAS_GPS
+
+#ifndef HAS_WIDE_GPS
+#define HAS_WIDE_GPS	1
+#endif
+
+#if HAS_WIDE_GPS
+typedef int32_t		gps_alt_t;
+#define AO_TELEMETRY_LOCATION_ALTITUDE(l) 	(((gps_alt_t) (l)->altitude_high << 16) | ((l)->altitude_low))
+#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->mode |= AO_GPS_MODE_ALTITUDE_24), \
+						 ((l)->altitude_high = (a) >> 16), \
+						 ((l)->altitude_low = (a)))
+#else
+typedef int16_t		gps_alt_t;
+#define AO_TELEMETRY_LOCATION_ALTITUDE(l)	((gps_alt_t) (l)->altitude_low)
+#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a)	(((l)->mode = 0, \
+						  (l)->altitude_low = (a)))
+#endif /* HAS_WIDE_GPS */
+
+#endif /* HAS_GPS */
+
 #define AO_TELEMETRY_SATELLITE		0x06
 
 struct ao_telemetry_satellite_info {
diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c
index 9febc7f0..d9434048 100644
--- a/src/kernel/ao_tracker.c
+++ b/src/kernel/ao_tracker.c
@@ -36,9 +36,9 @@ ao_usb_connected(void)
 #endif
 
 struct gps_position {
-	int32_t	latitude;
-	int32_t	longitude;
-	int16_t	altitude;
+	int32_t		latitude;
+	int32_t		longitude;
+	gps_alt_t	altitude;
 };
 
 #define GPS_RING	16
@@ -122,12 +122,13 @@ ao_tracker(void)
 			{
 				uint8_t	ring;
 				uint8_t	moving = 0;
+				gps_alt_t altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data);
 
 				for (ring = ao_gps_ring_next(gps_head); ring != gps_head; ring = ao_gps_ring_next(ring)) {
 					ground_distance = ao_distance(gps_data.latitude, gps_data.longitude,
 								      gps_position[ring].latitude,
 								      gps_position[ring].longitude);
-					height = gps_position[ring].altitude - gps_data.altitude;
+					height = gps_position[ring].altitude - altitude;
 					if (height < 0)
 						height = -height;
 
@@ -153,7 +154,7 @@ ao_tracker(void)
 					ao_log_gps_data(gps_tick, &gps_data);
 					gps_position[gps_head].latitude = gps_data.latitude;
 					gps_position[gps_head].longitude = gps_data.longitude;
-					gps_position[gps_head].altitude = gps_data.altitude;
+					gps_position[gps_head].altitude = altitude;
 					gps_head = ao_gps_ring_next(gps_head);
 					ao_mutex_put(&tracker_mutex);
 				}
@@ -203,7 +204,7 @@ ao_tracker_set_telem(void)
 	printf ("log_started: %d\n", log_started);
 	printf ("latitude: %ld\n", (long) gps_data.latitude);
 	printf ("longitude: %ld\n", (long) gps_data.longitude);
-	printf ("altitude: %d\n", gps_data.altitude);
+	printf ("altitude: %ld\n", AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data));
 	printf ("log_running: %d\n", ao_log_running);
 	printf ("log_start_pos: %ld\n", (long) ao_log_start_pos);
 	printf ("log_cur_pos: %ld\n", (long) ao_log_current_pos);