+
+#if HAS_ACCEL
+static __pdata int16_t ao_error_a;
+static __pdata int32_t ao_accel_scale;
+
+static void
+ao_kalman_err_accel(void)
+{
+ int32_t accel;
+
+ accel = (ao_ground_accel - ao_raw_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;
+}
+
+static void
+ao_kalman_correct_both(void)
+{
+ ao_kalman_err_height();
+ ao_kalman_err_accel();
+
+#if 0
+ /*
+ * Check to see if things are crazy here --
+ * if the computed height is far above the
+ * measured height, we assume that the flight
+ * trajectory is not vertical, and so ignore
+ * the accelerometer for the remainder of the
+ * flight.
+ */
+ if (ao_error_h_sq_avg > 10)
+ {
+ ao_kalman_correct_baro();
+ return;
+ }
+#endif
+
+#ifdef AO_FLIGHT_TEST
+ if (ao_flight_tick - ao_flight_prev_tick > 5) {
+ ao_k_height +=
+ (int32_t) AO_BOTH_K00_10 * ao_error_h +
+ (int32_t) (AO_BOTH_K01_10 >> 4) * ao_error_a;
+ ao_k_speed +=
+ ((int32_t) AO_BOTH_K10_10 << 4) * ao_error_h +
+ (int32_t) AO_BOTH_K11_10 * ao_error_a;
+ ao_k_accel +=
+ ((int32_t) AO_BOTH_K20_10 << 4) * ao_error_h +
+ (int32_t) AO_BOTH_K21_10 * ao_error_a;
+ return;
+ }
+#endif
+ ao_k_height +=
+ (int32_t) AO_BOTH_K00_100 * ao_error_h +
+ (int32_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_accel +=
+ (int32_t) AO_BOTH_K20_100 * ao_error_h +
+ (int32_t) AO_BOTH_K21_100 * ao_error_a;
+}
+
+static void
+ao_kalman_correct_accel(void)
+{
+ ao_kalman_err_accel();
+
+#ifdef AO_FLIGHT_TEST
+ if (ao_flight_tick - ao_flight_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;
+ return;
+ }