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

LPF setting is not needed to detect the gyro sensor, only when it's

Browse source 
initialised; now the lpf setting is passed to gyroInit().

This saves a bit of code size and ram as well as making the code
cleaner.
This commit is contained in:
Dominic Clifton 2015-09-21 14:16:21 +01:00
parent 06ceac0614
commit aac13914f9
20 changed files with 83 additions and 85 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/drivers/accgyro.h
View file

@ -20,7 +20,7 @@
extern uint16_t acc_1G; // FIXME move into acc_t extern uint16_t acc_1G; // FIXME move into acc_t
typedef struct gyro_s { typedef struct gyro_s {
sensorInitFuncPtr init; // initialize function sensorGyroInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available sensorReadFuncPtr temperature; // read temperature if available
float scale; // scalefactor float scale; // scalefactor

23
src/main/drivers/accgyro_l3g4200d.c
View file

@ -54,12 +54,10 @@
#define L3G4200D_DLPF_78HZ 0x80 #define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0 #define L3G4200D_DLPF_93HZ 0xC0
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ; static void l3g4200dInit(uint16_t lpf);
static void l3g4200dInit(void);
static bool l3g4200dRead(int16_t *gyroADC); static bool l3g4200dRead(int16_t *gyroADC);
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf) bool l3g4200dDetect(gyro_t *gyro)
{ {
uint8_t deviceid; uint8_t deviceid;
@ -75,7 +73,15 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
// 14.2857dps/lsb scalefactor // 14.2857dps/lsb scalefactor
gyro->scale = 1.0f / 14.2857f; gyro->scale = 1.0f / 14.2857f;
// default LPF is set to 32Hz return true;
}
static void l3g4200dInit(uint16_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
switch (lpf) { switch (lpf) {
default: default:
case 32: case 32:
@ -92,13 +98,6 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
break; break;
} }
return true;
}
static void l3g4200dInit(void)
{
bool ack;
delay(100); delay(100);
ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS); ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);

2
src/main/drivers/accgyro_l3g4200d.h
View file

@ -17,4 +17,4 @@
#pragma once #pragma once
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf); bool l3g4200dDetect(gyro_t *gyro);

8
src/main/drivers/accgyro_l3gd20.c
View file

@ -86,8 +86,10 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER); spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER);
} }
void l3gd20GyroInit(void) void l3gd20GyroInit(uint16_t lpf)
{ {
UNUSED(lpf); // FIXME use it!
l3gd20SpiInit(L3GD20_SPI); l3gd20SpiInit(L3GD20_SPI);
GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN); GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
@ -150,10 +152,8 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
#define L3GD20_GYRO_SCALE_FACTOR 0.07f #define L3GD20_GYRO_SCALE_FACTOR 0.07f
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf) bool l3gd20Detect(gyro_t *gyro)
{ {
UNUSED(lpf);
gyro->init = l3gd20GyroInit; gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead; gyro->read = l3gd20GyroRead;

2
src/main/drivers/accgyro_l3gd20.h
View file

@ -17,4 +17,4 @@
#pragma once #pragma once
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf); bool l3gd20Detect(gyro_t *gyro);

10
src/main/drivers/accgyro_mpu.c
View file

@ -59,10 +59,6 @@ mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration; mpuConfiguration_t mpuConfiguration;
static const extiConfig_t *mpuIntExtiConfig = NULL; static const extiConfig_t *mpuIntExtiConfig = NULL;
// FIXME move into mpuConfiguration
uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
#define MPU_ADDRESS 0x68 #define MPU_ADDRESS 0x68
// MPU6050 // MPU6050
@ -299,8 +295,10 @@ void mpuIntExtiInit(void)
mpuExtiInitDone = true; mpuExtiInitDone = true;
} }
void configureMPULPF(uint16_t lpf) uint8_t determineMPULPF(uint16_t lpf)
{ {
uint8_t mpuLowPassFilter;
if (lpf == 256) if (lpf == 256)
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
else if (lpf >= 188) else if (lpf >= 188)
@ -317,6 +315,8 @@ void configureMPULPF(uint16_t lpf)
mpuLowPassFilter = INV_FILTER_5HZ; mpuLowPassFilter = INV_FILTER_5HZ;
else else
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
return mpuLowPassFilter;
} }
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)

4
src/main/drivers/accgyro_mpu.h
View file

