summary |
shortlog |
log |
commit | commitdiff |
tree
raw |
patch |
inline | side by side (from parent 1:
31ae24b)
Stores 32-bits for all of the flight parameters. Uses 64-bit
intermediates for kalman computation.
Signed-off-by: Keith Packard <keithp@keithp.com>
23 files changed:
altitude-pa-small.h: make-altitude-pa
nickle $< --sample 3 > $@
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
bash $< kalman > $@
ao_whiten.h: make-whiten
#define HAS_RADIO 1
#define DISABLE_LOG_SPACE 1
#define HAS_RADIO 1
#define DISABLE_LOG_SPACE 1
#define HAS_WIDE_GPS 0
#if defined(TELEMETRUM_V_1_0)
#define HAS_WIDE_GPS 0
#if defined(TELEMETRUM_V_1_0)
name = sprintf("%s_K%d_%d", prefix, i, time_inc);
else
name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
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);
}
printf ("\n");
exit(0);
name = sprintf("%s_K%d_%d", prefix, i, time_inc);
else
name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
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);
}
printf ("\n");
exit(0);
* Given raw data, convert to SI units
*/
* Given raw data, convert to SI units
*/
/* pressure from the sensor to altitude in meters */
/* 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;
int16_t
ao_temp_to_dC(int16_t temp) __reentrant;
-ao_pa_to_altitude(int32_t pa);
+ao_pa_to_altitude(pres_t pa);
int32_t
ao_altitude_to_pa(alt_t alt);
int32_t
ao_altitude_to_pa(alt_t alt);
#if HAS_DBG
#include <ao_dbg.h>
#if HAS_DBG
#include <ao_dbg.h>
#endif
#if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND)
"program-space %u\n"
#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
#endif
, ao_manufacturer
, ao_product
-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)
#include "altitude.h"
};
#define ALT_FRAC_SCALE (1 << ALT_FRAC_BITS)
#define ALT_FRAC_MASK (ALT_FRAC_SCALE - 1)
ao_pres_to_altitude(int16_t pres) __reentrant
{
uint8_t o;
ao_pres_to_altitude(int16_t pres) __reentrant
{
uint8_t o;
#if AO_NEED_ALTITUDE_TO_PRES
int16_t
#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;
uint8_t l, h, m;
int32_t pres;
uint8_t l, h, m;
int32_t pres;
#define ALT_MASK (ALT_SCALE - 1)
alt_t
#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;
{
int16_t o;
int16_t part;
o = pa >> ALT_SHIFT;
part = pa & ALT_MASK;
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
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;
uint16_t l, h, m;
int32_t pa;
uint16_t l, h, m;
int32_t pa;
}
span = altitude_table[l] - altitude_table[h];
sub_span = altitude_table[l] - 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)
if (pa > 120000)
pa = 120000;
if (pa < 0)
#include <stdint.h>
#define AO_CONVERT_TEST
typedef int32_t alt_t;
#include <stdint.h>
#define AO_CONVERT_TEST
typedef int32_t alt_t;
#include "ao_host.h"
#include "ao_convert_pa.c"
#include "ao_host.h"
#include "ao_convert_pa.c"
#define AO_ALT_TYPE int32_t
#define AO_ALT_TYPE int32_t
typedef AO_ALT_TYPE alt_t;
typedef AO_ALT_TYPE alt_t;
-#if !HAS_BARO
-typedef int16_t alt_t;
-#endif
-
/*
* Need a few macros to pull data from the sensors:
*
/*
* Need a few macros to pull data from the sensors:
*
* resting
*/
static __data uint16_t ao_interval_end;
* 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;
-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;
#endif
__pdata uint8_t ao_flight_force_idle;
-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
static void
ao_flight_dump(void)
{
#if HAS_ACCEL
accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
#endif
accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
#endif
#include "ao_sample.h"
#include "ao_kalman.h"
#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;
-__pdata int16_t ao_error_a;
+__pdata ao_v_t ao_error_a;
{
#ifdef AO_FLIGHT_TEST
if (ao_sample_tick - ao_sample_prev_tick > 50) {
{
#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) {
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,
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));
- 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)
{
}
static void
ao_kalman_err_height(void)
{
- int16_t e;
- int16_t height_distrust;
+ ao_v_t e;
+ ao_v_t height_distrust;
- int16_t speed_distrust;
- 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)
e = ao_error_h;
if (e < 0)
#endif
if (height_distrust > 0x100)
height_distrust = 0x100;
#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",
#ifdef AO_FLIGHT_TEST
if (ao_flight_debug) {
printf("over height %g over speed %g distrust: %g height: error %d -> %d\n",
ao_kalman_err_height();
#ifdef AO_FLIGHT_TEST
if (ao_sample_tick - ao_sample_prev_tick > 50) {
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) {
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;
- 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;
static void
ao_kalman_err_accel(void)
{
static void
ao_kalman_err_accel(void)
{
accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
(double) ao_error_h, AO_BOTH_K10_1 / 65536.0,
(double) ao_error_a, AO_BOTH_K11_1 / 65536.0,
(ao_k_speed +
(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));
- (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;
- (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;
- (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) {
return;
}
if (ao_sample_tick - ao_sample_prev_tick > 5) {
(double) ao_error_h, AO_BOTH_K10_10 / 65536.0,
(double) ao_error_a, AO_BOTH_K11_10 / 65536.0,
(ao_k_speed +
(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));
- (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;
- (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;
- (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) {
return;
}
if (ao_flight_debug) {
(double) ao_error_h, AO_BOTH_K10_100 / 65536.0,
(double) ao_error_a, AO_BOTH_K11_100 / 65536.0,
(ao_k_speed +
(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));
- (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;
- (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;
- (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;
ao_kalman_err_accel();
if (ao_sample_tick - ao_sample_prev_tick > 5) {
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;
- 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 */
}
#endif /* else FORCE_ACCEL */
-#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 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 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)
-#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 */
uint32_t ao_k_pa; /* 24.8 fixed point */
int32_t ao_k_pa_speed; /* 16.16 fixed point */
{
ao_pa = pa;
ao_k_pa = pa << 8;
{
ao_pa = pa;
ao_k_pa = pa << 8;
void
ao_microkalman_predict(void)
void
ao_microkalman_predict(void)
} else {
#if HAS_ACCEL
ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
} 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;
#endif
ao_sample_preflight_set();
ao_preflight = FALSE;
+#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
*
/*
* Barometer calibration
*
* 2047m/s² (over 200g)
*/
* 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 */
extern __pdata uint16_t ao_sample_tick; /* time of last data */
extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */
-#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)
#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;
-extern __pdata int16_t ao_error_a;
+extern __pdata ao_v_t ao_error_a;
+#endif
#endif
void ao_kalman(void);
#endif
void ao_kalman(void);
altitude.h \
ao_kalman.h \
ao_product.h \
altitude.h \
ao_kalman.h \
ao_product.h \
CORE_SRC = \
ao_cmd.c \
ao_config.c \
CORE_SRC = \
ao_cmd.c \
ao_config.c \
ao_mutex.c \
ao_panic.c \
ao_stdio.c \
ao_mutex.c \
ao_panic.c \
ao_stdio.c \
CORE_SRC = \
ao_cmd.c \
ao_config.c \
CORE_SRC = \
ao_cmd.c \
ao_config.c \
ao_mutex.c \
ao_panic.c \
ao_stdio.c \
ao_mutex.c \
ao_panic.c \
ao_stdio.c \
ao_mpu.h \
stm32l.h \
math.h \
ao_mpu.h \
stm32l.h \
math.h \
#define HAS_FLIGHT 1
#define HAS_USB 1
#define HAS_FLIGHT 1
#define HAS_USB 1
#define HAS_USB_PULLUP 1
#define AO_USB_PULLUP_PORT P1
#define HAS_USB_PULLUP 1
#define AO_USB_PULLUP_PORT P1
#define BATTERY_PIN 5
#define HAS_TELEMETRY 0
#define BATTERY_PIN 5
#define HAS_TELEMETRY 0
+
+ #define AO_VALUE_32 0
#define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1))
#define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1))
#define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1))
#define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1))
#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) ((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_GPS_NEW_DATA 1
#define AO_GPS_NEW_TRACKING 2
#define AO_GPS_NEW_DATA 1
#define AO_GPS_NEW_TRACKING 2
#include <ao_data.h>
#include <ao_log.h>
#include <ao_telemetry.h>
#include <ao_data.h>
#include <ao_log.h>
#include <ao_telemetry.h>
#if TELEMEGA
int ao_gps_count;
#if TELEMEGA
int ao_gps_count;
-static int32_t ao_k_height;
+static ao_k_t ao_k_height;
#define AO_FLIGHT_TEST
typedef int32_t alt_t;
#define AO_FLIGHT_TEST
typedef int32_t alt_t;
#define AO_MS_TO_TICKS(ms) ((ms) / 10)
#define AO_MS_TO_TICKS(ms) ((ms) / 10)