Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Merge branch 'master' into betaflight

Browse source 
Conflicts:
	src/main/drivers/accgyro_mpu3050.c
	src/main/drivers/accgyro_mpu6050.c
	src/main/drivers/accgyro_spi_mpu6000.c
	src/main/drivers/accgyro_spi_mpu6500.c
	src/main/drivers/sensor.h
This commit is contained in:
borisbstyle 2015-09-15 23:36:40 +02:00
commit dae052632c
24 changed files with 201 additions and 92 deletions
Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -597,7 +597,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

@ -843,7 +843,7 @@ void readEEPROM(void)
{
// Sanity check
if (!isEEPROMContentValid())
failureMode(10);
failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
suspendRxSignal();
@ -923,7 +923,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

@ -60,8 +60,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);
static void checkMPU3050Interrupt(bool *gyroIsUpdated);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
@ -116,7 +116,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);
@ -125,23 +125,31 @@ 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;
}
void checkMPU3050Interrupt(bool *gyroIsUpdated) {

51
src/main/drivers/accgyro_mpu6050.c
View file

@ -1,4 +1,4 @@
/*
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
@ -162,9 +162,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);
static void checkMPU6050Interrupt(bool *gyroIsUpdated);
typedef enum {
@ -326,13 +326,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 {
@ -397,55 +397,70 @@ 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);
<<<<<<< HEAD
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_SMPLRT_DIV, gyroMPU6xxxGetDivider()); //SMPLRT_DIV -- SMPLRT_DIV = 7 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
=======
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)
>>>>>>> master
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;
}
void checkMPU6050Interrupt(bool *gyroIsUpdated) {

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

@ -127,8 +127,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);
void checkMPU6000Interrupt(bool *gyroIsUpdated);
static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
@ -321,7 +321,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];
@ -332,9 +332,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];
@ -345,6 +347,8 @@ 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;
}
void checkMPU6000Interrupt(bool *gyroIsUpdated) {

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

14
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -73,10 +73,10 @@ 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 void checkMPU6500Interrupt(bool *gyroIsUpdated);
static bool mpu6500GyroRead(int16_t *gyroADC);
static bool checkMPU6500Interrupt(bool *gyroIsUpdated);
extern uint16_t acc_1G;
@ -197,7 +197,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];
@ -206,6 +206,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)
@ -232,7 +234,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];
@ -241,6 +243,8 @@ 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;
}
void checkMPU6500Interrupt(bool *gyroIsUpdated) {

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,5 +18,5 @@
#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
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor read and align prototype

44
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)
@ -193,21 +193,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
void failureMode(failureMode_e mode)
{
int codeRepeatsRemaining = 10;
int codeFlashesRemaining;
int shortFlashesRemaining;
while (codeRepeatsRemaining--) {
LED1_ON;
LED0_OFF;
while (flashesRemaining--) {
shortFlashesRemaining = 5;
codeFlashesRemaining = mode + 1;
uint8_t flashDuration = SHORT_FLASH_DURATION;
while (shortFlashesRemaining || codeFlashesRemaining) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(475 * mode - 2);
BEEP_ON;
delay(25);
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, masterConfig.looptime, masterConfig.syncGyroToLoop)) {
// 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

@ -140,11 +140,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)
@ -159,8 +161,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)