@ -78,7 +78,9 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution; mpu6050Resolution_e resolution;
} mpuDetectionResult_t; } mpuDetectionResult_t;
void configureMPULPF(uint16_t lpf); extern mpuDetectionResult_t mpuDetectionResult;
uint8_t determineMPULPF(uint16_t lpf);
void configureMPUDataReadyInterruptHandling(void); void configureMPUDataReadyInterruptHandling(void);
void mpuIntExtiInit(void); void mpuIntExtiInit(void);
bool mpuAccRead(int16_t *accData); bool mpuAccRead(int16_t *accData);

13
src/main/drivers/accgyro_mpu3050.c
View file

@ -31,9 +31,6 @@
#include "accgyro_mpu.h" #include "accgyro_mpu.h"
#include "accgyro_mpu3050.h" #include "accgyro_mpu3050.h"
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
// MPU3050, Standard address 0x68 // MPU3050, Standard address 0x68
#define MPU3050_ADDRESS 0x68 #define MPU3050_ADDRESS 0x68
@ -49,10 +46,10 @@ extern uint8_t mpuLowPassFilter;
#define MPU3050_USER_RESET 0x01 #define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01 #define MPU3050_CLK_SEL_PLL_GX 0x01
void mpu3050Init(void); static void mpu3050Init(uint16_t lpf);
static bool mpu3050ReadTemp(int16_t *tempData); static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf) bool mpu3050Detect(gyro_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_3050) { if (mpuDetectionResult.sensor != MPU_3050) {
return false; return false;
@ -64,15 +61,15 @@ bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true; return true;
} }
void mpu3050Init(void) static void mpu3050Init(uint16_t lpf)
{ {
bool ack; bool ack;
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0); ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);

3
src/main/drivers/accgyro_mpu3050.h
View file

@ -26,5 +26,4 @@
#define MPU3050_USER_CTRL 0x3D #define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E #define MPU3050_PWR_MGM 0x3E
void mpu3050Init(void); bool mpu3050Detect(gyro_t *gyro);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf);

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

@ -37,7 +37,6 @@
#include "accgyro_mpu.h" #include "accgyro_mpu.h"
#include "accgyro_mpu6050.h" #include "accgyro_mpu6050.h"
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter; extern uint8_t mpuLowPassFilter;
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
@ -146,7 +145,7 @@ extern uint8_t mpuLowPassFilter;
#define MPU6050_SMPLRT_DIV 0 // 8000Hz #define MPU6050_SMPLRT_DIV 0 // 8000Hz
static void mpu6050AccInit(void); static void mpu6050AccInit(void);
static void mpu6050GyroInit(void); static void mpu6050GyroInit(uint16_t lpf);
bool mpu6050AccDetect(acc_t *acc) bool mpu6050AccDetect(acc_t *acc)
{ {
@ -161,7 +160,7 @@ bool mpu6050AccDetect(acc_t *acc)
return true; return true;
} }
bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf) bool mpu6050GyroDetect(gyro_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_60x0) { if (mpuDetectionResult.sensor != MPU_60x0) {
return false; return false;
@ -172,8 +171,6 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true; return true;
} }
@ -191,11 +188,14 @@ static void mpu6050AccInit(void)
} }
} }
static void mpu6050GyroInit(void) static void mpu6050GyroInit(uint16_t lpf)
{ {
bool ack;
mpuIntExtiInit(); mpuIntExtiInit();
bool ack; uint8_t mpuLowPassFilter = determineMPULPF(lpf);
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100); delay(100);
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)

2
src/main/drivers/accgyro_mpu6050.h
View file

@ -18,4 +18,4 @@
#pragma once #pragma once
bool mpu6050AccDetect(acc_t *acc); bool mpu6050AccDetect(acc_t *acc);
bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf); bool mpu6050GyroDetect(gyro_t *gyro);

13
src/main/drivers/accgyro_mpu6500.c
View file

@ -33,13 +33,8 @@
#include "accgyro_mpu.h" #include "accgyro_mpu.h"
#include "accgyro_mpu6500.h" #include "accgyro_mpu6500.h"
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
extern uint16_t acc_1G; extern uint16_t acc_1G;
bool mpu6500AccDetect(acc_t *acc) bool mpu6500AccDetect(acc_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_I2C) { if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
@ -52,7 +47,7 @@ bool mpu6500AccDetect(acc_t *acc)
return true; return true;
} }
bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf) bool mpu6500GyroDetect(gyro_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_I2C) { if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false; return false;
@ -64,8 +59,6 @@ bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true; return true;
} }
@ -74,7 +67,7 @@ void mpu6500AccInit(void)
acc_1G = 512 * 8; acc_1G = 512 * 8;
} }
void mpu6500GyroInit(void) void mpu6500GyroInit(uint16_t lpf)
{ {
#ifdef NAZE #ifdef NAZE
// FIXME target specific code in driver code. // FIXME target specific code in driver code.
@ -89,6 +82,8 @@ void mpu6500GyroInit(void)
} }
#endif #endif
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET); mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100); delay(100);
mpuConfiguration.write(MPU6500_RA_SIGNAL_PATH_RST, 0x07); mpuConfiguration.write(MPU6500_RA_SIGNAL_PATH_RST, 0x07);

