ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100;
}
+#if HAS_BARO
static void
ao_kalman_err_height(void)
{
#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;
#endif /* else FORCE_ACCEL */
#endif /* HAS_ACCEL */
+#if !HAS_BARO
+static ao_k_t ao_k_height_prev;
+static ao_k_t ao_k_speed_prev;
+
+/*
+ * While in pad mode without a barometric sensor, remove accumulated
+ * speed and height values to reduce the effect of systematic sensor
+ * error
+ */
+void
+ao_kalman_reset_accumulate(void)
+{
+ ao_k_height -= ao_k_height_prev;
+ ao_k_speed -= ao_k_speed_prev;
+ ao_k_height_prev = ao_k_height;
+ ao_k_speed_prev = ao_k_speed;
+}
+#endif
+
void
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;