From 527994d05e8caf9d3bc31461a8d4a05a596b4603 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 6 May 2020 22:34:51 -0700 Subject: [PATCH] altos: Adjust flight algorithm to handle accel-only timer products MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit Make our accel-only product (easytimer) follow a shortend flight state sequence: 1) startup Average samples 2.1) startup -> idle. Defined axis accel indicates not pointing up 2.2) startup -> pad. Defined axes accel indicates pointing up 3) pad -> boost. Accel > 20m/s² and speed > 5m/s 4) boost -> coast. accel < -2.5m/s² or 15 second boost 5.1) coast -> boost. average accel > 20m/s² 5.2) coast -> landed. All three accel axes stable for 10 seconds (< 2m/s² accel change) Signed-off-by: Keith Packard --- src/drivers/ao_bmx160.h | 2 + src/easytimer-v1/ao_pins.h | 2 + src/kernel/ao_data.h | 3 +- src/kernel/ao_flight.c | 77 +++++++++++++++++++++++++++++++++++--- 4 files changed, 77 insertions(+), 7 deletions(-) diff --git a/src/drivers/ao_bmx160.h b/src/drivers/ao_bmx160.h index 9fc33341..c5bf0aff 100644 --- a/src/drivers/ao_bmx160.h +++ b/src/drivers/ao_bmx160.h @@ -306,4 +306,6 @@ ao_bmx160_accel(int16_t sensor) { return (float) sensor * ((float) (BMX160_ACCEL_FULLSCALE * GRAVITY / 32767.0)); } +#define ao_bmx_accel_to_sample(accel) ((accel_t) (accel) * (32767.0f / (BMX160_ACCEL_FULLSCALE * GRAVITY))) + #endif /* _BMX160_H_ */ diff --git a/src/easytimer-v1/ao_pins.h b/src/easytimer-v1/ao_pins.h index 69e6e43c..593119b9 100644 --- a/src/easytimer-v1/ao_pins.h +++ b/src/easytimer-v1/ao_pins.h @@ -227,6 +227,8 @@ struct ao_adc { #define ao_data_mag_across(packet) (-(packet)->bmx160.mag_y) #define ao_data_mag_through(packet) ((packet)->bmx160.mag_z) +#define ao_data_accel_cook(packet) (-ao_data_along(packet)) + /* * Monitor */ diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h index b28c76b7..a22eee87 100644 --- a/src/kernel/ao_data.h +++ b/src/kernel/ao_data.h @@ -454,10 +454,11 @@ static inline float ao_convert_accel(int16_t sensor) typedef int16_t accel_t; #define ao_data_accel(packet) ((packet)->z_accel) -#define ao_data_accel_cook(packet) ((packet)->bmx160.acc_y) #define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) #define ao_data_accel_invert(a) (-(a)) +#define ao_data_accel_to_sample(accel) ao_bmx_accel_to_sample(accel) + #endif #if !HAS_GYRO && HAS_BMX160 diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index ba1f00d0..8a5f9d37 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -64,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 @@ -244,7 +261,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; @@ -253,7 +281,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, @@ -270,6 +298,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 @@ -314,9 +343,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; @@ -327,6 +392,7 @@ ao_flight(void) #endif break; +#if HAS_BARO case ao_flight_drogue: /* drogue to main deploy: @@ -370,10 +436,7 @@ 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) - ao_interval_max_height = ao_avg_height; + check_bounds(ao_avg_height, ao_interval_ if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4)) @@ -387,10 +450,12 @@ ao_flight(void) ao_wakeup(&ao_flight_state); } + init ao_interval_min_height = ao_interval_max_height = ao_avg_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 -- 2.47.2