Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

48 files changed, 398 insertions, 211 deletions

diff --git a/src/Makefile b/src/Makefile
index a7a26b26..3494ba62 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -109,7 +109,7 @@ altitude-pa.h: make-altitude-pa
 altitude-pa-small.h: make-altitude-pa
 	nickle $< --sample 3 > $@
 
-ao_kalman.h: make-kalman kalman.5c kalman_filter.5c load_csv.5c matrix.5c
+ao_kalman.h: make-kalman kalman.5c kalman_micro.5c kalman_filter.5c load_csv.5c matrix.5c
 	bash $< kalman > $@
 
 ao_whiten.h: make-whiten
diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h
index 1bc3d716..e12f813f 100644
--- a/src/cc1111/ao_pins.h
+++ b/src/cc1111/ao_pins.h
@@ -20,6 +20,8 @@
 
 #define HAS_RADIO		1
 #define DISABLE_LOG_SPACE	1
+#define AO_VALUE_32		0
+#define HAS_WIDE_GPS		0
 
 #if defined(TELEMETRUM_V_1_0)
 	/* Discontinued and was never built with CC1111 chips needing this */
diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c
index a9047149..19beb78f 100644
--- a/src/drivers/ao_aprs.c
+++ b/src/drivers/ao_aprs.c
@@ -713,7 +713,7 @@ static int tncPositionPacket(void)
     if (ao_gps_data.flags & AO_GPS_VALID) {
 	latitude = ao_gps_data.latitude;
 	longitude = ao_gps_data.longitude;
-	altitude = ao_gps_data.altitude;
+	altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data);
 	if (altitude < 0)
 	    altitude = 0;
     }
diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
index 944a37f9..066df6ff 100644
--- a/src/drivers/ao_gps_skytraq.c
+++ b/src/drivers/ao_gps_skytraq.c
@@ -278,16 +278,17 @@ ao_nmea_gga(void)
 
 	ao_gps_lexchar();
 	i = ao_gps_decimal(0xff);
-	if (i <= 50) {
-		i = (uint8_t) 5 * i;
+	if (i <= 25) {
+		i = (uint8_t) 10 * i;
 		if (ao_gps_char == '.')
-			i = (i + ((uint8_t) ao_gps_decimal(1) >> 1));
+			i = (i + ((uint8_t) ao_gps_decimal(1)));
 	} else
 		i = 255;
 	ao_gps_next.hdop = i;
 	ao_gps_skip_field();
 
-	ao_gps_next.altitude = ao_gps_decimal(0xff);
+	AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_next, ao_gps_decimal(0xff));
+
 	ao_gps_skip_field();	/* skip any fractional portion */
 
 	ao_nmea_finish();
diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index 077698a9..74c29e0a 100644
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -728,7 +728,7 @@ ao_gps(void) __reentrant
 				if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
 					ao_gps_data.flags |= AO_GPS_DATE_VALID;
 
-				ao_gps_data.altitude = nav_posllh.alt_msl / 1000;
+				AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000);
 				ao_gps_data.latitude = nav_posllh.lat;
 				ao_gps_data.longitude = nav_posllh.lon;
 
@@ -740,11 +740,11 @@ ao_gps(void) __reentrant
 				ao_gps_data.minute = nav_timeutc.min;
 				ao_gps_data.second = nav_timeutc.sec;
 
-				ao_gps_data.pdop = nav_dop.pdop;
-				ao_gps_data.hdop = nav_dop.hdop;
-				ao_gps_data.vdop = nav_dop.vdop;
-
-				/* mode is not set */
+				/* we report dop scaled by 10, but ublox provides dop scaled by 100
+				 */
+				ao_gps_data.pdop = nav_dop.pdop / 10;
+				ao_gps_data.hdop = nav_dop.hdop / 10;
+				ao_gps_data.vdop = nav_dop.vdop / 10;
 
 				ao_gps_data.ground_speed = nav_velned.g_speed;
 				ao_gps_data.climb_rate = -nav_velned.vel_d;
diff --git a/src/drivers/ao_lco_func.c b/src/drivers/ao_lco_func.c
index 9e642836..32c00068 100644
--- a/src/drivers/ao_lco_func.c
+++ b/src/drivers/ao_lco_func.c
@@ -28,7 +28,21 @@ ao_lco_query(uint16_t box, struct ao_pad_query *query, uint16_t *tick_offset)
 {
 	int8_t		r;
 	uint16_t	sent_time;
+	uint16_t	timeout = AO_MS_TO_TICKS(10);
 
+#if HAS_RADIO_RATE
+	switch (ao_config.radio_rate) {
+	case AO_RADIO_RATE_38400:
+	default:
+		break;
+	case AO_RADIO_RATE_9600:
+		timeout = AO_MS_TO_TICKS(20);
+		break;
+	case AO_RADIO_RATE_2400:
+		timeout = AO_MS_TO_TICKS(80);
+		break;
+	}
+#endif
 	ao_mutex_get(&ao_lco_mutex);
 	command.tick = ao_time() - *tick_offset;
 	command.box = box;
@@ -36,7 +50,7 @@ ao_lco_query(uint16_t box, struct ao_pad_query *query, uint16_t *tick_offset)
 	command.channels = 0;
 	ao_radio_cmac_send(&command, sizeof (command));
 	sent_time = ao_time();
-	r = ao_radio_cmac_recv(query, sizeof (*query), AO_MS_TO_TICKS(10));
+	r = ao_radio_cmac_recv(query, sizeof (*query), timeout);
 	if (r == AO_RADIO_CMAC_OK)
 		*tick_offset = sent_time - query->tick;
 	ao_mutex_put(&ao_lco_mutex);
diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c
index 46fa8e1f..55fde04c 100755
--- a/src/kalman/kalman.5c
+++ b/src/kalman/kalman.5c
@@ -457,7 +457,7 @@ void main() {
 					name = sprintf("%s_K%d_%d", prefix, i, time_inc);
 				else
 					name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
-				printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]);
+				printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]);
 			}
 		printf ("\n");
 		exit(0);
diff --git a/src/kalman/kalman_micro.5c b/src/kalman/kalman_micro.5c
index 266a1182..1b080384 100644
--- a/src/kalman/kalman_micro.5c
+++ b/src/kalman/kalman_micro.5c
@@ -307,7 +307,7 @@ void main() {
 					name = sprintf("%s_K%d_%d", prefix, i, time_inc);
 				else
 					name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
-				printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]);
+				printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]);
 			}
 		printf ("\n");
 		exit(0);
diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index a225bc4a..ad5bbf8e 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -278,15 +278,17 @@ ao_report_init(void);
  * Given raw data, convert to SI units
  */
 
+#if HAS_BARO
 /* pressure from the sensor to altitude in meters */
-int16_t
-ao_pres_to_altitude(int16_t pres) __reentrant;
+alt_t
+ao_pres_to_altitude(pres_t pres) __reentrant;
 
-int16_t
-ao_altitude_to_pres(int16_t alt) __reentrant;
+pres_t
+ao_altitude_to_pres(alt_t alt) __reentrant;
 
 int16_t
 ao_temp_to_dC(int16_t temp) __reentrant;
+#endif
 
 /*
  * ao_convert_pa.c
@@ -296,11 +298,13 @@ ao_temp_to_dC(int16_t temp) __reentrant;
 
 #include <ao_data.h>
 
+#if HAS_BARO
 alt_t
-ao_pa_to_altitude(int32_t pa);
+ao_pa_to_altitude(pres_t pa);
 
 int32_t
 ao_altitude_to_pa(alt_t alt);
+#endif
 
 #if HAS_DBG
 #include <ao_dbg.h>
diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c
index 0052bdce..d2f583ef 100644
--- a/src/kernel/ao_cmd.c
+++ b/src/kernel/ao_cmd.c
@@ -290,6 +290,9 @@ version(void)
 #if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND)
 	       "program-space    %u\n"
 #endif
+#if AO_VALUE_32
+	       "altitude-32      1\n"
+#endif
 	       , ao_manufacturer
 	       , ao_product
 	       , ao_serial_number
diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
index 32a0967c..d73a3733 100644
--- a/src/kernel/ao_config.c
+++ b/src/kernel/ao_config.c
@@ -518,6 +518,9 @@ ao_config_radio_rate_set(void) __reentrant
 	ao_telemetry_reset_interval();
 #endif
 	_ao_config_edit_finish();
+#if HAS_RADIO_RECV
+	ao_radio_recv_abort();
+#endif
 }
 #endif
 
diff --git a/src/kernel/ao_convert.c b/src/kernel/ao_convert.c
index aa9b5f48..db1f2301 100644
--- a/src/kernel/ao_convert.c
+++ b/src/kernel/ao_convert.c
@@ -19,14 +19,16 @@
 #include "ao.h"
 #endif
 
-static const int16_t altitude_table[] = {
+#include <ao_sample.h>
+
+static const ao_v_t altitude_table[] = {
 #include "altitude.h"
 };
 
 #define ALT_FRAC_SCALE	(1 << ALT_FRAC_BITS)
 #define ALT_FRAC_MASK	(ALT_FRAC_SCALE - 1)
 
-int16_t
+ao_v_t
 ao_pres_to_altitude(int16_t pres) __reentrant
 {
 	uint8_t	o;
@@ -43,9 +45,9 @@ ao_pres_to_altitude(int16_t pres) __reentrant
 
 #if AO_NEED_ALTITUDE_TO_PRES
 int16_t
-ao_altitude_to_pres(int16_t alt) __reentrant
+ao_altitude_to_pres(ao_v_t alt) __reentrant
 {
-	int16_t span, sub_span;
+	ao_v_t span, sub_span;
 	uint8_t	l, h, m;
 	int32_t pres;
 
diff --git a/src/kernel/ao_convert_pa.c b/src/kernel/ao_convert_pa.c
index 20162c1f..410815b6 100644
--- a/src/kernel/ao_convert_pa.c
+++ b/src/kernel/ao_convert_pa.c
@@ -39,11 +39,11 @@ static const alt_t altitude_table[] AO_CONST_ATTRIB = {
 #define ALT_MASK	(ALT_SCALE - 1)
 
 alt_t
-ao_pa_to_altitude(int32_t pa)
+ao_pa_to_altitude(pres_t pa)
 {
 	int16_t	o;
 	int16_t	part;
-	int32_t low, high;
+	alt_t low, high;
 
 	if (pa < 0)
 		pa = 0;
@@ -52,16 +52,16 @@ ao_pa_to_altitude(int32_t pa)
 	o = pa >> ALT_SHIFT;
 	part = pa & ALT_MASK;
 
-	low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part);
-	high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1);
+	low = (alt_t) FETCH_ALT(o) * (ALT_SCALE - part);
+	high = (alt_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1);
 	return (low + high) >> ALT_SHIFT;
 }
 
 #ifdef AO_CONVERT_TEST
-int32_t
-ao_altitude_to_pa(int32_t alt)
+pres_t
+ao_altitude_to_pa(alt_t alt)
 {
-	int32_t 	span, sub_span;
+	alt_t 	span, sub_span;
 	uint16_t	l, h, m;
 	int32_t 	pa;
 
@@ -76,7 +76,7 @@ ao_altitude_to_pa(int32_t alt)
 	}
 	span = altitude_table[l] - altitude_table[h];
 	sub_span = altitude_table[l] - alt;
-	pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span;
+	pa = ((((alt_t) l * (span - sub_span) + (alt_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span;
 	if (pa > 120000)
 		pa = 120000;
 	if (pa < 0)
diff --git a/src/kernel/ao_convert_pa_test.c b/src/kernel/ao_convert_pa_test.c
index 7d5b1922..95422862 100644
--- a/src/kernel/ao_convert_pa_test.c
+++ b/src/kernel/ao_convert_pa_test.c
@@ -18,6 +18,7 @@
 #include <stdint.h>
 #define AO_CONVERT_TEST
 typedef int32_t alt_t;
+typedef int32_t pres_t;
 #include "ao_host.h"
 #include "ao_convert_pa.c"
 
diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index c4b062fd..8f75ad87 100644
--- a/src/kernel/ao_data.h
+++ b/src/kernel/ao_data.h
@@ -117,9 +117,7 @@ extern volatile __data uint8_t		ao_data_count;
 
 typedef int32_t	pres_t;
 
-#ifndef AO_ALT_TYPE
 #define AO_ALT_TYPE	int32_t
-#endif
 
 typedef AO_ALT_TYPE	alt_t;
 
@@ -146,10 +144,6 @@ typedef int16_t alt_t;
 
 #endif
 
-#if !HAS_BARO
-typedef int16_t alt_t;
-#endif
-
 /*
  * Need a few macros to pull data from the sensors:
  *
diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 2b433ee9..251dbc02 100644
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -60,10 +60,10 @@ __xdata uint8_t			ao_sensor_errors;
  * resting
  */
 static __data uint16_t		ao_interval_end;
