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 a7a26b2664755c6ee3ed5f8b4485b4808958c6bc..3494ba62703a2da842a7a5255cf1ee0471509f05 100644 (file)
--- 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 4db49215946ebb872f7b392b2f57273ad43c65a4..e12f813f8b0b7f8a48fef078252f093de42bb615 100644 (file)
--- 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 46fa8e1fdefecbcd9dad65a9e1208c4bb2df306a..55fde04cc658d55c87816cae1217dc0cae2e6e7a 100755 (executable)
--- 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 266a118264a385ca073fb02f74dc5e8db540791a..1b0803845864cb6883ad3b37fba96d3381002671 100644 (file)
--- 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 a225bc4aca9d81eac96e3f77bc04bc92f5323039..ad5bbf8e76ae152fefaed06eed6758963b8ec0da 100644 (file)
--- 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 0052bdce916be3fc93245f7812db33ed004c4fcf..d2f583ef7d8c795bf6cafe18bd6db3faebf9a164 100644 (file)
--- 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 aa9b5f484b438e0871aa395a1cd37d7979237a11..db1f2301d2c9ecaffc18b0f67f661b08b74d683a 100644 (file)
--- 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 20162c1f5c1fea5ae5ec52b097e269344c2f4d3b..410815b68bcdabac38d417a75d70910b0de91578 100644 (file)
--- 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 7d5b1922a69a56abc29982a653c6d2a827abded9..954228621e2ed10f45493b4bfea8d93005c8833a 100644 (file)
--- 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 c4b062fdb1d0f9f4631354ada9b2baa30df9910a..8f75ad876b612e3cc1480e1fc9aa9fc8024908bd 100644 (file)
--- 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 2b433ee9b9bd998ba7325be82e62e21643299255..251dbc02a7f35eaa7994c1541af5be3800205f84 100644 (file)
--- 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 9aea1f14762f6139d9fb8dce932b98a6c946068d..7b0f8145d807ce74bd5db66c0d23408251a8d745 100644 (file)
--- 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 0684ea2bbaad4b03e747ab6421b83dce885a78b3..75a29cc41d9d7a43e1a817f3d7289717e479a742 100644 (file)
--- 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 34658951724e7166dbc3008a9079a2f7a1df8e59..29bf2bf6ed0661a98aab90182b903977c1148880 100644 (file)
--- 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 16d4c5076774d04ad6adefc70064f1dae43d6626..2ec998bd257adb4a9274447d4111504526c3d829 100644 (file)
--- 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 dbbf57d80437e25b6f259ce7ea1aa996935f861a..e9b144c02dca1dd23e16b68b4278523d6ef9c67a 100644 (file)
--- 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 25267268e3a5dd0c67ec409983c46338884af147..99d29826fe972bb16c2391a5f2e14ab280b871f8 100644 (file)
--- 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 ad5065c175d5f6a738ad0b7bfabe5f5da9bf5be0..944543c5d77a16e3919e6cc71d20a053563c4569 100644 (file)
--- 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 46c768e47c0c0901ba6f96671c35cb5cd19a3f85..4a1b3908ab3cbcf2dea2b8a7942b3c10e93e99d1 100644 (file)
--- 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 948310e553207f196c76d1121217e3a06e6ac0cf..ed911798930537292a1177ff48db3478a60f18b5 100644 (file)
--- 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 60d627ad21e628947d69f82aa7f5549cecbe1d68..472af53430cfa21dac9e846a0429444562813702 100644 (file)
--- 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 314998c1bc750667bfe4a50abba06cb2f689b75b..bb5c3a7d86d7618938e05d8795b5f7ef684eca79 100644 (file)
--- 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 5961bd9309ff700782dd639505d5d78acc32b7a6..f4af707e2bd54b68d873ad168915184001173728 100644 (file)
--- 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)