From 0820f5c6dfe067590f36e8201a4049719dcf3d7c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 21 Sep 2011 11:40:56 -0700 Subject: [PATCH 1/1] altos: Support staging by going back to boost as needed Detect additional motor burns by looking for high positive acceleration in coast or fast modes. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 85c1825b..a5cf7468 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -34,7 +34,7 @@ /* Main flight thread. */ __pdata enum ao_flight_state ao_flight_state; /* current flight state */ -__pdata uint16_t ao_launch_tick; /* time of launch detect */ +__pdata uint16_t ao_boost_tick; /* time of launch detect */ /* * track min/max data over a long interval to detect @@ -151,7 +151,7 @@ ao_flight(void) ) { ao_flight_state = ao_flight_boost; - ao_launch_tick = ao_sample_tick; + ao_boost_tick = ao_sample_tick; /* start logging data */ ao_log_start(); @@ -184,7 +184,7 @@ ao_flight(void) * (15 seconds) has past. */ if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) || - (int16_t) (ao_sample_tick - ao_launch_tick) > BOOST_TICKS_MAX) + (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) { #if HAS_ACCEL ao_flight_state = ao_flight_fast; @@ -205,7 +205,8 @@ ao_flight(void) { ao_flight_state = ao_flight_coast; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } + } else + goto check_re_boost; break; #endif case ao_flight_coast: @@ -237,6 +238,16 @@ ao_flight(void) ao_flight_state = ao_flight_drogue; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } +#if HAS_ACCEL + else { + check_re_boost: + if (ao_accel > AO_MSS_TO_ACCEL(20)) { + ao_boost_tick = ao_sample_tick; + ao_flight_state = ao_flight_boost; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + } +#endif break; case ao_flight_drogue: -- 2.30.2