-static __data int16_t		ao_interval_min_height;
-static __data int16_t		ao_interval_max_height;
+static __data ao_v_t		ao_interval_min_height;
+static __data ao_v_t		ao_interval_max_height;
 #if HAS_ACCEL
-static __data int16_t		ao_coast_avg_accel;
+static __data ao_v_t		ao_coast_avg_accel;
 #endif
 
 __pdata uint8_t			ao_flight_force_idle;
@@ -398,14 +398,14 @@ ao_flight(void)
 }
 
 #if HAS_FLIGHT_DEBUG
-static inline int int_part(int16_t i)	{ return i >> 4; }
-static inline int frac_part(int16_t i)	{ return ((i & 0xf) * 100 + 8) / 16; }
+static inline int int_part(ao_v_t i)	{ return i >> 4; }
+static inline int frac_part(ao_v_t i)	{ return ((i & 0xf) * 100 + 8) / 16; }
 
 static void
 ao_flight_dump(void)
 {
 #if HAS_ACCEL
-	int16_t	accel;
+	ao_v_t	accel;
 
 	accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
 #endif
diff --git a/src/kernel/ao_gps_print.c b/src/kernel/ao_gps_print.c
index 47c945d7..d26021da 100644
--- a/src/kernel/ao_gps_print.c
+++ b/src/kernel/ao_gps_print.c
@@ -20,6 +20,10 @@
 #endif
 #include "ao_telem.h"
 
+#ifndef AO_TELEMETRY_LOCATION_ALTITUDE
+#define AO_TELEMETRY_LOCATION_ALTITUDE(l)	((l)->altitude)
+#endif
+
 void
 ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant
 {
@@ -42,7 +46,7 @@ ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant
 	       AO_TELEM_GPS_ALTITUDE " %d ",
 	       (long) gps_data->latitude,
 	       (long) gps_data->longitude,
-	       gps_data->altitude);
+	       AO_TELEMETRY_LOCATION_ALTITUDE(gps_data));
 
 	if (gps_data->flags & AO_GPS_DATE_VALID)
 		printf(AO_TELEM_GPS_YEAR " %d "
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..8ce074fe 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;
@@ -55,6 +56,7 @@ ao_gps_report_metrum(void)
 			gps_log.u.gps_time.year = gps_data.year;
 			gps_log.u.gps_time.month = gps_data.month;
 			gps_log.u.gps_time.day = gps_data.day;
+			gps_log.u.gps_time.pdop = gps_data.pdop;
 			ao_log_metrum(&gps_log);
 		}
 
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_kalman.c b/src/kernel/ao_kalman.c
index 9aea1f14..7b0f8145 100644
--- a/src/kernel/ao_kalman.c
+++ b/src/kernel/ao_kalman.c
@@ -23,32 +23,31 @@
 #include "ao_sample.h"
 #include "ao_kalman.h"
 
+static __pdata ao_k_t		ao_k_height;
+static __pdata ao_k_t		ao_k_speed;
+static __pdata ao_k_t		ao_k_accel;
 
-static __pdata int32_t		ao_k_height;
-static __pdata int32_t		ao_k_speed;
-static __pdata int32_t		ao_k_accel;
+#define AO_K_STEP_100		to_fix_v(0.01)
+#define AO_K_STEP_2_2_100	to_fix_v(0.00005)
 
-#define AO_K_STEP_100		to_fix16(0.01)
-#define AO_K_STEP_2_2_100	to_fix16(0.00005)
+#define AO_K_STEP_10		to_fix_v(0.1)
+#define AO_K_STEP_2_2_10	to_fix_v(0.005)
 
-#define AO_K_STEP_10		to_fix16(0.1)
-#define AO_K_STEP_2_2_10	to_fix16(0.005)
+#define AO_K_STEP_1		to_fix_v(1)
+#define AO_K_STEP_2_2_1		to_fix_v(0.5)
 
-#define AO_K_STEP_1		to_fix16(1)
-#define AO_K_STEP_2_2_1		to_fix16(0.5)
+__pdata ao_v_t			ao_height;
+__pdata ao_v_t			ao_speed;
+__pdata ao_v_t			ao_accel;
+__xdata ao_v_t			ao_max_height;
+static __pdata ao_k_t		ao_avg_height_scaled;
+__xdata ao_v_t			ao_avg_height;
 
-__pdata int16_t			ao_height;
-__pdata int16_t			ao_speed;
-__pdata int16_t			ao_accel;
-__xdata int16_t			ao_max_height;
-static __pdata int32_t		ao_avg_height_scaled;
-__xdata int16_t			ao_avg_height;
-
-__pdata int16_t			ao_error_h;
-__pdata int16_t			ao_error_h_sq_avg;
+__pdata ao_v_t			ao_error_h;
+__pdata ao_v_t			ao_error_h_sq_avg;
 
 #if HAS_ACCEL
-__pdata int16_t			ao_error_a;
+__pdata ao_v_t			ao_error_a;
 #endif
 
 static void