4
src/main/drivers/accgyro_mpu6500.h
View file

@ -36,7 +36,7 @@
#pragma once #pragma once
bool mpu6500AccDetect(acc_t *acc); bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf); bool mpu6500GyroDetect(gyro_t *gyro);
void mpu6500AccInit(void); void mpu6500AccInit(void);
void mpu6500GyroInit(void); void mpu6500GyroInit(uint16_t lpf);

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

@ -43,7 +43,6 @@
static void mpu6000AccAndGyroInit(void); static void mpu6000AccAndGyroInit(void);
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter; extern uint8_t mpuLowPassFilter;
static bool mpuSpi6000InitDone = false; static bool mpuSpi6000InitDone = false;
@ -121,8 +120,10 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true; return true;
} }
void mpu6000SpiGyroInit(void) void mpu6000SpiGyroInit(uint16_t lpf)
{ {
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpu6000AccAndGyroInit(); mpu6000AccAndGyroInit();
spiResetErrorCounter(MPU6000_SPI_INSTANCE); spiResetErrorCounter(MPU6000_SPI_INSTANCE);
@ -249,7 +250,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
return true; return true;
} }
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) bool mpu6000SpiGyroDetect(gyro_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) { if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false; return false;
@ -261,7 +262,5 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true; return true;
} }

2
src/main/drivers/accgyro_spi_mpu6000.h
View file

@ -47,7 +47,7 @@
bool mpu6000SpiDetect(void); bool mpu6000SpiDetect(void);
bool mpu6000SpiAccDetect(acc_t *acc); bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf); bool mpu6000SpiGyroDetect(gyro_t *gyro);
bool mpu6000WriteRegister(uint8_t reg, uint8_t data); bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

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

@ -38,9 +38,6 @@
#define DISABLE_MPU6500 GPIO_SetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN) #define DISABLE_MPU6500 GPIO_SetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
#define ENABLE_MPU6500 GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN) #define ENABLE_MPU6500 GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
extern uint16_t acc_1G; extern uint16_t acc_1G;
bool mpu6500WriteRegister(uint8_t reg, uint8_t data) bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
@ -128,7 +125,7 @@ bool mpu6500SpiAccDetect(acc_t *acc)
return true; return true;
} }
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf) bool mpu6500SpiGyroDetect(gyro_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) { if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false; return false;
@ -140,8 +137,6 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true; return true;
} }

2
src/main/drivers/accgyro_spi_mpu6500.h
View file

@ -38,7 +38,7 @@
bool mpu6500SpiDetect(void); bool mpu6500SpiDetect(void);
bool mpu6500SpiAccDetect(acc_t *acc); bool mpu6500SpiAccDetect(acc_t *acc);
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf); bool mpu6500SpiGyroDetect(gyro_t *gyro);
bool mpu6500WriteRegister(uint8_t reg, uint8_t data); bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

1
src/main/drivers/sensor.h
View file

@ -19,3 +19,4 @@
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorGyroInitFuncPtr)(uint16_t lpf); // gyro sensor init prototype

7
src/main/flight/mixer.h
View file

@ -194,16 +194,19 @@ void filterServos(void);
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct escAndServoConfig_s;
struct rxConfig_s;
void mixerUseConfigs( void mixerUseConfigs(
#ifdef USE_SERVOS #ifdef USE_SERVOS
servoParam_t *servoConfToUse, servoParam_t *servoConfToUse,
struct gimbalConfig_s *gimbalConfigToUse, struct gimbalConfig_s *gimbalConfigToUse,
#endif #endif
flight3DConfig_t *flight3DConfigToUse, flight3DConfig_t *flight3DConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse, struct escAndServoConfig_s *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse, mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse, airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse); struct rxConfig_s *rxConfigToUse);
void writeAllMotors(int16_t mc); void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerLoadMix(int index, motorMixer_t *customMixers);

40
src/main/sensors/initialisation.c
View file

