#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 ao_k_t ao_k_height;
+static ao_k_t ao_k_speed;
+static ao_k_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_1 to_fix_v(1)
#define AO_K_STEP_2_2_1 to_fix_v(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;
+ao_v_t ao_height;
+ao_v_t ao_speed;
+ao_v_t ao_accel;
+ao_v_t ao_max_height;
+static ao_k_t ao_avg_height_scaled;
+ao_v_t ao_avg_height;
-__pdata ao_v_t ao_error_h;
-#if !HAS_ACCEL
-__pdata ao_v_t ao_error_h_sq_avg;
+ao_v_t ao_error_h;
+#if !HAS_ACCEL || AO_FLIGHT_TEST
+#define AO_ERROR_H_SQ_AVG 1
+#endif
+
+#if AO_ERROR_H_SQ_AVG
+ao_v_t ao_error_h_sq_avg;
#endif
#if HAS_ACCEL
-__pdata ao_v_t ao_error_a;
+ao_v_t ao_error_a;
#endif
static void
ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100;
}
+#if HAS_BARO
static void
ao_kalman_err_height(void)
{
-#if !HAS_ACCEL
+#if AO_ERROR_H_SQ_AVG
ao_v_t e;
#endif
ao_v_t height_distrust;
ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16);
-#if !HAS_ACCEL
+#if AO_ERROR_H_SQ_AVG
e = ao_error_h;
if (e < 0)
e = -e;
#endif
}
}
+#endif
+#if HAS_BARO
static void
ao_kalman_correct_baro(void)
{
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;
}
+#endif
#if HAS_ACCEL
ao_error_a = (accel - ao_k_accel) >> 16;
}
-#ifndef FORCE_ACCEL
+#if !defined(FORCE_ACCEL) && HAS_BARO
static void
ao_kalman_correct_both(void)
{
{
ao_kalman_err_accel();
+#ifdef AO_FLIGHT_TEST
if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) {
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;
}
+#endif
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;
ao_kalman(void)
{
ao_kalman_predict();
+#if HAS_BARO
#if HAS_ACCEL
if (ao_flight_state <= ao_flight_coast) {
#ifdef FORCE_ACCEL
} else
#endif
ao_kalman_correct_baro();
+#else
+#if HAS_ACCEL
+ ao_kalman_correct_accel();
+#endif
+#endif
ao_height = from_fix(ao_k_height);
ao_speed = from_fix(ao_k_speed);
ao_accel = from_fix(ao_k_accel);
if (ao_height > ao_max_height)
ao_max_height = ao_height;
+#if HAS_BARO
ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height;
+#else
+ ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height;
+#endif
#ifdef AO_FLIGHT_TEST
if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50)
ao_avg_height = (ao_avg_height_scaled + 1) >> 1;