altos: Complain about sensor self-test errors only in idle mode

When the accelerometer says to go into pad mode, don't look for other
sensor self test errors. Only look for sensor self test errors to
choose between idle and invalid mode. This will prevent minor sensor
self test errors from letting the rocket fly safely.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index 463ff4a2974b38e045fb2214ca9bc163cd490d28..5918b997e6250eefb84bee7f26d8fd057760ae85 100644 (file)
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -104,9 +104,6 @@ ao_flight(void)
                            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_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 ||

-#if HAS_IMU
-                           ao_sensor_errors ||
-#endif

                            ao_ground_height < -1000 ||
                            ao_ground_height > 7000)
                        {
                            ao_ground_height < -1000 ||
                            ao_ground_height > 7000)
                        {


@@ -152,7 +149,11 @@ ao_flight(void)
 #endif
                        } else {
                                /* Set idle mode */
 #endif
                        } else {
                                /* Set idle mode */

-                               ao_flight_state = ao_flight_idle;
+                               ao_flight_state = ao_flight_idle;
+#if HAS_IMU
+                               if (ao_sensor_errors)
+                                       ao_flight_state = ao_flight_invalid;
+#endif

  
 #if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE
                                /* Turn on packet system in idle mode on TeleMetrum */
  
 #if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE
                                /* Turn on packet system in idle mode on TeleMetrum */