@ -128,19 +128,25 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
} }
#ifdef USE_FAKE_GYRO #ifdef USE_FAKE_GYRO
static void fakeGyroInit(void) {} static void fakeGyroInit(uint16_t lpf)
static bool fakeGyroRead(int16_t *gyroADC) { {
UNUSED(lpf);
}
static bool fakeGyroRead(int16_t *gyroADC)
{
memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT])); memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
return true; return true;
} }
static bool fakeGyroReadTemp(int16_t *tempData) {
static bool fakeGyroReadTemp(int16_t *tempData)
{
UNUSED(tempData); UNUSED(tempData);
return true; return true;
} }
bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) bool fakeGyroDetect(gyro_t *gyro)
{ {
UNUSED(lpf);
gyro->init = fakeGyroInit; gyro->init = fakeGyroInit;
gyro->read = fakeGyroRead; gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp; gyro->temperature = fakeGyroReadTemp;
@ -164,7 +170,7 @@ bool fakeAccDetect(acc_t *acc)
} }
#endif #endif
bool detectGyro(uint16_t gyroLpf) bool detectGyro(void)
{ {
gyroSensor_e gyroHardware = GYRO_DEFAULT; gyroSensor_e gyroHardware = GYRO_DEFAULT;
@ -175,7 +181,7 @@ bool detectGyro(uint16_t gyroLpf)
; // fallthrough ; // fallthrough
case GYRO_MPU6050: case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050 #ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(&gyro, gyroLpf)) { if (mpu6050GyroDetect(&gyro)) {
#ifdef GYRO_MPU6050_ALIGN #ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050; gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN; gyroAlign = GYRO_MPU6050_ALIGN;
@ -186,7 +192,7 @@ bool detectGyro(uint16_t gyroLpf)
; // fallthrough ; // fallthrough
case GYRO_L3G4200D: case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D #ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) { if (l3g4200dDetect(&gyro)) {
#ifdef GYRO_L3G4200D_ALIGN #ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D; gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN; gyroAlign = GYRO_L3G4200D_ALIGN;
@ -198,7 +204,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_MPU3050: case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050 #ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) { if (mpu3050Detect(&gyro)) {
#ifdef GYRO_MPU3050_ALIGN #ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050; gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN; gyroAlign = GYRO_MPU3050_ALIGN;
@ -210,7 +216,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_L3GD20: case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20 #ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) { if (l3gd20Detect(&gyro)) {
#ifdef GYRO_L3GD20_ALIGN #ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20; gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN; gyroAlign = GYRO_L3GD20_ALIGN;
@ -222,7 +228,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_MPU6000: case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000 #ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) { if (mpu6000SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6000_ALIGN #ifdef GYRO_MPU6000_ALIGN
gyroHardware = GYRO_MPU6000; gyroHardware = GYRO_MPU6000;
gyroAlign = GYRO_MPU6000_ALIGN; gyroAlign = GYRO_MPU6000_ALIGN;
@ -237,7 +243,7 @@ bool detectGyro(uint16_t gyroLpf)
#ifdef USE_HARDWARE_REVISION_DETECTION #ifdef USE_HARDWARE_REVISION_DETECTION
spiBusInit(); spiBusInit();
#endif #endif
if (mpu6500GyroDetect(&gyro, gyroLpf) || mpu6500SpiGyroDetect(&gyro, gyroLpf)) { if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6500_ALIGN #ifdef GYRO_MPU6500_ALIGN
gyroHardware = GYRO_MPU6500; gyroHardware = GYRO_MPU6500;
gyroAlign = GYRO_MPU6500_ALIGN; gyroAlign = GYRO_MPU6500_ALIGN;
@ -249,7 +255,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_FAKE: case GYRO_FAKE:
#ifdef USE_FAKE_GYRO #ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) { if (fakeGyroDetect(&gyro)) {
gyroHardware = GYRO_FAKE; gyroHardware = GYRO_FAKE;
break; break;
} }
@ -403,7 +409,9 @@ retry:
static void detectBaro(baroSensor_e baroHardwareToUse) static void detectBaro(baroSensor_e baroHardwareToUse)
{ {
#ifdef BARO #ifndef BARO
UNUSED(baroHardwareToUse);
#else
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = baroHardwareToUse; baroSensor_e baroHardware = baroHardwareToUse;
@ -603,7 +611,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
UNUSED(mpuDetectionResult); UNUSED(mpuDetectionResult);
#endif #endif
if (!detectGyro(gyroLpf)) { if (!detectGyro()) {
return false; return false;
} }
detectAcc(accHardwareToUse); detectAcc(accHardwareToUse);
@ -614,7 +622,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
if (sensors(SENSOR_ACC)) if (sensors(SENSOR_ACC))
acc.init(); acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(); gyro.init(gyroLpf);
detectMag(magHardwareToUse); detectMag(magHardwareToUse);