@@ -56,40 +55,40 @@ ao_kalman_predict(void)
 {
 #ifdef AO_FLIGHT_TEST
 	if (ao_sample_tick - ao_sample_prev_tick > 50) {
-		ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 +
-				(int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
-		ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1;
+		ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_1 +
+				(ao_k_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
+		ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_1;
 
 		return;
 	}
 	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 +
-				(int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
-		ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10;
+		ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_10 +
+				(ao_k_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
+		ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_10;
 
 		return;
 	}
 	if (ao_flight_debug) {
 		printf ("predict speed %g + (%g * %g) = %g\n",
 			ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0,
-			(ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
+			(ao_k_speed + (ao_k_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
 	}
 #endif
-	ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 +
-			(int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
-	ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100;
+	ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_100 +
+			(ao_k_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
+	ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100;
 }
 
 static void
 ao_kalman_err_height(void)
 {
-	int16_t	e;
-	int16_t height_distrust;
+	ao_v_t	e;
+	ao_v_t height_distrust;
 #if HAS_ACCEL
-	int16_t	speed_distrust;
+	ao_v_t	speed_distrust;
 #endif
 
-	ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16);
+	ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16);
 
 	e = ao_error_h;
 	if (e < 0)
@@ -123,7 +122,7 @@ ao_kalman_err_height(void)
 #endif
 		if (height_distrust > 0x100)
 			height_distrust = 0x100;
-		ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8);
+		ao_error_h = (ao_v_t) (((ao_k_t) ao_error_h * (0x100 - height_distrust)) >> 8);
 #ifdef AO_FLIGHT_TEST
 		if (ao_flight_debug) {
 			printf("over height %g over speed %g distrust: %g height: error %d -> %d\n",
@@ -142,21 +141,21 @@ ao_kalman_correct_baro(void)
 	ao_kalman_err_height();
 #ifdef AO_FLIGHT_TEST
 	if (ao_sample_tick - ao_sample_prev_tick > 50) {
-		ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h;
-		ao_k_speed  += (int32_t) AO_BARO_K1_1 * ao_error_h;
-		ao_k_accel  += (int32_t) AO_BARO_K2_1 * ao_error_h;
+		ao_k_height += (ao_k_t) AO_BARO_K0_1 * ao_error_h;
+		ao_k_speed  += (ao_k_t) AO_BARO_K1_1 * ao_error_h;
+		ao_k_accel  += (ao_k_t) AO_BARO_K2_1 * ao_error_h;
 		return;
 	}
 	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h;
-		ao_k_speed  += (int32_t) AO_BARO_K1_10 * ao_error_h;
-		ao_k_accel  += (int32_t) AO_BARO_K2_10 * ao_error_h;
+		ao_k_height += (ao_k_t) AO_BARO_K0_10 * ao_error_h;
+		ao_k_speed  += (ao_k_t) AO_BARO_K1_10 * ao_error_h;
+		ao_k_accel  += (ao_k_t) AO_BARO_K2_10 * ao_error_h;
 		return;
 	}
 #endif
-	ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h;
-	ao_k_speed  += (int32_t) AO_BARO_K1_100 * ao_error_h;
-	ao_k_accel  += (int32_t) AO_BARO_K2_100 * ao_error_h;
+	ao_k_height += (ao_k_t) AO_BARO_K0_100 * ao_error_h;
+	ao_k_speed  += (ao_k_t) AO_BARO_K1_100 * ao_error_h;
+	ao_k_accel  += (ao_k_t) AO_BARO_K2_100 * ao_error_h;
 }
 
 #if HAS_ACCEL
@@ -164,7 +163,7 @@ ao_kalman_correct_baro(void)
 static void
 ao_kalman_err_accel(void)
 {
-	int32_t	accel;
+	ao_k_t	accel;
 
 	accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
 
@@ -187,18 +186,18 @@ ao_kalman_correct_both(void)
 				(double) ao_error_h, AO_BOTH_K10_1 / 65536.0,
 				(double) ao_error_a, AO_BOTH_K11_1 / 65536.0,
 				(ao_k_speed +
-				 (int32_t) AO_BOTH_K10_1 * ao_error_h +
-				 (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
+				 (ao_k_t) AO_BOTH_K10_1 * ao_error_h +
+				 (ao_k_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
 		}
 		ao_k_height +=
-			(int32_t) AO_BOTH_K00_1 * ao_error_h +
-			(int32_t) AO_BOTH_K01_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K00_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K01_1 * ao_error_a;
 		ao_k_speed +=
-			(int32_t) AO_BOTH_K10_1 * ao_error_h +
-			(int32_t) AO_BOTH_K11_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K10_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K11_1 * ao_error_a;
 		ao_k_accel +=
-			(int32_t) AO_BOTH_K20_1 * ao_error_h +
-			(int32_t) AO_BOTH_K21_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K20_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K21_1 * ao_error_a;
 		return;
 	}
 	if (ao_sample_tick - ao_sample_prev_tick > 5) {
@@ -208,18 +207,18 @@ ao_kalman_correct_both(void)
 				(double) ao_error_h, AO_BOTH_K10_10 / 65536.0,
 				(double) ao_error_a, AO_BOTH_K11_10 / 65536.0,
 				(ao_k_speed +
-				 (int32_t) AO_BOTH_K10_10 * ao_error_h +
-				 (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
+				 (ao_k_t) AO_BOTH_K10_10 * ao_error_h +
+				 (ao_k_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
 		}
 		ao_k_height +=
-			(int32_t) AO_BOTH_K00_10 * ao_error_h +
-			(int32_t) AO_BOTH_K01_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K00_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K01_10 * ao_error_a;
 		ao_k_speed +=
-			(int32_t) AO_BOTH_K10_10 * ao_error_h +
-			(int32_t) AO_BOTH_K11_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K10_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K11_10 * ao_error_a;
 		ao_k_accel +=
-			(int32_t) AO_BOTH_K20_10 * ao_error_h +
-			(int32_t) AO_BOTH_K21_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K20_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K21_10 * ao_error_a;
 		return;
 	}
 	if (ao_flight_debug) {
@@ -228,19 +227,19 @@ ao_kalman_correct_both(void)
 			(double) ao_error_h, AO_BOTH_K10_100 / 65536.0,
 			(double) ao_error_a, AO_BOTH_K11_100 / 65536.0,
 			(ao_k_speed +
-			 (int32_t) AO_BOTH_K10_100 * ao_error_h +
-			 (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
+			 (ao_k_t) AO_BOTH_K10_100 * ao_error_h +
+			 (ao_k_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
 	}
 #endif
 	ao_k_height +=
-		(int32_t) AO_BOTH_K00_100 * ao_error_h +
-		(int32_t) AO_BOTH_K01_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K00_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K01_100 * ao_error_a;
 	ao_k_speed +=
-		(int32_t) AO_BOTH_K10_100 * ao_error_h +
-		(int32_t) AO_BOTH_K11_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K10_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K11_100 * ao_error_a;
 	ao_k_accel +=
-		(int32_t) AO_BOTH_K20_100 * ao_error_h +
-		(int32_t) AO_BOTH_K21_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K20_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K21_100 * ao_error_a;
 }
 
 #else
@@ -251,14 +250,14 @@ ao_kalman_correct_accel(void)
 	ao_kalman_err_accel();
 
 	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a;
-		ao_k_speed  += (int32_t) AO_ACCEL_K1_10 * ao_error_a;
-		ao_k_accel  += (int32_t) AO_ACCEL_K2_10 * ao_error_a;
+		ao_k_height +=(ao_k_t) AO_ACCEL_K0_10 * ao_error_a;
+		ao_k_speed  += (ao_k_t) AO_ACCEL_K1_10 * ao_error_a;
+		ao_k_accel  += (ao_k_t) AO_ACCEL_K2_10 * ao_error_a;
 		return;
 	}
-	ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a;
-	ao_k_speed  += (int32_t) AO_ACCEL_K1_100 * ao_error_a;
-	ao_k_accel  += (int32_t) AO_ACCEL_K2_100 * ao_error_a;
+	ao_k_height += (ao_k_t) AO_ACCEL_K0_100 * ao_error_a;
+	ao_k_speed  += (ao_k_t) AO_ACCEL_K1_100 * ao_error_a;
+	ao_k_accel  += (ao_k_t) AO_ACCEL_K2_100 * ao_error_a;
 }
 
 #endif /* else FORCE_ACCEL */
diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h
index 33cda3eb..da20e067 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 */
@@ -317,7 +324,7 @@ struct ao_log_metrum {
 			uint8_t		year;		/* 8 */
 			uint8_t		month;		/* 9 */
 			uint8_t		day;		/* 10 */
-			uint8_t		pad;		/* 11 */
+			uint8_t		pdop;		/* 11 */
 		} gps_time;	/* 12 */
 		/* AO_LOG_GPS_SAT (up to three packets) */
 		struct {
@@ -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_microkalman.c b/src/kernel/ao_microkalman.c
index 0684ea2b..75a29cc4 100644
--- a/src/kernel/ao_microkalman.c
+++ b/src/kernel/ao_microkalman.c
@@ -22,19 +22,19 @@
 
 #define FIX_BITS	16
 
-#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
-#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_v(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix_k(x) ((int32_t) ((x) * 65536.0 + 0.5))
 #define from_fix8(x)	((x) >> 8)
 #define from_fix(x)	((x) >> 16)
-#define fix8_to_fix16(x)	((x) << 8)
+#define fix8_to_fix_v(x)	((x) << 8)
 #define fix16_to_fix8(x)	((x) >> 8)
 
 #include <ao_kalman.h>
 
 /* Basic time step (96ms) */
-#define AO_MK_STEP	to_fix16(0.096)
+#define AO_MK_STEP	to_fix_v(0.096)
 /* step ** 2 / 2 */
-#define AO_MK_STEP_2_2	to_fix16(0.004608)
+#define AO_MK_STEP_2_2	to_fix_v(0.004608)
 
 uint32_t	ao_k_pa;		/* 24.8 fixed point */
 int32_t		ao_k_pa_speed;		/* 16.16 fixed point */
@@ -49,7 +49,7 @@ ao_microkalman_init(void)
 {
 	ao_pa = pa;
 	ao_k_pa = pa << 8;
-}	
+}
 
 void
 ao_microkalman_predict(void)
diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
index 34658951..29bf2bf6 100644
--- a/src/kernel/ao_sample.c
+++ b/src/kernel/ao_sample.c
@@ -245,7 +245,7 @@ ao_sample_preflight(void)
 	} else {
 #if HAS_ACCEL
 		ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
-		ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g;
+		ao_accel_scale = to_fix_32(GRAVITY * 2 * 16) / ao_accel_2g;
 #endif
 		ao_sample_preflight_set();
 		ao_preflight = FALSE;
diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h
index 16d4c507..2ec998bd 100644
--- a/src/kernel/ao_sample.h
+++ b/src/kernel/ao_sample.h
@@ -24,6 +24,24 @@
  * ao_sample.c
  */
 
+#ifndef AO_VALUE_32
+#define AO_VALUE_32	1
+#endif
+
+#if AO_VALUE_32
+/*
+ * For 32-bit computed values, use 64-bit intermediates.
+ */
+typedef int64_t			ao_k_t;
+typedef int32_t			ao_v_t;
+#else
+/*
+ * For 16-bit computed values, use 32-bit intermediates.
+ */
+typedef int32_t			ao_k_t;
+typedef int16_t			ao_v_t;
+#endif
+
 /*
  * Barometer calibration
  *
@@ -87,9 +105,9 @@
  * 2047m/s² (over 200g)
  */
 
-#define AO_M_TO_HEIGHT(m)	((int16_t) (m))
-#define AO_MS_TO_SPEED(ms)	((int16_t) ((ms) * 16))
-#define AO_MSS_TO_ACCEL(mss)	((int16_t) ((mss) * 16))
+#define AO_M_TO_HEIGHT(m)	((ao_v_t) (m))
+#define AO_MS_TO_SPEED(ms)	((ao_v_t) ((ms) * 16))
+#define AO_MSS_TO_ACCEL(mss)	((ao_v_t) ((mss) * 16))
 
 extern __pdata uint16_t	ao_sample_tick;		/* time of last data */
 extern __data uint8_t	ao_sample_adc;		/* Ring position of last processed sample */
@@ -134,21 +152,33 @@ uint8_t ao_sample(void);
  * ao_kalman.c
  */
 
-#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
-#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_16(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix_32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_64(x) ((int64_t) ((x) * 65536.0 + 0.5))
+
+#ifdef AO_VALUE_32
+#if AO_VALUE_32
+#define to_fix_v(x)	to_fix_32(x)
+#define to_fix_k(x)	to_fix_64(x)
+#else
+#define to_fix_v(x)	to_fix_16(x)
+#define to_fix_k(x)	to_fix_32(x)
+#endif
+
 #define from_fix(x)	((x) >> 16)
 
-extern __pdata int16_t			ao_height;	/* meters */
-extern __pdata int16_t			ao_speed;	/* m/s * 16 */
-extern __pdata int16_t			ao_accel;	/* m/s² * 16 */
-extern __xdata int16_t			ao_max_height;	/* max of ao_height */
-extern __xdata int16_t			ao_avg_height;	/* running average of height */
+extern __pdata ao_v_t			ao_height;	/* meters */
+extern __pdata ao_v_t			ao_speed;	/* m/s * 16 */
+extern __pdata ao_v_t			ao_accel;	/* m/s² * 16 */
+extern __xdata ao_v_t			ao_max_height;	/* max of ao_height */
+extern __xdata ao_v_t			ao_avg_height;	/* running average of height */
 
-extern __pdata int16_t			ao_error_h;
-extern __pdata int16_t			ao_error_h_sq_avg;
+extern __pdata ao_v_t			ao_error_h;
+extern __pdata ao_v_t			ao_error_h_sq_avg;
 
 #if HAS_ACCEL
-extern __pdata int16_t			ao_error_a;
+extern __pdata ao_v_t			ao_error_a;
+#endif
 #endif
 
 void ao_kalman(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);
diff --git a/src/product/Makefile.telemetrum b/src/product/Makefile.telemetrum
index dbbf57d8..e9b144c0 100644
--- a/src/product/Makefile.telemetrum
+++ b/src/product/Makefile.telemetrum
@@ -24,6 +24,7 @@ INC = \
 	altitude.h \
 	ao_kalman.h \
 	ao_product.h \
+	ao_telemetry.h \
 	$(TM_INC)
 
 CORE_SRC = \
diff --git a/src/telefire-v0.1/Makefile b/src/telefire-v0.1/Makefile
index 25267268..99d29826 100644
--- a/src/telefire-v0.1/Makefile
+++ b/src/telefire-v0.1/Makefile
@@ -25,7 +25,6 @@ INC = \
 CORE_SRC = \
 	ao_cmd.c \
 	ao_config.c \
-	ao_convert.c \
 	ao_mutex.c \
 	ao_panic.c \
 	ao_stdio.c \
diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile
index ad5065c1..944543c5 100644
--- a/src/telefire-v0.2/Makefile
+++ b/src/telefire-v0.2/Makefile
@@ -25,7 +25,6 @@ INC = \
 CORE_SRC = \
 	ao_cmd.c \
 	ao_config.c \
-	ao_convert.c \
 	ao_mutex.c \
 	ao_panic.c \
 	ao_stdio.c \
diff --git a/src/telefire-v0.2/ao_pins.h b/src/telefire-v0.2/ao_pins.h
index 9e6631ce..ef2d4822 100644
--- a/src/telefire-v0.2/ao_pins.h
+++ b/src/telefire-v0.2/ao_pins.h
@@ -19,6 +19,7 @@
 #define _AO_PINS_H_
 
 #define HAS_RADIO		1
+#define HAS_RADIO_RATE		1
 #define HAS_TELEMETRY		0
 
 #define HAS_FLIGHT		0
diff --git a/src/telelco-v0.2/ao_lco.c b/src/telelco-v0.2/ao_lco.c
index b3f5bb16..4b5f7a9b 100644
--- a/src/telelco-v0.2/ao_lco.c
+++ b/src/telelco-v0.2/ao_lco.c
@@ -258,7 +258,7 @@ ao_lco_search(void)
 	for (box = 0; box < AO_PAD_MAX_BOXES; box++) {
 		if ((box % 10) == 0)
 			ao_lco_set_box(box);
-		for (try = 0; try < 5; try++) {
+		for (try = 0; try < 3; try++) {
 			tick_offset = 0;
 			r = ao_lco_query(box, &ao_pad_query, &tick_offset);
 			PRINTD("box %d result %d\n", box, r);
diff --git a/src/telelco-v0.2/ao_pins.h b/src/telelco-v0.2/ao_pins.h
index a6fd4ff8..da790b14 100644
--- a/src/telelco-v0.2/ao_pins.h
+++ b/src/telelco-v0.2/ao_pins.h
@@ -48,6 +48,7 @@
 #define HAS_USB			1
 #define HAS_BEEP		1
 #define HAS_RADIO		1
+#define HAS_RADIO_RATE		1
 #define HAS_TELEMETRY		0
 #define HAS_AES			1
 
diff --git a/src/telemega-v1.0/Makefile b/src/telemega-v1.0/Makefile
index 46c768e4..4a1b3908 100644
--- a/src/telemega-v1.0/Makefile
+++ b/src/telemega-v1.0/Makefile
@@ -31,6 +31,7 @@ INC = \
 	ao_mpu.h \
 	stm32l.h \
 	math.h \
+	ao_ms5607_convert.c \
 	Makefile
 
 #
diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h
index 948310e5..ed911798 100644
--- a/src/telemini-v2.0/ao_pins.h
+++ b/src/telemini-v2.0/ao_pins.h
@@ -22,6 +22,7 @@
 
 #define HAS_FLIGHT		1
 #define HAS_USB			1
+#define AO_VALUE_32		0
 
 #define HAS_USB_PULLUP	1
 #define AO_USB_PULLUP_PORT	P1
diff --git a/src/teleterra-v0.2/ao_pins.h b/src/teleterra-v0.2/ao_pins.h
index 60d627ad..472af534 100644
--- a/src/teleterra-v0.2/ao_pins.h
+++ b/src/teleterra-v0.2/ao_pins.h
@@ -72,6 +72,8 @@
 
 	#define BATTERY_PIN		5
 	#define HAS_TELEMETRY		0
+
+	#define AO_VALUE_32		0
 #endif
 
 #if DBG_ON_P1
diff --git a/src/test/Makefile b/src/test/Makefile
index 017f7f71..02e1d22b 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -1,6 +1,7 @@
 vpath % ..:../kernel:../drivers:../util:../micropeak:../aes:../product
 
 PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \
+	ao_flight_test_metrum \
 	ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \
 	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \
 	ao_ms5607_convert_test ao_quaternion_test
@@ -33,6 +34,9 @@ ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c  ao_sample.c ao_kal
 ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c ao_pyro.c ao_pyro.h $(INCS)
 	cc -DTELEMEGA=1 $(CFLAGS) -o $@ $< -lm
 
+ao_flight_test_metrum: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c ao_pyro.c ao_pyro.h $(INCS)
+	cc -DTELEMETRUM_V2=1 $(CFLAGS) -o $@ $< -lm
+
 ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h
 	cc $(CFLAGS) -o $@ $<
 
diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c
index 573b5cb2..ae505dea 100644
--- a/src/test/ao_aprs_test.c
+++ b/src/test/ao_aprs_test.c
@@ -21,6 +21,8 @@
 #include <stdint.h>
 #include <stdarg.h>
 
+#define HAS_GPS	1
+
 #include <ao_telemetry.h>
 
 #define AO_GPS_NUM_SAT_MASK	(0xf << 0)
@@ -100,14 +102,11 @@ audio_gap(int secs)
 // This is where we go after reset.
 int main(int argc, char **argv)
 {
-	int	e, x;
-	int	a;
-
     audio_gap(1);
 
     ao_gps_data.latitude = (45.0 + 28.25 / 60.0) * 10000000;
     ao_gps_data.longitude = (-(122 + 44.2649 / 60.0)) * 10000000;
-    ao_gps_data.altitude = 84;
+    AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, 84);
     ao_gps_data.flags = (AO_GPS_VALID|AO_GPS_RUNNING);
 
     /* Transmit one packet */
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 1ab22e5b..8b737ca1 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -34,15 +34,21 @@
 #define ao_data_ring_next(n)	(((n) + 1) & (AO_DATA_RING - 1))
 #define ao_data_ring_prev(n)	(((n) - 1) & (AO_DATA_RING - 1))
 
+#if 0
 #define AO_M_TO_HEIGHT(m)	((int16_t) (m))
 #define AO_MS_TO_SPEED(ms)	((int16_t) ((ms) * 16))
 #define AO_MSS_TO_ACCEL(mss)	((int16_t) ((mss) * 16))
+#endif
 
 #define AO_GPS_NEW_DATA		1
 #define AO_GPS_NEW_TRACKING	2
 
 int ao_gps_new;
 
+#if !defined(TELEMEGA) && !defined(TELEMETRUM_V2)
+#define TELEMETRUM_V1 1
+#endif
+
 #if TELEMEGA
 #define AO_ADC_NUM_SENSE	6
 #define HAS_MS5607		1
@@ -58,7 +64,25 @@ struct ao_adc {
 	int16_t			v_pbatt;
 	int16_t			temp;
 };
-#else
+#endif
+
+#if TELEMETRUM_V2
+#define AO_ADC_NUM_SENSE	2
+#define HAS_MS5607		1
+#define HAS_MMA655X		1
+#define HAS_BEEP		1
+#define AO_CONFIG_MAX_SIZE	1024
+
+struct ao_adc {
+	int16_t			sense_a;
+	int16_t			sense_m;
+	int16_t			v_batt;
+	int16_t			temp;
+};
+#endif
+
+
+#if TELEMETRUM_V1
 /*
  * One set of samples read from the A/D converter
  */
@@ -93,6 +117,7 @@ struct ao_adc {
 #include <ao_data.h>
 #include <ao_log.h>
 #include <ao_telemetry.h>
+#include <ao_sample.h>
 
 #if TELEMEGA
 int ao_gps_count;
@@ -175,7 +200,7 @@ ao_gps_angle(void)
 			ao_gps_static.latitude / 1e7,
 			ao_gps_static.longitude / 1e7,
 			&dist, &bearing);
-	height = ao_gps_static.altitude - ao_gps_prev.altitude;
+	height = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_static) - AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_prev);
 
 	angle = atan2(dist, height);
 	return angle * 180/M_PI;
@@ -186,17 +211,6 @@ ao_gps_angle(void)
 #define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
 #define from_fix(x)	((x) >> 16)
 
-/*
- * Above this height, the baro sensor doesn't work
- */
-#define AO_BARO_SATURATE	13000
-#define AO_MIN_BARO_VALUE	ao_altitude_to_pres(AO_BARO_SATURATE)
-
-/*
- * Above this speed, baro measurements are unreliable
- */
-#define AO_MAX_BARO_SPEED	200
-
 #define ACCEL_NOSE_UP	(ao_accel_2g >> 2)
 
 extern enum ao_flight_state ao_flight_state;
@@ -234,7 +248,7 @@ double	main_time;
 
 int	tick_offset;
 
-static int32_t	ao_k_height;
+static ao_k_t	ao_k_height;
 
 int16_t
 ao_time(void)
@@ -306,7 +320,7 @@ struct ao_cmds {
 #define ao_xmemcmp(d,s,c) memcmp(d,s,c)
 
 #define AO_NEED_ALTITUDE_TO_PRES 1
-#if TELEMEGA
+#if TELEMEGA || TELEMETRUM_V2
 #include "ao_convert_pa.c"
 #include <ao_ms5607.h>
 struct ao_ms5607_prom	ao_ms5607_prom;
@@ -456,7 +470,7 @@ ao_insert(void)
 #else
 		double	accel = 0.0;
 #endif
-#if TELEMEGA
+#if TELEMEGA || TELEMETRUM_V2
 		double	height;
 
 		ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked);
@@ -551,6 +565,7 @@ ao_insert(void)
 				mag_azel.el,
 				mag_azel.az);
 #endif
+#if 0
 			printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n",
 				time,
 				ao_state_names[ao_flight_state],
@@ -560,6 +575,7 @@ ao_insert(void)
 				(int) floor (ao_gps_angle() + 0.5),
 				(ao_gps_static.flags & 0xf) * 10);
 
+#endif
 #if 0
 			printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n",
 				ao_state_names[ao_flight_state],
@@ -582,9 +598,9 @@ ao_insert(void)
 #endif
 #endif
 
-#if 0
+#if 1
 			printf("%7.2f height %8.2f accel %8.3f "
-#if TELEMEGA
+#if TELEMEGA && 0
 			       "angle %5d "
 			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d "
 #endif
@@ -592,7 +608,7 @@ ao_insert(void)
 			       time,
 			       height,
 			       accel,
-#if TELEMEGA
+#if TELEMEGA && 0
 			       ao_sample_orient,
 
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
@@ -675,7 +691,8 @@ ao_sleep(void *wchan)
 			{
 #if TELEMEGA
 				ao_data_static.mpu6000 = ao_ground_mpu6000;
-#else
+#endif
+#if TELEMETRUM_V1
 				ao_data_static.adc.accel = ao_flight_ground_accel;
 #endif
 				ao_insert();
@@ -756,7 +773,10 @@ ao_sleep(void *wchan)
 					ao_gps_static.tick = tick;
 					ao_gps_static.latitude = int32(bytes, 0);
 					ao_gps_static.longitude = int32(bytes, 4);
-					ao_gps_static.altitude = int32(bytes, 8);
+					{
+						int32_t	altitude = int32(bytes, 8);
+						AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_static, altitude);
+					}
 					ao_gps_static.flags = bytes[13];
 					if (!ao_gps_count)
 						ao_gps_first = ao_gps_static;
@@ -801,7 +821,59 @@ ao_sleep(void *wchan)
 					}
 				}
 			}
-#else
+#endif
+#if TELEMETRUM_V2
+			if (log_format == AO_LOG_FORMAT_TELEMETRUM && nword == 14 && strlen(words[0]) == 1) {
+				int	i;
+				struct ao_ms5607_value	value;
+
+				type = words[0][0];
+				tick = strtoul(words[1], NULL, 16);
+//				printf ("%c %04x", type, tick);
+				for (i = 2; i < nword; i++) {
+					bytes[i - 2] = strtoul(words[i], NULL, 16);
+//					printf(" %02x", bytes[i-2]);
+				}
+//				printf ("\n");
+				switch (type) {
+				case 'F':
+					ao_flight_ground_accel = int16(bytes, 2);
+					ao_flight_started = 1;
+					ao_ground_pres = int32(bytes, 4);
+					ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
+					break;
+				case 'A':
+					ao_data_static.tick = tick;
+					ao_data_static.ms5607_raw.pres = int32(bytes, 0);
+					ao_data_static.ms5607_raw.temp = int32(bytes, 4);
+					ao_ms5607_convert(&ao_data_static.ms5607_raw, &value);
+					ao_data_static.mma655x = int16(bytes, 8);
+					ao_records_read++;
+					ao_insert();
+					return;
+				}
+				continue;
+			} else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
+				if (strcmp(words[1], "reserved:") == 0)
+					ao_ms5607_prom.reserved = strtoul(words[2], NULL, 10);
+				else if (strcmp(words[1], "sens:") == 0)
+					ao_ms5607_prom.sens = strtoul(words[2], NULL, 10);
+				else if (strcmp(words[1], "off:") == 0)
+					ao_ms5607_prom.off = strtoul(words[2], NULL, 10);
+				else if (strcmp(words[1], "tcs:") == 0)
+					ao_ms5607_prom.tcs = strtoul(words[2], NULL, 10);
+				else if (strcmp(words[1], "tco:") == 0)
+					ao_ms5607_prom.tco = strtoul(words[2], NULL, 10);
+				else if (strcmp(words[1], "tref:") == 0)
+					ao_ms5607_prom.tref = strtoul(words[2], NULL, 10);
+				else if (strcmp(words[1], "tempsens:") == 0)
+					ao_ms5607_prom.tempsens = strtoul(words[2], NULL, 10);
+				else if (strcmp(words[1], "crc:") == 0)
+					ao_ms5607_prom.crc = strtoul(words[2], NULL, 10);
+				continue;
+			}
+#endif
+#if TELEMETRUM_V1
 			if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) {
 				type = words[0][0];
 				tick = strtoul(words[1], NULL, 16);
@@ -926,7 +998,7 @@ ao_sleep(void *wchan)
 			if (type != 'F' && !ao_flight_started)
 				continue;
 
-#if TELEMEGA
+#if TELEMEGA || TELEMETRUM_V2
 			(void) a;
 			(void) b;
 #else
@@ -947,8 +1019,6 @@ ao_sleep(void *wchan)
 				ao_data_static.tick = tick;
 				ao_data_static.adc.accel = a;
 				ao_data_static.adc.pres_real = b;
-				if (b < AO_MIN_BARO_VALUE)
-					b = AO_MIN_BARO_VALUE;
 				ao_data_static.adc.pres = b;
 				ao_records_read++;
 				ao_insert();
diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c
index e799ab0f..543bbcc3 100644
--- a/src/test/ao_gps_test.c
+++ b/src/test/ao_gps_test.c
@@ -53,6 +53,9 @@ struct ao_gps_orig {
 	uint16_t		v_error;	/* m */
 };
 
+#define AO_TELEMETRY_LOCATION_ALTITUDE(l) 	((l)->altitude)
+#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) ((l)->altitude = (a))
+
 #define SIRF_SAT_STATE_ACQUIRED			(1 << 0)
 #define SIRF_SAT_STATE_CARRIER_PHASE_VALID	(1 << 1)
 #define SIRF_SAT_BIT_SYNC_COMPLETE		(1 << 2)
@@ -433,7 +436,7 @@ ao_dump_state(void *wchan)
 
 	if (wchan != &ao_gps_new)
 		return;
-	
+
 	if (ao_gps_new & AO_GPS_NEW_DATA) {
 		ao_gps_print(&ao_gps_data);
 		putchar('\n');
diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c
index 1b590d5e..5eb7118d 100644
--- a/src/test/ao_gps_test_skytraq.c
+++ b/src/test/ao_gps_test_skytraq.c
@@ -16,6 +16,7 @@
  */
 
 #define AO_GPS_TEST
+#define HAS_GPS 1
 #include "ao_host.h"
 #include <termios.h>
 #include <errno.h>
@@ -53,6 +54,9 @@ struct ao_gps_orig {
 	uint16_t		v_error;	/* m */
 };
 
+#define AO_TELEMETRY_LOCATION_ALTITUDE(l) 	((l)->altitude)
+#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) ((l)->altitude = (a))
+
 #define SIRF_SAT_STATE_ACQUIRED			(1 << 0)
 #define SIRF_SAT_STATE_CARRIER_PHASE_VALID	(1 << 1)
 #define SIRF_SAT_BIT_SYNC_COMPLETE		(1 << 2)
diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c
index 4eb4b837..5ea205d6 100644
--- a/src/test/ao_gps_test_ublox.c
+++ b/src/test/ao_gps_test_ublox.c
@@ -16,6 +16,7 @@
  */
 
 #define AO_GPS_TEST
+#define HAS_GPS	1
 #include "ao_host.h"
 #include <termios.h>
 #include <errno.h>
@@ -44,7 +45,7 @@ struct ao_telemetry_location {
 	uint8_t			flags;
 	int32_t			latitude;	/* degrees * 10⁷ */
 	int32_t			longitude;	/* degrees * 10⁷ */
-	int16_t			altitude;	/* m */
+	int16_t			altitude_low;	/* m */
 	uint16_t		ground_speed;	/* cm/s */
 	uint8_t			course;		/* degrees / 2 */
 	uint8_t			pdop;		/* * 5 */
@@ -53,8 +54,14 @@ struct ao_telemetry_location {
 	int16_t			climb_rate;	/* cm/s */
 	uint16_t		h_error;	/* m */
 	uint16_t		v_error;	/* m */
+	int16_t			altitude_high;	/* m */
 };
 
+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)->altitude_high = (a) >> 16), \
+						 ((l)->altitude_low = (a)))
+
 #define UBLOX_SAT_STATE_ACQUIRED		(1 << 0)
 #define UBLOX_SAT_STATE_CARRIER_PHASE_VALID	(1 << 1)
 #define UBLOX_SAT_BIT_SYNC_COMPLETE		(1 << 2)
diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c
index 5961bd93..f4af707e 100644
--- a/src/test/ao_micropeak_test.c
+++ b/src/test/ao_micropeak_test.c
@@ -33,6 +33,7 @@ uint8_t ao_flight_debug;
 #define AO_FLIGHT_TEST
 
 typedef int32_t alt_t;
+typedef int32_t pres_t;
 
 #define AO_MS_TO_TICKS(ms)	((ms) / 10)
 
diff --git a/src/test/plottest b/src/test/plottest
index 76af5ee7..7d253ff1 100755
--- a/src/test/plottest
+++ b/src/test/plottest
@@ -10,7 +10,7 @@ plot "$1" using 1:3 with lines axes x1y1 title "raw height",\
 "$1" using 1:9 with lines axes x1y1 title "height",\
 "$1" using 1:11 with lines axes x1y2 title "speed",\
 "$1" using 1:13 with lines axes x1y2 title "accel",\
-"$1" using 1:15 with lines axes x1y1 title "drogue",\
-"$1" using 1:17 with lines axes x1y1 title "main",\
-"$1" using 1:19 with lines axes x1y1 title "error"
+"$1" using 1:17 with lines axes x1y1 title "drogue",\
+"$1" using 1:19 with lines axes x1y1 title "main",\
+"$1" using 1:21 with lines axes x1y1 title "error"
 EOF