Save speed/height values from 64 samples ago and subtract them from
the current value. This reduces the effect of systematic accelerometer
error causing these values to slowly creep when there's no barometric
sensor to keep them in check.
Signed-off-by: Keith Packard <keithp@keithp.com>
#define abs(a) ((a) < 0 ? -(a) : (a))
#define abs(a) ((a) < 0 ? -(a) : (a))
+#if !HAS_BARO
+// #define DEBUG_ACCEL_ONLY 1
+#endif
+
#if HAS_ACCEL
if (ao_config.accel_plus_g == 0 ||
ao_config.accel_minus_g == 0 ||
#if HAS_ACCEL
if (ao_config.accel_plus_g == 0 ||
ao_config.accel_minus_g == 0 ||
- ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP ||
- ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP
+ ao_ground_accel < (accel_t) ao_config.accel_plus_g - ACCEL_NOSE_UP ||
+ ao_ground_accel > (accel_t) ao_config.accel_minus_g + ACCEL_NOSE_UP
#if HAS_BARO
|| ao_ground_height < -1000 ||
ao_ground_height > 7000
#if HAS_BARO
|| ao_ground_height < -1000 ||
ao_ground_height > 7000
ao_wakeup(&ao_flight_state);
break;
ao_wakeup(&ao_flight_state);
break;
+
+#if DEBUG_ACCEL_ONLY
+ case ao_flight_invalid:
+ case ao_flight_idle:
+ printf("+g %d ga %d sa %d accel %ld speed %ld\n",
+ ao_config.accel_plus_g, ao_ground_accel, ao_sample_accel, ao_accel, ao_speed);
+ break;
+#endif
+
case ao_flight_pad:
/* pad to boost:
*
case ao_flight_pad:
/* pad to boost:
*
*/
if (ao_height > AO_M_TO_HEIGHT(20)
#if HAS_ACCEL
*/
if (ao_height > AO_M_TO_HEIGHT(20)
#if HAS_ACCEL
- || (ao_accel > AO_MSS_TO_ACCEL(20) &&
- ao_speed > AO_MS_TO_SPEED(5))
+ || (ao_accel > AO_MSS_TO_ACCEL(20)
+ && ao_speed > AO_MS_TO_SPEED(5))
#endif /* else FORCE_ACCEL */
#endif /* HAS_ACCEL */
#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
+
++nsamples;
else
ao_sample_preflight_set();
++nsamples;
else
ao_sample_preflight_set();
+#if !HAS_BARO
+ if ((nsamples & 0x3f) == 0)
+ ao_kalman_reset_accumulate();
+#endif
+#if !HAS_BARO
+void ao_kalman_reset_accumulate(void);
+#endif
+
#endif /* _AO_SAMPLE_H_ */
#endif /* _AO_SAMPLE_H_ */