#ifndef AO_FLIGHT_TEST
#include "ao.h"
+#include "ao_flight.h"
#endif
+#include "ao_sample.h"
#include "ao_kalman.h"
+
static __pdata int32_t ao_k_height;
static __pdata int32_t ao_k_speed;
static __pdata int32_t ao_k_accel;
__pdata int16_t ao_height;
__pdata int16_t ao_speed;
__pdata int16_t ao_accel;
-__pdata int16_t ao_max_height;
+__xdata int16_t ao_max_height;
static __pdata int32_t ao_avg_height_scaled;
-__pdata int16_t ao_avg_height;
+__xdata int16_t ao_avg_height;
__pdata int16_t ao_error_h;
__pdata int16_t ao_error_h_sq_avg;
{
int32_t accel;
- accel = (ao_ground_accel - ao_sample_accel) * ao_accel_scale;
+ accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
/* Can't use ao_accel here as it is the pre-prediction value still */
ao_error_a = (accel - ao_k_accel) >> 16;
}
+#ifndef FORCE_ACCEL
static void
ao_kalman_correct_both(void)
{
(int32_t) AO_BOTH_K21_100 * ao_error_a;
}
-#ifdef FORCE_ACCEL
+#else
+
static void
ao_kalman_correct_accel(void)
{
ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a;
ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a;
}
-#endif
+
+#endif /* else FORCE_ACCEL */
#endif /* HAS_ACCEL */
void
else
#endif
ao_avg_height = (ao_avg_height_scaled + 63) >> 7;
-#ifdef AO_FLIGHT_TEST
- ao_sample_prev_tick = ao_sample_tick;
-#endif
}