/*
* Above this height, the baro sensor doesn't work
*/
-#define AO_MAX_BARO_HEIGHT 8000
+#define AO_MAX_BARO_HEIGHT 12000
/*
* Above this speed, baro measurements are unreliable
*/
-#define AO_MAX_BARO_SPEED 300
+#define AO_MAX_BARO_SPEED 200
static void
ao_kalman_predict(void)
return;
}
+ if (ao_flight_debug) {
+ printf ("predict speed %g + (%g * %g) = %g\n",
+ ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0,
+ (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
+ }
#endif
ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 +
(int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
ao_kalman_err_height(void)
{
int16_t e;
+ int16_t height_distrust;
+#if HAS_ACCEL
+ int16_t speed_distrust;
+#endif
+
ao_error_h = ao_raw_height - (int16_t) (ao_k_height >> 16);
e = ao_error_h;
e = -e;
if (e > 127)
e = 127;
+#if HAS_ACCEL
+ ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2;
+ ao_error_h_sq_avg += (e * e) >> 2;
+#else
ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4;
ao_error_h_sq_avg += (e * e) >> 4;
+#endif
+
+ height_distrust = ao_raw_height - 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.
+ */
+ speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1);
+ if (speed_distrust <= 0)
+ speed_distrust = 0;
+ else if (speed_distrust > height_distrust)
+ height_distrust = speed_distrust;
+#endif
+ if (height_distrust <= 0)
+ height_distrust = 0;
+
+ if (height_distrust) {
+#ifdef AO_FLIGHT_TEST
+ int old_ao_error_h = ao_error_h;
+#endif
+ if (height_distrust > 0x100)
+ height_distrust = 0x100;
+ ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8);
+#ifdef AO_FLIGHT_TEST
+ if (ao_flight_debug) {
+ printf("over height %g over speed %g distrust: %g height: error %d -> %d\n",
+ (double) (ao_raw_height - 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);
+ }
+#endif
+ }
}
static 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) {
+ 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_10 / 65536.0,
+ (double) ao_error_a, AO_BOTH_K11_10 / 65536.0,
+ (ao_k_speed +
+ (int32_t) AO_BOTH_K10_10 * ao_error_h +
+ (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
+ }
ao_k_height +=
(int32_t) AO_BOTH_K00_10 * ao_error_h +
- (int32_t) (AO_BOTH_K01_10 >> 4) * ao_error_a;
+ (int32_t) AO_BOTH_K01_10 * ao_error_a;
ao_k_speed +=
- ((int32_t) AO_BOTH_K10_10 << 4) * ao_error_h +
+ (int32_t) AO_BOTH_K10_10 * 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_K20_10 * ao_error_h +
(int32_t) AO_BOTH_K21_10 * ao_error_a;
return;
}
+ 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_100 / 65536.0,
+ (double) ao_error_a, AO_BOTH_K11_100 / 65536.0,
+ (ao_k_speed +
+ (int32_t) AO_BOTH_K10_100 * ao_error_h +
+ (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
+ }
#endif
ao_k_height +=
(int32_t) AO_BOTH_K00_100 * ao_error_h +
(int32_t) AO_BOTH_K21_100 * ao_error_a;
}
+#ifdef FORCE_ACCEL
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;
}
-#endif
ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a;
ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a;
ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a;
}
+#endif
#endif /* HAS_ACCEL */
__xdata int32_t ao_raw_pres_sum;
ao_kalman_predict();
#if HAS_ACCEL
if (ao_flight_state <= ao_flight_coast) {
-#ifndef FORCE_ACCEL
- if (/*ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED) &&*/
- ao_raw_alt < AO_MAX_BARO_HEIGHT)
- ao_kalman_correct_both();
- else
+#ifdef FORCE_ACCEL
+ ao_kalman_correct_accel();
+#else
+ ao_kalman_correct_both();
#endif
- ao_kalman_correct_accel();
} else
#endif
- if (ao_raw_alt < AO_MAX_BARO_HEIGHT || ao_flight_state >= ao_flight_drogue)
ao_kalman_correct_baro();
ao_height = from_fix(ao_k_height);
ao_speed = from_fix(ao_k_speed);
if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) ||
(int16_t) (ao_flight_tick - ao_launch_tick) > BOOST_TICKS_MAX)
{
+#if HAS_ACCEL
ao_flight_state = ao_flight_fast;
+#else
+ ao_flight_state = ao_flight_coast;
+#endif
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
break;
}
break;
+#if HAS_ACCEL
case ao_flight_fast:
/*
* This is essentially the same as coast,
* but the barometer is being ignored as
* it may be unreliable.
*/
- if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) {
+ if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED) &&
+ (ao_raw_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 30))
+ {
ao_flight_state = ao_flight_coast;
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
break;
}
break;
+#endif
case ao_flight_coast:
/* apogee detect: coast to drogue deploy:
* the measured altitude reasonably closely; otherwise
* we're probably transsonic.
*/
- if (ao_speed < 0 && (ao_raw_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100))
+ if (ao_speed < 0
+#if !HAS_ACCEL
+ && (ao_raw_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 30)
+#endif
+ )
{
/* ignite the drogue charge */
ao_ignite(ao_igniter_drogue);
projects / fw / altos / blobdiff