+__xdata int32_t ao_raw_accel_sum;
+#endif
+
+#if USE_KALMAN
+__pdata int16_t ao_ground_height;
+__pdata int32_t ao_k_max_height;
+__pdata int32_t ao_k_height;
+__pdata int32_t ao_k_speed;
+__pdata int32_t ao_k_accel;
+
+#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+
+#define from_fix(x) ((x) >> 16)
+
+#define AO_K0_100 to_fix16(0.05680323)
+#define AO_K1_100 to_fix16(0.16608182)
+#define AO_K2_100 to_fix16(0.24279580)
+
+#define AO_K_STEP_100 to_fix16(0.01)
+#define AO_K_STEP_2_2_100 to_fix16(0.00005)
+
+#define AO_K0_10 to_fix16(0.23772023)
+#define AO_K1_10 to_fix16(0.32214149)
+#define AO_K2_10 to_fix16(0.21827159)
+
+#define AO_K_STEP_10 to_fix16(0.1)
+#define AO_K_STEP_2_2_10 to_fix16(0.005)
+
+static void
+ao_kalman_baro(void)
+{
+ int16_t err = ((ao_pres_to_altitude(ao_raw_pres) - ao_ground_height))
+ - (int16_t) (ao_k_height >> 16);
+
+#ifdef AO_FLIGHT_TEST
+ if (ao_flight_tick - ao_flight_prev_tick > 5) {
+ ao_k_height += ((ao_k_speed >> 16) * AO_K_STEP_10 +
+ (ao_k_accel >> 16) * AO_K_STEP_2_2_10);
+ ao_k_speed += (ao_k_accel >> 16) * AO_K_STEP_10;
+
+ /* correct */
+ ao_k_height += (int32_t) AO_K0_10 * err;
+ ao_k_speed += (int32_t) AO_K1_10 * err;
+ ao_k_accel += (int32_t) AO_K2_10 * err;
+ return;
+ }
+#endif
+ ao_k_height += ((ao_k_speed >> 16) * AO_K_STEP_100 +
+ (ao_k_accel >> 16) * AO_K_STEP_2_2_100);
+ ao_k_speed += (ao_k_accel >> 16) * AO_K_STEP_100;
+
+ /* correct */
+ ao_k_height += (int32_t) AO_K0_100 * err;
+ ao_k_speed += (int32_t) AO_K1_100 * err;
+ ao_k_accel += (int32_t) AO_K2_100 * err;
+}
+#endif
+
+__xdata int32_t ao_raw_pres_sum;