#include <ao_log.h>
#endif
+#include <ao_flight.h>
+
#if HAS_MPU6000 || HAS_MPU9250
#include <ao_quaternion.h>
#endif
* resting
*/
static uint16_t ao_interval_end;
+#ifdef HAS_BARO
static ao_v_t ao_interval_min_height;
static ao_v_t ao_interval_max_height;
+#else
+static accel_t ao_interval_min_accel_along, ao_interval_max_accel_along;
+static accel_t ao_interval_min_accel_across, ao_interval_max_accel_across;
+static accel_t ao_interval_min_accel_through, ao_interval_max_accel_through;
+#endif
#if HAS_ACCEL
static ao_v_t ao_coast_avg_accel;
#endif
+#define init_bounds(_cur, _min, _max) do { \
+ _min = _max = _cur; \
+ } while (0)
+
+#define check_bounds(_cur, _min, _max) do { \
+ if (_cur < _min) \
+ _min = _cur; \
+ if (_cur > _max) \
+ _max = _cur; \
+ } while(0)
+
uint8_t ao_flight_force_idle;
/* We also have a clock, which can be used to sanity check things in
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_height < -1000 ||
- ao_ground_height > 7000)
+ ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP
+#if HAS_BARO
+ || ao_ground_height < -1000 ||
+ ao_ground_height > 7000
+#endif
+ )
{
/* Detected an accel value outside -1.5g to 1.5g
* (or uncalibrated values), so we go into invalid mode
ao_launch_tick = ao_boost_tick = ao_sample_tick;
/* start logging data */
+#if HAS_LOG
ao_log_start();
+#endif
#if HAS_TELEMETRY
/* Increase telemetry rate */
(int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX)
{
#if HAS_ACCEL
+#if HAS_BARO
ao_flight_state = ao_flight_fast;
+#else
+ ao_flight_state = ao_flight_coast;
+
+ /* Initialize landing detection interval values */
+ ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
+
+ init_bounds(ao_sample_accel_along, ao_interval_min_accel_along, ao_interval_max_accel_along);
+ init_bounds(ao_sample_accel_across, ao_interval_min_accel_across, ao_interval_max_accel_across);
+ init_bounds(ao_sample_accel_through, ao_interval_min_accel_through, ao_interval_max_accel_through);
+#endif
ao_coast_avg_accel = ao_accel;
#else
ao_flight_state = ao_flight_coast;
ao_wakeup(&ao_flight_state);
}
break;
-#if HAS_ACCEL
+#if HAS_ACCEL && HAS_BARO
case ao_flight_fast:
/*
* This is essentially the same as coast,
#endif
case ao_flight_coast:
+#if HAS_BARO
/*
* By customer request - allow the user
* to lock out apogee detection for a specified
ao_flight_state = ao_flight_drogue;
ao_wakeup(&ao_flight_state);
}
+ else
+#else /* not HAS_BARO */
+ /* coast to land:
+ *
+ * accel: values stable
+ */
+ check_bounds(ao_sample_accel_along, ao_interval_min_accel_along, ao_interval_max_accel_along);
+ check_bounds(ao_sample_accel_across, ao_interval_min_accel_across, ao_interval_max_accel_across);
+ check_bounds(ao_sample_accel_through, ao_interval_min_accel_through, ao_interval_max_accel_through);
+
+#define MAX_QUIET_ACCEL 2
+
+ if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) {
+ if (ao_interval_max_accel_along - ao_interval_min_accel_along <= ao_data_accel_to_sample(MAX_QUIET_ACCEL) &&
+ ao_interval_max_accel_across - ao_interval_min_accel_across <= ao_data_accel_to_sample(MAX_QUIET_ACCEL) &&
+ ao_interval_max_accel_through - ao_interval_min_accel_through <= ao_data_accel_to_sample(MAX_QUIET_ACCEL))
+ {
+ ao_flight_state = ao_flight_landed;
+#if HAS_ADC
+ /* turn off the ADC capture */
+ ao_timer_set_adc_interval(0);
+#endif
+
+ ao_wakeup(&ao_flight_state);
+ }
+
+ /* Reset interval values */
+ ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
+
+ init_bounds(ao_sample_accel_along, ao_interval_min_accel_along, ao_interval_max_accel_along);
+ init_bounds(ao_sample_accel_across, ao_interval_min_accel_across, ao_interval_max_accel_across);
+ init_bounds(ao_sample_accel_through, ao_interval_min_accel_through, ao_interval_max_accel_through);
+ }
+#endif
#if HAS_ACCEL
- else {
+ {
+#if HAS_BARO
check_re_boost:
+#endif
ao_coast_avg_accel = ao_coast_avg_accel + ((ao_accel - ao_coast_avg_accel) >> 5);
if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) {
ao_boost_tick = ao_sample_tick;
#endif
break;
+#if HAS_BARO
case ao_flight_drogue:
/* drogue to main deploy:
*
* barometer: altitude stable
*/
-
if (ao_avg_height < ao_interval_min_height)
ao_interval_min_height = ao_avg_height;
if (ao_avg_height > ao_interval_max_height)
ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
}
break;
+#endif /* HAS_BARO */
#if HAS_FLIGHT_DEBUG
case ao_flight_test:
#if HAS_GYRO
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;
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;
#endif
uint16_t ao_sample_tick; /* time of last data */
+#if HAS_BARO
pres_t ao_sample_pres;
alt_t ao_sample_alt;
alt_t ao_sample_height;
+#endif
#if HAS_ACCEL
accel_t ao_sample_accel;
#endif
* Sensor calibration values
*/
+#if HAS_BARO
pres_t ao_ground_pres; /* startup pressure */
alt_t ao_ground_height; /* MSL of ao_ground_pres */
+#endif
#if HAS_ACCEL
accel_t ao_ground_accel; /* startup acceleration */
static uint8_t ao_preflight; /* in preflight mode */
static uint16_t nsamples;
+#if HAS_BARO
int32_t ao_sample_pres_sum;
+#endif
#if HAS_ACCEL
int32_t ao_sample_accel_sum;
#endif
#if HAS_ACCEL
ao_sample_accel_sum += ao_sample_accel;
#endif
+#if HAS_BARO
ao_sample_pres_sum += ao_sample_pres;
+#endif
#if HAS_GYRO
ao_sample_accel_along_sum += ao_sample_accel_along;
ao_sample_accel_across_sum += ao_sample_accel_across;
ao_ground_accel = ao_sample_accel_sum >> 9;
ao_sample_accel_sum = 0;
#endif
+#if HAS_BARO
ao_ground_pres = ao_sample_pres_sum >> 9;
ao_ground_height = pres_to_altitude(ao_ground_pres);
ao_sample_pres_sum = 0;
+#endif
#if HAS_GYRO
ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
{
ao_config_get();
nsamples = 0;
+#if HAS_BARO
ao_sample_pres_sum = 0;
ao_sample_pres = 0;
+#endif
#if HAS_ACCEL
ao_sample_accel_sum = 0;
ao_sample_accel = 0;