From c6f5b98a7916a369bcf9445d04feef4036dc3ce5 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 12 Sep 2015 00:07:12 +0100 Subject: [PATCH] Improve failure LED status flashing. Now users can identify and report hardware failures by counting the number of long flashes. Fix up sensor read API so that code that uses sensors can detect malfunctions. If a failure mode occurs in a debug mode the code reboots the system rather than rebooting to the bootloader. --- Makefile | 2 +- src/main/config/config.c | 4 +- src/main/drivers/accgyro_adxl345.c | 18 +++++++-- src/main/drivers/accgyro_bma280.c | 10 +++-- src/main/drivers/accgyro_l3g4200d.c | 12 ++++-- src/main/drivers/accgyro_l3gd20.c | 6 ++- src/main/drivers/accgyro_lsm303dlhc.c | 11 +++-- src/main/drivers/accgyro_mma845x.c | 11 +++-- src/main/drivers/accgyro_mpu3050.c | 22 ++++++---- src/main/drivers/accgyro_mpu6050.c | 48 +++++++++++++--------- src/main/drivers/accgyro_spi_mpu6000.c | 12 ++++-- src/main/drivers/accgyro_spi_mpu6000.h | 4 +- src/main/drivers/accgyro_spi_mpu6500.c | 12 ++++-- src/main/drivers/compass_ak8975.c | 13 +++--- src/main/drivers/compass_ak8975.h | 2 +- src/main/drivers/compass_hmc5883l.c | 9 ++++- src/main/drivers/compass_hmc5883l.h | 2 +- src/main/drivers/sensor.h | 2 +- src/main/drivers/system.c | 56 +++++++++++++++++++------- src/main/drivers/system.h | 11 +++++ src/main/main.c | 2 +- src/main/sensors/acceleration.c | 4 +- src/main/sensors/gyro.c | 6 +-- src/main/sensors/initialisation.c | 9 +++-- 24 files changed, 196 insertions(+), 92 deletions(-) diff --git a/Makefile b/Makefile index 2ee87a465a..f22bde515a 100755 --- a/Makefile +++ b/Makefile @@ -575,7 +575,7 @@ OPTIMIZE = -Os LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) endif -DEBUG_FLAGS = -ggdb3 +DEBUG_FLAGS = -ggdb3 -DDEBUG CFLAGS = $(ARCH_FLAGS) \ $(LTO_FLAGS) \ diff --git a/src/main/config/config.c b/src/main/config/config.c index 6cb1a75690..6189322f5a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -840,7 +840,7 @@ void readEEPROM(void) { // Sanity check if (!isEEPROMContentValid()) - failureMode(10); + failureMode(FAILURE_INVALID_EEPROM_CONTENTS); suspendRxSignal(); @@ -920,7 +920,7 @@ void writeEEPROM(void) // Flash write failed - just die now if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { - failureMode(10); + failureMode(FAILURE_FLASH_WRITE_FAILED); } resumeRxSignal(); diff --git a/src/main/drivers/accgyro_adxl345.c b/src/main/drivers/accgyro_adxl345.c index f1466accd8..82082e161d 100644 --- a/src/main/drivers/accgyro_adxl345.c +++ b/src/main/drivers/accgyro_adxl345.c @@ -57,7 +57,7 @@ #define ADXL345_FIFO_STREAM 0x80 static void adxl345Init(void); -static void adxl345Read(int16_t *accelData); +static bool adxl345Read(int16_t *accelData); static bool useFifo = false; @@ -96,7 +96,7 @@ static void adxl345Init(void) uint8_t acc_samples = 0; -static void adxl345Read(int16_t *accelData) +static bool adxl345Read(int16_t *accelData) { uint8_t buf[8]; @@ -109,7 +109,11 @@ static void adxl345Read(int16_t *accelData) do { i++; - i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf); + + if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) { + return false;; + } + x += (int16_t)(buf[0] + (buf[1] << 8)); y += (int16_t)(buf[2] + (buf[3] << 8)); z += (int16_t)(buf[4] + (buf[5] << 8)); @@ -120,9 +124,15 @@ static void adxl345Read(int16_t *accelData) accelData[2] = z / i; acc_samples = i; } else { - i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf); + + if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf)) { + return false; + } + accelData[0] = buf[0] + (buf[1] << 8); accelData[1] = buf[2] + (buf[3] << 8); accelData[2] = buf[4] + (buf[5] << 8); } + + return true; } diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c index 536401ea89..193ae09cea 100644 --- a/src/main/drivers/accgyro_bma280.c +++ b/src/main/drivers/accgyro_bma280.c @@ -33,7 +33,7 @@ #define BMA280_PMU_RANGE 0x0F static void bma280Init(void); -static void bma280Read(int16_t *accelData); +static bool bma280Read(int16_t *accelData); bool bma280Detect(acc_t *acc) { @@ -57,14 +57,18 @@ static void bma280Init(void) acc_1G = 512 * 8; } -static void bma280Read(int16_t *accelData) +static bool bma280Read(int16_t *accelData) { uint8_t buf[6]; - i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf); + if (!i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) { + return false; + } // Data format is lsb<5:0> | msb<13:6> accelData[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8)); accelData[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8)); accelData[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8)); + + return true; } diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c index dd083ea735..82bd43e76f 100644 --- a/src/main/drivers/accgyro_l3g4200d.c +++ b/src/main/drivers/accgyro_l3g4200d.c @@ -57,7 +57,7 @@ static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ; static void l3g4200dInit(void); -static void l3g4200dRead(int16_t *gyroADC); +static bool l3g4200dRead(int16_t *gyroADC); bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf) { @@ -103,20 +103,24 @@ static void l3g4200dInit(void) ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS); if (!ack) - failureMode(3); + failureMode(FAILURE_ACC_INIT); delay(5); i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter); } // Read 3 gyro values into user-provided buffer. No overrun checking is done. -static void l3g4200dRead(int16_t *gyroADC) +static bool l3g4200dRead(int16_t *gyroADC) { uint8_t buf[6]; - i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf); + if (!i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) { + return false; + } gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]); gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]); gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]); + + return true; } diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index 2253548d31..fd56d2512e 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -68,8 +68,8 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) { + UNUSED(SPIx); // FIXME GPIO_InitTypeDef GPIO_InitStructure; - SPI_InitTypeDef SPI_InitStructure; RCC_AHBPeriphClockCmd(L3GD20_CS_GPIO_CLK_PERIPHERAL, ENABLE); @@ -120,7 +120,7 @@ void l3gd20GyroInit(void) delay(100); } -static void l3gd20GyroRead(int16_t *gyroADC) +static bool l3gd20GyroRead(int16_t *gyroADC) { uint8_t buf[6]; @@ -143,6 +143,8 @@ static void l3gd20GyroRead(int16_t *gyroADC) debug[1] = (int16_t)((buf[3] << 8) | buf[2]); debug[2] = (int16_t)((buf[5] << 8) | buf[4]); #endif + + return true; } // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 4aaa7188d6..fecf4801a2 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -131,14 +131,15 @@ void lsm303dlhcAccInit(void) } // Read 3 gyro values into user-provided buffer. No overrun checking is done. -static void lsm303dlhcAccRead(int16_t *gyroADC) +static bool lsm303dlhcAccRead(int16_t *gyroADC) { uint8_t buf[6]; - bool ok = i2cRead(LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf); + bool ack = i2cRead(LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf); - if (!ok) - return; + if (!ack) { + return false; + } // the values range from -8192 to +8191 gyroADC[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2; @@ -150,6 +151,8 @@ static void lsm303dlhcAccRead(int16_t *gyroADC) debug[1] = (int16_t)((buf[3] << 8) | buf[2]); debug[2] = (int16_t)((buf[5] << 8) | buf[4]); #endif + + return true; } bool lsm303dlhcAccDetect(acc_t *acc) diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index ab3a01aca7..bf39f41b04 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -78,7 +78,7 @@ static uint8_t device_id; static void mma8452Init(void); -static void mma8452Read(int16_t *accelData); +static bool mma8452Read(int16_t *accelData); bool mma8452Detect(acc_t *acc) { @@ -132,12 +132,17 @@ static void mma8452Init(void) acc_1G = 256; } -static void mma8452Read(int16_t *accelData) +static bool mma8452Read(int16_t *accelData) { uint8_t buf[6]; - i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf); + if (!i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf)) { + return false; + } + accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4; accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4; accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4; + + return true; } diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index ff90dd06c1..055edd9405 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -58,8 +58,8 @@ static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ; static void mpu3050Init(void); -static void mpu3050Read(int16_t *gyroADC); -static void mpu3050ReadTemp(int16_t *tempData); +static bool mpu3050Read(int16_t *gyroADC); +static bool mpu3050ReadTemp(int16_t *tempData); bool mpu3050Detect(gyro_t *gyro, uint16_t lpf) { @@ -112,7 +112,7 @@ static void mpu3050Init(void) ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0); if (!ack) - failureMode(3); + failureMode(FAILURE_ACC_INIT); i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter); i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0); @@ -121,21 +121,29 @@ static void mpu3050Init(void) } // Read 3 gyro values into user-provided buffer. No overrun checking is done. -static void mpu3050Read(int16_t *gyroADC) +static bool mpu3050Read(int16_t *gyroADC) { uint8_t buf[6]; - i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf); + if (!i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf)) { + return false; + } gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]); gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]); gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]); + + return true; } -static void mpu3050ReadTemp(int16_t *tempData) +static bool mpu3050ReadTemp(int16_t *tempData) { uint8_t buf[2]; - i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf); + if (!i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf)) { + return false; + } *tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280; + + return true; } diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index b67da25d2d..8b71ad2cba 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -173,9 +173,9 @@ enum accel_fsr_e { static uint8_t mpuLowPassFilter = INV_FILTER_42HZ; static void mpu6050AccInit(void); -static void mpu6050AccRead(int16_t *accData); +static bool mpu6050AccRead(int16_t *accData); static void mpu6050GyroInit(void); -static void mpu6050GyroRead(int16_t *gyroADC); +static bool mpu6050GyroRead(int16_t *gyroADC); typedef enum { MPU_6050_HALF_RESOLUTION, @@ -336,13 +336,13 @@ bool mpu6050AccDetect(const mpu6050Config_t *configToUse, acc_t *acc) } else if (revision == 2) { mpuAccelTrim = MPU_6050_FULL_RESOLUTION; } else { - failureMode(5); + failureMode(FAILURE_ACC_INCOMPATIBLE); } } else { i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId); revision = productId & 0x0F; if (!revision) { - failureMode(5); + failureMode(FAILURE_ACC_INCOMPATIBLE); } else if (revision == 4) { mpuAccelTrim = MPU_6050_HALF_RESOLUTION; } else { @@ -406,52 +406,62 @@ static void mpu6050AccInit(void) } } -static void mpu6050AccRead(int16_t *accData) +static bool mpu6050AccRead(int16_t *accData) { uint8_t buf[6]; - if (!i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf)) { - return; + bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf); + if (!ack) { + return false; } accData[0] = (int16_t)((buf[0] << 8) | buf[1]); accData[1] = (int16_t)((buf[2] << 8) | buf[3]); accData[2] = (int16_t)((buf[4] << 8) | buf[5]); + + return true; } static void mpu6050GyroInit(void) { mpu6050GpioInit(); - i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 + bool ack; + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); - i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) - i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure - i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) - i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops. // Accel scale 8g (4096 LSB/g) - i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); - i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, - 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS +// ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, +// 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, + 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS #ifdef USE_MPU_DATA_READY_SIGNAL - i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); #endif + UNUSED(ack); } -static void mpu6050GyroRead(int16_t *gyroADC) +static bool mpu6050GyroRead(int16_t *gyroADC) { uint8_t buf[6]; - if (!i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf)) { - return; + bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf); + if (!ack) { + return false; } gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]); gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]); gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]); + + return true; } diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 56406ffaf9..3bee15fbed 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -125,8 +125,8 @@ static bool mpuSpi6000InitDone = false; #define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN) #define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN) -void mpu6000SpiGyroRead(int16_t *gyroADC); -void mpu6000SpiAccRead(int16_t *gyroADC); +bool mpu6000SpiGyroRead(int16_t *gyroADC); +bool mpu6000SpiAccRead(int16_t *gyroADC); static void mpu6000WriteRegister(uint8_t reg, uint8_t data) { @@ -313,7 +313,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) return true; } -void mpu6000SpiGyroRead(int16_t *gyroData) +bool mpu6000SpiGyroRead(int16_t *gyroData) { uint8_t buf[6]; @@ -324,9 +324,11 @@ void mpu6000SpiGyroRead(int16_t *gyroData) gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]); gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]); + + return true; } -void mpu6000SpiAccRead(int16_t *gyroData) +bool mpu6000SpiAccRead(int16_t *gyroData) { uint8_t buf[6]; @@ -337,4 +339,6 @@ void mpu6000SpiAccRead(int16_t *gyroData) gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]); gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]); + + return true; } diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h index d296b04272..26e5ba037d 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro_spi_mpu6000.h @@ -16,5 +16,5 @@ bool mpu6000SpiAccDetect(acc_t *acc); bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf); -void mpu6000SpiGyroRead(int16_t *gyroADC); -void mpu6000SpiAccRead(int16_t *gyroADC); +bool mpu6000SpiGyroRead(int16_t *gyroADC); +bool mpu6000SpiAccRead(int16_t *gyroADC); diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index e83c5d8889..08b14ebbfa 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -72,9 +72,9 @@ enum accel_fsr_e { static uint8_t mpuLowPassFilter = INV_FILTER_42HZ; static void mpu6500AccInit(void); -static void mpu6500AccRead(int16_t *accData); +static bool mpu6500AccRead(int16_t *accData); static void mpu6500GyroInit(void); -static void mpu6500GyroRead(int16_t *gyroADC); +static bool mpu6500GyroRead(int16_t *gyroADC); extern uint16_t acc_1G; @@ -194,7 +194,7 @@ static void mpu6500AccInit(void) acc_1G = 512 * 8; } -static void mpu6500AccRead(int16_t *accData) +static bool mpu6500AccRead(int16_t *accData) { uint8_t buf[6]; @@ -203,6 +203,8 @@ static void mpu6500AccRead(int16_t *accData) accData[X] = (int16_t)((buf[0] << 8) | buf[1]); accData[Y] = (int16_t)((buf[2] << 8) | buf[3]); accData[Z] = (int16_t)((buf[4] << 8) | buf[5]); + + return true; } static void mpu6500GyroInit(void) @@ -229,7 +231,7 @@ static void mpu6500GyroInit(void) mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R } -static void mpu6500GyroRead(int16_t *gyroADC) +static bool mpu6500GyroRead(int16_t *gyroADC) { uint8_t buf[6]; @@ -238,4 +240,6 @@ static void mpu6500GyroRead(int16_t *gyroADC) gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]); gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]); gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]); + + return true; } diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index c0aeca9305..365e24968f 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -111,7 +111,7 @@ void ak8975Init() #define BIT_STATUS2_REG_DATA_ERROR (1 << 2) #define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW (1 << 3) -void ak8975Read(int16_t *magData) +bool ak8975Read(int16_t *magData) { bool ack; UNUSED(ack); @@ -120,7 +120,7 @@ void ak8975Read(int16_t *magData) ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status); if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) { - return; + return false; } #if 1 // USE_I2C_SINGLE_BYTE_READS @@ -129,22 +129,22 @@ void ak8975Read(int16_t *magData) for (uint8_t i = 0; i < 6; i++) { ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH if (!ack) { - break; + break false } } #endif ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status); if (!ack) { - return; + return false; } if (status & BIT_STATUS2_REG_DATA_ERROR) { - return; + return false; } if (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW) { - return; + return false; } magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4; @@ -153,4 +153,5 @@ void ak8975Read(int16_t *magData) ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again + return true; } diff --git a/src/main/drivers/compass_ak8975.h b/src/main/drivers/compass_ak8975.h index d0d78ecdf1..f80e646130 100644 --- a/src/main/drivers/compass_ak8975.h +++ b/src/main/drivers/compass_ak8975.h @@ -19,4 +19,4 @@ bool ak8975detect(mag_t *mag); void ak8975Init(void); -void ak8975Read(int16_t *magData); +bool ak8975Read(int16_t *magData); diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 4bfdfa41e2..a59186d51f 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -305,14 +305,19 @@ void hmc5883lInit(void) hmc5883lConfigureDataReadyInterruptHandling(); } -void hmc5883lRead(int16_t *magData) +bool hmc5883lRead(int16_t *magData) { uint8_t buf[6]; - i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); + bool ack = i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); + if (ack) { + return false; + } // During calibration, magGain is 1.0, so the read returns normal non-calibrated values. // After calibration is done, magGain is set to calculated gain values. magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X]; magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z]; magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y]; + + return true; } diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index a4338849b9..53c4c9f3e5 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -35,4 +35,4 @@ typedef struct hmc5883Config_s { bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse); void hmc5883lInit(void); -void hmc5883lRead(int16_t *magData); +bool hmc5883lRead(int16_t *magData); diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index f494c5713a..e2e30b779e 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -18,4 +18,4 @@ #pragma once typedef void (*sensorInitFuncPtr)(void); // sensor init prototype -typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype +typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index ba4dd6b0a4..0f99b1bec8 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -47,7 +47,7 @@ void registerExti15_10_CallbackHandler(extiCallbackHandler *fn) return; } } - failureMode(15); // EXTI15_10_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required. + failureMode(FAILURE_DEVELOPER); // EXTI15_10_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required. } void unregisterExti15_10_CallbackHandler(extiCallbackHandler *fn) @@ -194,21 +194,49 @@ void delay(uint32_t ms) delayMicroseconds(1000); } -// FIXME replace mode with an enum so usage can be tracked, currently mode is a magic number -void failureMode(uint8_t mode) -{ - uint8_t flashesRemaining = 10; +#define SHORT_FLASH_DURATION 50 +#define CODE_FLASH_DURATION 250 - LED1_ON; - LED0_OFF; - while (flashesRemaining--) { - LED1_TOGGLE; - LED0_TOGGLE; - delay(475 * mode - 2); - BEEP_ON; - delay(25); - BEEP_OFF; +void failureMode(failureMode_e mode) +{ + int codeRepeatsRemaining = 10; + int codeFlashesRemaining; + int shortFlashesRemaining; + + while (codeRepeatsRemaining--) { + LED1_ON; + LED0_OFF; + shortFlashesRemaining = 5; + codeFlashesRemaining = mode + 1; + uint8_t flashDuration = SHORT_FLASH_DURATION; + + while (shortFlashesRemaining || codeFlashesRemaining) { + LED1_TOGGLE; + LED0_TOGGLE; + BEEP_ON; + delay(flashDuration); + + LED1_TOGGLE; + LED0_TOGGLE; + BEEP_OFF; + delay(flashDuration); + + if (shortFlashesRemaining) { + shortFlashesRemaining--; + if (shortFlashesRemaining == 0) { + delay(500); + flashDuration = CODE_FLASH_DURATION; + } + } else { + codeFlashesRemaining--; + } + } + delay(1000); } +#ifdef DEBUG + systemReset(); +#else systemResetToBootloader(); +#endif } diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 33f3108a06..67ea4a3f4a 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -42,3 +42,14 @@ void registerExti15_10_CallbackHandler(extiCallbackHandler *fn); void unregisterExti15_10_CallbackHandler(extiCallbackHandler *fn); extern uint32_t cachedRccCsrValue; + +typedef enum { + FAILURE_DEVELOPER = 0, + FAILURE_MISSING_ACC, + FAILURE_ACC_INIT, + FAILURE_ACC_INCOMPATIBLE, + FAILURE_INVALID_EEPROM_CONTENTS, + FAILURE_FLASH_WRITE_FAILED +} failureMode_e; + +#define FAILURE_MODE_COUNT 4 diff --git a/src/main/main.c b/src/main/main.c index 6f5f04f74c..2626224c91 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -373,7 +373,7 @@ void init(void) if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. - failureMode(3); + failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 152cfdd609..17d58e8667 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -171,7 +171,9 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { - acc.read(accADC); + if (!acc.read(accADC)) { + return; + } alignSensors(accADC, accADC, accAlign); if (!isAccelerationCalibrationComplete()) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4a631a5a38..4f703b676d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -116,10 +116,10 @@ static void applyGyroZero(void) void gyroUpdate(void) { - // FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time. - // range: +/- 8192; +/- 2000 deg/sec - gyro.read(gyroADC); + if (!gyro.read(gyroADC)) { + return; + } alignSensors(gyroADC, gyroADC, gyroAlign); if (!isGyroCalibrationComplete()) { diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 9505bbc1ef..333e39c0ae 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -125,11 +125,13 @@ const mpu6050Config_t *selectMPU6050Config(void) #ifdef USE_FAKE_GYRO static void fakeGyroInit(void) {} -static void fakeGyroRead(int16_t *gyroADC) { +static bool fakeGyroRead(int16_t *gyroADC) { memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); + return true; } -static void fakeGyroReadTemp(int16_t *tempData) { +static bool fakeGyroReadTemp(int16_t *tempData) { UNUSED(tempData); + return true; } bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) @@ -144,8 +146,9 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) #ifdef USE_FAKE_ACC static void fakeAccInit(void) {} -static void fakeAccRead(int16_t *accData) { +static bool fakeAccRead(int16_t *accData) { memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); + return true; } bool fakeAccDetect(acc_t *acc)