rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Dominic Clifton 2015-09-12 00:07:12 +01:00
parent 6c231e189b
commit c6f5b98a79
24 changed files with 196 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -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) \

4
src/main/config/config.c
View File

@ -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();

18
src/main/drivers/accgyro_adxl345.c
View File

@ -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;
}

10
src/main/drivers/accgyro_bma280.c
View File

@ -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><crap><new_data_bit> | 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;
}

12
src/main/drivers/accgyro_l3g4200d.c
View File

@ -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;
}

6
src/main/drivers/accgyro_l3gd20.c
View File

@ -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

11
src/main/drivers/accgyro_lsm303dlhc.c
View File

@ -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)

11
src/main/drivers/accgyro_mma845x.c
View File

@ -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;
}

22
src/main/drivers/accgyro_mpu3050.c
View File

@ -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;
}

48
src/main/drivers/accgyro_mpu6050.c
View File

@ -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;
}

12
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -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;
}

4
src/main/drivers/accgyro_spi_mpu6000.h
View File

@ -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);

12
src/main/drivers/accgyro_spi_mpu6500.c
View File

@ -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;
}

13
src/main/drivers/compass_ak8975.c
View File

@ -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;
}

2
src/main/drivers/compass_ak8975.h
View File

@ -19,4 +19,4 @@
bool ak8975detect(mag_t *mag);
void ak8975Init(void);
void ak8975Read(int16_t *magData);
bool ak8975Read(int16_t *magData);

9
src/main/drivers/compass_hmc5883l.c
View File

@ -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;
}

2
src/main/drivers/compass_hmc5883l.h
View File

@ -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);

2
src/main/drivers/sensor.h
View File

@ -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

56
src/main/drivers/system.c
View File

@ -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
}

11
src/main/drivers/system.h
View File

@ -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

2
src/main/main.c
View File

@ -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;

4
src/main/sensors/acceleration.c
View File

@ -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()) {

6
src/main/sensors/gyro.c
View File

@ -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()) {

9
src/main/sensors/initialisation.c
View File

@ -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)