From ee4279613b4757453d0d8f8afc06037c61eeb520 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 18 Dec 2013 20:32:05 -0800 Subject: [PATCH] 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 --- src/core/ao_flight.c | 8 +++ src/core/ao_flight.h | 4 ++ src/drivers/ao_mpu6000.c | 104 +++++++++++++++++++----------------- src/telemega-v0.1/ao_pins.h | 1 + src/telemega-v1.0/ao_pins.h | 3 +- 5 files changed, 70 insertions(+), 50 deletions(-) diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 4a53bdc6..463ff4a2 100644 --- 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 ac3e9cfb..c7c02ccf 100644 --- 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 5e75b78a..f8ce7346 100644 --- 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 7ba3a1a7..daeb9f17 100644 --- 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 7ff676ea..95644dae 100644 --- 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 -- 2.30.2