altos: Respect apogee lockout time in flight algorithm

[fw/altos] / src / core / ao_flight.c

diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index a5cf7468f439728617bfe5877c55d6341ac6becd..494e656d5ccd3dabe5139cdd1dab6303ba84c8d0 100644 (file)
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -40,9 +40,11 @@ __pdata uint16_t             ao_boost_tick;          /* time of launch detect */
  * track min/max data over a long interval to detect
  * resting
  */
-__pdata uint16_t               ao_interval_end;
-__pdata int16_t                        ao_interval_min_height;
-__pdata int16_t                        ao_interval_max_height;
+static __data uint16_t         ao_interval_end;
+static __data int16_t          ao_interval_min_height;
+static __data int16_t          ao_interval_max_height;
+static __data int16_t          ao_coast_avg_accel;
+
 __pdata uint8_t                        ao_flight_force_idle;
 
 /* We also have a clock, which can be used to sanity check things in
@@ -91,6 +93,8 @@ ao_flight(void)
                                 */
                                ao_flight_state = ao_flight_invalid;
 
+                               /* Turn on packet system in invalid mode on TeleMetrum */
+                               ao_packet_slave_start();
                        } else
 #endif
                                if (!ao_flight_force_idle
@@ -108,8 +112,10 @@ ao_flight(void)
                                ao_usb_disable();
 #endif
 
-                               /* Disable packet mode in pad state */
+#if !HAS_ACCEL
+                               /* Disable packet mode in pad state on TeleMini */
                                ao_packet_slave_stop();
+#endif
 
                                /* Turn on telemetry system */
                                ao_rdf_set(1);
@@ -121,6 +127,11 @@ ao_flight(void)
                                /* Set idle mode */
                                ao_flight_state = ao_flight_idle;
  
+#if HAS_ACCEL
+                               /* Turn on packet system in idle mode on TeleMetrum */
+                               ao_packet_slave_start();
+#endif
+
                                /* signal successful initialization by turning off the LED */
                                ao_led_off(AO_LED_RED);
                        }
@@ -188,6 +199,7 @@ ao_flight(void)
                        {
 #if HAS_ACCEL
                                ao_flight_state = ao_flight_fast;
+                               ao_coast_avg_accel = ao_accel;
 #else
                                ao_flight_state = ao_flight_coast;
 #endif
@@ -211,6 +223,17 @@ ao_flight(void)
 #endif
                case ao_flight_coast:
 
+                       /*
+                        * By customer request - allow the user
+                        * to lock out apogee detection for a specified
+                        * number of seconds.
+                        */
+                       if (ao_config.apogee_lockout) {
+                               if ((ao_sample_tick - ao_boost_tick) <
+                                   AO_SEC_TO_TICKS(ao_config.apogee_lockout))
+                                       break;
+                       }
+
                        /* apogee detect: coast to drogue deploy:
                         *
                         * speed: < 0
@@ -241,7 +264,8 @@ ao_flight(void)
 #if HAS_ACCEL
                        else {
                        check_re_boost:
-                               if (ao_accel > AO_MSS_TO_ACCEL(20)) {
+                               ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6);
+                               if (ao_coast_avg_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));