#define AO_K_STEP_10 to_fix16(0.1)
#define AO_K_STEP_2_2_10 to_fix16(0.005)
+#define AO_K_STEP_1 to_fix16(1)
+#define AO_K_STEP_2_2_1 to_fix16(0.5)
+
__pdata int16_t ao_height;
__pdata int16_t ao_speed;
__pdata int16_t ao_accel;
__pdata int16_t ao_max_height;
+static __pdata int32_t ao_avg_height_scaled;
+__pdata int16_t ao_avg_height;
__pdata int16_t ao_error_h;
__pdata int16_t ao_error_h_sq_avg;
ao_kalman_predict(void)
{
#ifdef AO_FLIGHT_TEST
+ if (ao_sample_tick - ao_sample_prev_tick > 50) {
+ ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 +
+ (int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
+ ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1;
+
+ return;
+ }
if (ao_sample_tick - ao_sample_prev_tick > 5) {
ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 +
(int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
ao_error_h_sq_avg += (e * e) >> 4;
#endif
- height_distrust = ao_sample_height - AO_MAX_BARO_HEIGHT;
+ if (ao_flight_state >= ao_flight_drogue)
+ return;
+ height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT;
#if HAS_ACCEL
/* speed is stored * 16, but we need to ramp between 200 and 328, so
* we want to multiply by 2. The result is a shift by 3.
else if (speed_distrust > height_distrust)
height_distrust = speed_distrust;
#endif
- if (height_distrust <= 0)
- height_distrust = 0;
-
- if (height_distrust) {
+ if (height_distrust > 0) {
#ifdef AO_FLIGHT_TEST
int old_ao_error_h = ao_error_h;
#endif
#ifdef AO_FLIGHT_TEST
if (ao_flight_debug) {
printf("over height %g over speed %g distrust: %g height: error %d -> %d\n",
- (double) (ao_sample_height - AO_MAX_BARO_HEIGHT),
+ (double) (ao_sample_alt - AO_MAX_BARO_HEIGHT),
(ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) / 16.0,
height_distrust / 256.0,
old_ao_error_h, ao_error_h);
{
ao_kalman_err_height();
#ifdef AO_FLIGHT_TEST
+ if (ao_sample_tick - ao_sample_prev_tick > 50) {
+ ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h;
+ ao_k_speed += (int32_t) AO_BARO_K1_1 * ao_error_h;
+ ao_k_accel += (int32_t) AO_BARO_K2_1 * ao_error_h;
+ return;
+ }
if (ao_sample_tick - ao_sample_prev_tick > 5) {
ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h;
ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h;
ao_kalman_err_accel();
#ifdef AO_FLIGHT_TEST
+ if (ao_sample_tick - ao_sample_prev_tick > 50) {
+ if (ao_flight_debug) {
+ printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n",
+ ao_k_speed / (65536.0 * 16.0),
+ (double) ao_error_h, AO_BOTH_K10_1 / 65536.0,
+ (double) ao_error_a, AO_BOTH_K11_1 / 65536.0,
+ (ao_k_speed +
+ (int32_t) AO_BOTH_K10_1 * ao_error_h +
+ (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
+ }
+ ao_k_height +=
+ (int32_t) AO_BOTH_K00_1 * ao_error_h +
+ (int32_t) AO_BOTH_K01_1 * ao_error_a;
+ ao_k_speed +=
+ (int32_t) AO_BOTH_K10_1 * ao_error_h +
+ (int32_t) AO_BOTH_K11_1 * ao_error_a;
+ ao_k_accel +=
+ (int32_t) AO_BOTH_K20_1 * ao_error_h +
+ (int32_t) AO_BOTH_K21_1 * ao_error_a;
+ return;
+ }
if (ao_sample_tick - ao_sample_prev_tick > 5) {
if (ao_flight_debug) {
printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n",
ao_accel = from_fix(ao_k_accel);
if (ao_height > ao_max_height)
ao_max_height = ao_height;
+ ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height;
+#ifdef AO_FLIGHT_TEST
+ if (ao_sample_tick - ao_sample_prev_tick > 50)
+ ao_avg_height = (ao_avg_height_scaled + 1) >> 1;
+ else if (ao_sample_tick - ao_sample_prev_tick > 5)
+ ao_avg_height = (ao_avg_height_scaled + 7) >> 4;
+ else
+#endif
+ ao_avg_height = (ao_avg_height_scaled + 63) >> 7;
#ifdef AO_FLIGHT_TEST
ao_sample_prev_tick = ao_sample_tick;
#endif