altos: Try IMU self-test 10 times before giving up

This should keep the device from failing to boot unless the IMU is
actually broken. Oh, and if self test does fail, this places the
flight computer in 'Invalid' state rather than panic.

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

diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index 4a53bdc672f7af22deb6c94a0fcfb2b2581e14d5..463ff4a2974b38e045fb2214ca9bc163cd490d28 100644 (file)
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -46,6 +46,11 @@ __pdata enum ao_flight_state ao_flight_state;        /* current flight state */
 __pdata uint16_t               ao_boost_tick;          /* time of launch detect */
 __pdata uint16_t               ao_motor_number;        /* number of motors burned so far */
 
+#if HAS_IMU
+/* Any sensor can set this to mark the flight computer as 'broken' */
+__xdata uint8_t                        ao_sensor_errors;
+#endif
+
 /*
  * track min/max data over a long interval to detect
  * resting
@@ -99,6 +104,9 @@ 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 ||
+#if HAS_IMU
+                           ao_sensor_errors ||
+#endif
                            ao_ground_height < -1000 ||
                            ao_ground_height > 7000)
                        {

diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h
index ac3e9cfb56ab40a4b14fa076f92097a57f12f895..c7c02ccf69cc4166e0549513d5d1b02971a4c078 100644 (file)
--- a/src/core/ao_flight.h
+++ b/src/core/ao_flight.h
@@ -41,6 +41,10 @@ extern __pdata enum ao_flight_state  ao_flight_state;
 extern __pdata uint16_t                        ao_boost_tick;
 extern __pdata uint16_t                        ao_motor_number;
 
+#if HAS_IMU
+extern __xdata uint8_t                 ao_sensor_errors;
+#endif
+
 extern __pdata uint16_t                        ao_launch_time;
 extern __pdata uint8_t                 ao_flight_force_idle;
 

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index 5e75b78a13787e8546235bcaeeb44de4c45e0a41..f8ce7346c7149d690228c57445607382ce35c05b 100644 (file)
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -137,10 +137,10 @@ ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
 {
        int16_t diff = test - normal;
 
-       if (diff < MPU6000_ST_ACCEL(16) / 2) {
+       if (diff < MPU6000_ST_ACCEL(16) / 4) {
                return 1;
        }
-       if (diff > MPU6000_ST_ACCEL(16) * 2) {
+       if (diff > MPU6000_ST_ACCEL(16) * 4) {
                return 1;
        }
        return 0;
@@ -153,10 +153,10 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
 
        if (diff < 0)
                diff = -diff;
-       if (diff < MPU6000_ST_GYRO(2000) / 2) {
+       if (diff < MPU6000_ST_GYRO(2000) / 4) {
                return 1;
        }
-       if (diff > MPU6000_ST_GYRO(2000) * 2) {
+       if (diff > MPU6000_ST_GYRO(2000) * 4) {
                return 1;
        }
        return 0;
@@ -177,11 +177,14 @@ _ao_mpu6000_wait_alive(void)
                ao_panic(AO_PANIC_SELF_TEST_MPU6000);
 }
 
+#define ST_TRIES       10
+
 static void
 _ao_mpu6000_setup(void)
 {
        struct ao_mpu6000_sample        normal_mode, test_mode;
-       int                             errors =0;
+       int                             errors;
+       int                             st_tries;
 
        if (ao_mpu6000_configured)
                return;
@@ -236,23 +239,6 @@ _ao_mpu6000_setup(void)
                              (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
                              (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
 
-       /* Configure accelerometer to +/-16G in self-test mode */
-       _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
-                             (1 << MPU600_ACCEL_CONFIG_XA_ST) |
-                             (1 << MPU600_ACCEL_CONFIG_YA_ST) |
-                             (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
-                             (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
-
-       /* Configure gyro to +/- 2000°/s in self-test mode */
-       _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
-                             (1 << MPU600_GYRO_CONFIG_XG_ST) |
-                             (1 << MPU600_GYRO_CONFIG_YG_ST) |
-                             (1 << MPU600_GYRO_CONFIG_ZG_ST) |
-                             (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
-
-       ao_delay(AO_MS_TO_TICKS(200));
-       _ao_mpu6000_sample(&test_mode);
-
 #if TRIDGE
        // read the product ID rev c has 1/2 the sensitivity of rev d
        _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
@@ -268,36 +254,58 @@ _ao_mpu6000_setup(void)
                register_write(MPUREG_ACCEL_CONFIG,2<<3);
        }
        hal.scheduler->delay(1);
-
 #endif
 
-       /* Configure accelerometer to +/-16G */
-       _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
-                             (0 << MPU600_ACCEL_CONFIG_XA_ST) |
-                             (0 << MPU600_ACCEL_CONFIG_YA_ST) |
-                             (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
-                             (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
-
-       /* Configure gyro to +/- 2000°/s */
-       _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
-                             (0 << MPU600_GYRO_CONFIG_XG_ST) |
-                             (0 << MPU600_GYRO_CONFIG_YG_ST) |
-                             (0 << MPU600_GYRO_CONFIG_ZG_ST) |
-                             (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
-
-       ao_delay(AO_MS_TO_TICKS(10));
-       _ao_mpu6000_sample(&normal_mode);
+       for (st_tries = 0; st_tries < ST_TRIES; st_tries++) {
+               errors = 0;
+
+               /* Configure accelerometer to +/-16G in self-test mode */
+               _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+                                     (1 << MPU600_ACCEL_CONFIG_XA_ST) |
+                                     (1 << MPU600_ACCEL_CONFIG_YA_ST) |
+                                     (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
+                                     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+               /* Configure gyro to +/- 2000°/s in self-test mode */
+               _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+                                     (1 << MPU600_GYRO_CONFIG_XG_ST) |
+                                     (1 << MPU600_GYRO_CONFIG_YG_ST) |
+                                     (1 << MPU600_GYRO_CONFIG_ZG_ST) |
+                                     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+               ao_delay(AO_MS_TO_TICKS(200));
+               _ao_mpu6000_sample(&test_mode);
+
+               /* Configure accelerometer to +/-16G */
+               _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+                                     (0 << MPU600_ACCEL_CONFIG_XA_ST) |
+                                     (0 << MPU600_ACCEL_CONFIG_YA_ST) |
+                                     (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
+                                     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+               /* Configure gyro to +/- 2000°/s */
+               _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+                                     (0 << MPU600_GYRO_CONFIG_XG_ST) |
+                                     (0 << MPU600_GYRO_CONFIG_YG_ST) |
+                                     (0 << MPU600_GYRO_CONFIG_ZG_ST) |
+                                     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+               ao_delay(AO_MS_TO_TICKS(200));
+               _ao_mpu6000_sample(&normal_mode);
        
-       errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
-       errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
-       errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
-
-       errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
-       errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
-       errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+               errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
+               errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
+               errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
+
+               errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
+               errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
+               errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+               if (!errors)
+                       break;
+       }
 
-       if (errors)
-               ao_panic(AO_PANIC_SELF_TEST_MPU6000);
+       if (st_tries == ST_TRIES)
+               ao_sensor_errors = 1;
 
        /* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
        _ao_mpu6000_reg_write(MPU6000_CONFIG,

diff --git a/src/telemega-v0.1/ao_pins.h b/src/telemega-v0.1/ao_pins.h
index 7ba3a1a778c6daf74be40f228a4607ec2bdbb774..daeb9f1741d0e08aa6de2be1274dd8ac135a60a1 100644 (file)
--- a/src/telemega-v0.1/ao_pins.h
+++ b/src/telemega-v0.1/ao_pins.h
@@ -316,6 +316,7 @@ struct ao_adc {
 #define AO_MPU6000_INT_PORT    (&stm_gpioc)
 #define AO_MPU6000_INT_PIN     13
 #define AO_MPU6000_I2C_INDEX   STM_I2C_INDEX(1)
+#define HAS_IMU                        1
 
 /*
  * mma655x

diff --git a/src/telemega-v1.0/ao_pins.h b/src/telemega-v1.0/ao_pins.h
index 7ff676ea624ef143997c44b2ddf4602365d99964..95644dae03a1611b9e00197a980fa48e2af82f65 100644 (file)
--- a/src/telemega-v1.0/ao_pins.h
+++ b/src/telemega-v1.0/ao_pins.h
@@ -318,8 +318,7 @@ struct ao_adc {
 #define AO_MPU6000_SPI_BUS     AO_SPI_1_PE13_PE14_PE15
 #define AO_MPU6000_SPI_CS_PORT (&stm_gpiod)
 #define AO_MPU6000_SPI_CS_PIN  2
-
-#define HAS_HIGHG_ACCEL                1
+#define HAS_IMU                        1
 
 /*
  * mma655x