From 2f32a19aedb7d0e33899038e3fb04fe8a773291a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 12 Aug 2019 17:00:47 -0700 Subject: [PATCH] altos: Allow accel-only flight code EasyTimer won't have a baro sensor, so we need some way to track at least the ascent part of a flight. Signed-off-by: Keith Packard --- src/kernel/ao_flight.c | 85 +++++++++++++++++++++++++++++++++++++++--- src/kernel/ao_kalman.c | 18 ++++++++- src/kernel/ao_sample.c | 12 ++++++ 3 files changed, 108 insertions(+), 7 deletions(-) diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 5a5d5b72..afee1de7 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -21,6 +21,8 @@ #include #endif +#include + #if HAS_MPU6000 || HAS_MPU9250 #include #endif @@ -62,12 +64,29 @@ uint8_t ao_sensor_errors; * 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 @@ -109,9 +128,12 @@ ao_flight(void) 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 @@ -203,7 +225,9 @@ ao_flight(void) 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 */ @@ -238,7 +262,18 @@ ao_flight(void) (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; @@ -247,7 +282,7 @@ ao_flight(void) 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, @@ -264,6 +299,7 @@ ao_flight(void) #endif case ao_flight_coast: +#if HAS_BARO /* * By customer request - allow the user * to lock out apogee detection for a specified @@ -308,9 +344,45 @@ ao_flight(void) 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; @@ -321,6 +393,7 @@ ao_flight(void) #endif break; +#if HAS_BARO case ao_flight_drogue: /* drogue to main deploy: @@ -363,7 +436,6 @@ ao_flight(void) * * 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) @@ -385,6 +457,7 @@ ao_flight(void) ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; } break; +#endif /* HAS_BARO */ #if HAS_FLIGHT_DEBUG case ao_flight_test: #if HAS_GYRO diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 4f4ffe8f..aaf0595f 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -86,6 +86,7 @@ ao_kalman_predict(void) ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100; } +#if HAS_BARO static void ao_kalman_err_height(void) { @@ -140,7 +141,9 @@ ao_kalman_err_height(void) #endif } } +#endif +#if HAS_BARO static void ao_kalman_correct_baro(void) { @@ -163,6 +166,7 @@ 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 @@ -177,7 +181,7 @@ ao_kalman_err_accel(void) ao_error_a = (accel - ao_k_accel) >> 16; } -#ifndef FORCE_ACCEL +#if !defined(FORCE_ACCEL) && HAS_BARO static void ao_kalman_correct_both(void) { @@ -255,12 +259,14 @@ ao_kalman_correct_accel(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; @@ -273,6 +279,7 @@ void ao_kalman(void) { ao_kalman_predict(); +#if HAS_BARO #if HAS_ACCEL if (ao_flight_state <= ao_flight_coast) { #ifdef FORCE_ACCEL @@ -283,12 +290,21 @@ ao_kalman(void) } 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; diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 9cba36c1..b3e12b19 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -36,9 +36,11 @@ #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 @@ -60,8 +62,10 @@ uint8_t ao_sample_data; * 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 */ @@ -81,7 +85,9 @@ int32_t ao_ground_roll; 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 @@ -105,7 +111,9 @@ ao_sample_preflight_add(void) #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; @@ -171,9 +179,11 @@ ao_sample_preflight_set(void) 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; @@ -375,8 +385,10 @@ ao_sample_init(void) { 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; -- 2.30.2