altos: Added check for out of bounds accel

[fw/altos] / src / ao_flight.c

diff --git a/src/ao_flight.c b/src/ao_flight.c
index 7fe85cb1522ac786579b824cd714c97dc088d9f3..01dbb11b04d030f39aba529ecaae6e89b306add9 100644 (file)
--- a/src/ao_flight.c
+++ b/src/ao_flight.c
@@ -196,17 +196,17 @@ ao_flight(void)
 
                        /* startup state:
                         *
-                        * Collect 1000 samples of acceleration and pressure
+                        * Collect 512 samples of acceleration and pressure
                         * data and average them to find the resting values
                         */
-                       if (nsamples < 1000) {
+                       if (nsamples < 512) {
                                ao_raw_accel_sum += ao_raw_accel;
                                ao_raw_pres_sum += ao_raw_pres;
                                ++nsamples;
                                continue;
                        }
-                       ao_ground_accel = (ao_raw_accel_sum / nsamples);
-                       ao_ground_pres = (ao_raw_pres_sum / nsamples);
+                       ao_ground_accel = ao_raw_accel_sum >> 9;
+                       ao_ground_pres = ao_raw_pres_sum >> 9;
                        ao_min_pres = ao_ground_pres;
                        ao_config_get();
                        ao_main_pres = ao_altitude_to_pres(ao_pres_to_altitude(ao_ground_pres) + ao_config.main_deploy);
@@ -221,6 +221,7 @@ ao_flight(void)
                        if (ao_config.accel_plus_g != 0 &&
                            ao_config.accel_minus_g != 0 &&
                            ao_flight_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP &&
+                           ao_flight_accel > ao_config.accel_plus_g - ACCEL_NOSE_UP &&
                            !ao_flight_force_idle)
                        {
                                /* Disable the USB controller in flight mode
@@ -236,9 +237,18 @@ ao_flight(void)
                                ao_flight_state = ao_flight_pad;
                                ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
                        } else {
-                               ao_flight_state = ao_flight_idle;
+                               if (ao_flight_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP ||
+                                   ao_flight_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP)
+                               {
+                                       /* Detected an accel value outside -1.5g to 1.5g
+                                        * -> invalid mode
+                                        */
+                                       ao_flight_state = ao_flight_invalid;
+                               } else {
+                                       ao_flight_state = ao_flight_idle;
+                               }
 
-                               /* Turn on packet system in idle mode
+                               /* Turn on packet system in idle or invalid mode
                                 */
                                ao_packet_slave_start();
                                ao_wakeup(DATA_TO_XDATA(&ao_flight_state));