From: Keith Packard <keithp@keithp.com>
Date: Fri, 11 Jul 2014 00:18:38 +0000 (-0700)
Subject: altos: Use 32-bits for flight state data (alt/speed/accel)
X-Git-Tag: 1.4.9.2~15
X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=013e9ccfbe76dc46e8c69ea314950bed83d9a39f

altos: Use 32-bits for flight state data (alt/speed/accel)

Stores 32-bits for all of the flight parameters. Uses 64-bit
intermediates for kalman computation.

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

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 4db49215..e12f813f 100644
--- a/src/cc1111/ao_pins.h
+++ b/src/cc1111/ao_pins.h
@@ -20,6 +20,7 @@
 
 #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)
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
@@ -289,6 +289,9 @@ version(void)
 #endif
 #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
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_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_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/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/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/ao_flight_test.c b/src/test/ao_flight_test.c
index 314998c1..bb5c3a7d 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -34,9 +34,11 @@
 #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
@@ -93,6 +95,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;
@@ -234,7 +237,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)
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)