Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Fixed gyro detection for multi gyro setups.

Browse source
This commit is contained in:
mikeller 2019-04-03 00:41:06 +13:00
parent dcb34d3761
commit e44f75b4a7
7 changed files with 111 additions and 92 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -222,7 +222,8 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
{
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(config->spiBus));
if (!instance) {
if (!instance || !config->csnTag) {
return false;
}
spiBusSetInstance(&gyro->bus, instance);
@ -248,7 +249,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyro
spiPreinitByTag(config->csnTag);
return false;
return true;
}
#endif
@ -261,13 +262,13 @@ void mpuPreInit(const struct gyroDeviceConfig_s *config)
#endif
}
void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
{
// MPU datasheet specifies 30ms.
delay(35);
if (config->bustype == BUSTYPE_NONE) {
return;
return false;
}
if (config->bustype == BUSTYPE_GYRO_AUTO) {
@ -291,7 +292,7 @@ void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
return;
return true;
}
sig &= MPU_INQUIRY_MASK;
@ -301,14 +302,17 @@ void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
} else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return;
return true;
}
}
#endif
#ifdef USE_SPI_GYRO
gyro->bus.bustype = BUSTYPE_SPI;
detectSPISensorsAndUpdateDetectionResult(gyro, config);
return detectSPISensorsAndUpdateDetectionResult(gyro, config);
#else
return false;
#endif
}

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

@ -218,7 +218,7 @@ void mpuGyroInit(struct gyroDev_s *gyro);
bool mpuGyroRead(struct gyroDev_s *gyro);
bool mpuGyroReadSPI(struct gyroDev_s *gyro);
void mpuPreInit(const struct gyroDeviceConfig_s *config);
void mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config);
bool mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config);
uint8_t mpuGyroDLPF(struct gyroDev_s *gyro);
uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg);

3
src/main/msp/msp.c
View file

@ -1357,13 +1357,12 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, compassConfig()->mag_align);
// API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
sbufWriteU8(dst, getGyroDetectionFlags());
#ifdef USE_MULTI_GYRO
sbufWriteU8(dst, 1); // USE_MULTI_GYRO
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
sbufWriteU8(dst, gyroDeviceConfig(0)->align);
sbufWriteU8(dst, gyroDeviceConfig(1)->align);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1);
sbufWriteU8(dst, gyroDeviceConfig(0)->align);
sbufWriteU8(dst, ALIGN_DEFAULT);

142
src/main/sensors/gyro.c
View file

@ -86,7 +86,7 @@
FAST_RAM_ZERO_INIT gyro_t gyro;
static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode;
static uint8_t gyroToUse = 0;
static FAST_RAM_ZERO_INIT uint8_t gyroToUse;
static FAST_RAM_ZERO_INIT bool overflowDetected;
#ifdef USE_GYRO_OVERFLOW_CHECK
@ -167,6 +167,8 @@ STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor2;
#endif
static gyroDetectionFlags_t gyroDetectionFlags = NO_GYROS_DETECTED;
#ifdef UNIT_TEST
STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1;
STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
@ -393,7 +395,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
}
if (gyroHardware != GYRO_NONE) {
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
}
@ -411,25 +412,31 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
#endif
}
static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
{
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20)
if (!mpuDetect(&gyroSensor->gyroDev, config)) {
return false;
}
#else
UNUSED(config);
#endif
const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
gyroSensor->gyroDev.gyroHardware = gyroHardware;
return gyroHardware != GYRO_NONE;
}
static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
{
gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
gyroSensor->gyroDev.gyroAlign = config->align;
gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20)
mpuDetect(&gyroSensor->gyroDev, config);
#endif
const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
gyroSensor->gyroDev.gyroHardware = gyroHardware;
if (gyroHardware == GYRO_NONE) {
return false;
}
// Must set gyro targetLooptime before gyroDev.init and initialisation of filters
gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom);
gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
@ -437,7 +444,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
// As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug
// Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative.
switch (gyroHardware) {
switch (gyroSensor->gyroDev.gyroHardware) {
case GYRO_NONE: // Won't ever actually get here, but included to account for all gyro types
case GYRO_DEFAULT:
case GYRO_FAKE:
@ -471,8 +478,6 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
#endif
return true;
}
void gyroPreInit(void)
@ -516,64 +521,65 @@ bool gyroInit(void)
}
firstArmingCalibrationWasStarted = false;
enum {
NO_GYROS_DETECTED = 0,
DETECTED_GYRO_1 = (1 << 0),
DETECTED_GYRO_2 = (1 << 1),
};
uint8_t detectionFlags = NO_GYROS_DETECTED;
gyroDetectionFlags = NO_GYROS_DETECTED;
gyroToUse = gyroConfig()->gyro_to_use;
if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
if (gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0))) {
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
detectionFlags |= DETECTED_GYRO_1;
}
if (gyroDetectSensor(&gyroSensor1, gyroDeviceConfig(0))) {
gyroDetectionFlags |= DETECTED_GYRO_1;
}
#ifdef USE_MULTI_GYRO
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
if (gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1))) {
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
detectionFlags |= DETECTED_GYRO_2;
}
}
if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && detectionFlags != (DETECTED_GYRO_1 | DETECTED_GYRO_2)) {
// at least one gyro is missing.
if (detectionFlags & DETECTED_GYRO_1) {
gyroToUse = GYRO_CONFIG_USE_GYRO_1;
gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
if (detectionFlags & DETECTED_GYRO_2) {
gyroToUse = GYRO_CONFIG_USE_GYRO_2;
gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_2;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
}
// Only allow using both gyros simultaneously if they are the same hardware type.
// If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
if (gyroSensor1.gyroDev.gyroHardware != gyroSensor2.gyroDev.gyroHardware) {
gyroToUse = GYRO_CONFIG_USE_GYRO_1;
gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
sensorsSet(SENSOR_GYRO);
}
#if defined(USE_MULTI_GYRO)
if (gyroDetectSensor(&gyroSensor2, gyroDeviceConfig(1))) {
gyroDetectionFlags |= DETECTED_GYRO_2;
}
#endif
return detectionFlags != NO_GYROS_DETECTED;
if (gyroDetectionFlags == NO_GYROS_DETECTED) {
return false;
}
#if defined(USE_MULTI_GYRO)
if ((gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !(gyroDetectionFlags & DETECTED_BOTH_GYROS))
|| (gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & DETECTED_GYRO_1))
|| (gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & DETECTED_GYRO_2))) {
if (gyroDetectionFlags & DETECTED_GYRO_1) {
gyroToUse = GYRO_CONFIG_USE_GYRO_1;
} else {
gyroToUse = GYRO_CONFIG_USE_GYRO_2;
}
gyroConfigMutable()->gyro_to_use = gyroToUse;
}
// Only allow using both gyros simultaneously if they are the same hardware type.
if ((gyroDetectionFlags & DETECTED_BOTH_GYROS) && gyroSensor1.gyroDev.gyroHardware == gyroSensor2.gyroDev.gyroHardware) {
gyroDetectionFlags |= DETECTED_DUAL_GYROS;
} else if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
// If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
gyroToUse = GYRO_CONFIG_USE_GYRO_1;
gyroConfigMutable()->gyro_to_use = gyroToUse;
}
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1));
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware;
}
#endif
if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0));
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
}
return true;
}
gyroDetectionFlags_t getGyroDetectionFlags(void)
{
return gyroDetectionFlags;
}
#ifdef USE_DYN_LPF

21
src/main/sensors/gyro.h
View file

@ -36,18 +36,18 @@ typedef struct gyro_s {
extern gyro_t gyro;
typedef enum {
enum {
GYRO_OVERFLOW_CHECK_NONE = 0,
GYRO_OVERFLOW_CHECK_YAW,
GYRO_OVERFLOW_CHECK_ALL_AXES
} gyroOverflowCheck_e;
};
enum {
DYN_NOTCH_RANGE_HIGH = 0,
DYN_NOTCH_RANGE_MEDIUM,
DYN_NOTCH_RANGE_LOW,
DYN_NOTCH_RANGE_AUTO
} ;
};
#define DYN_NOTCH_RANGE_HZ_HIGH 2000
#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
@ -63,10 +63,20 @@ enum {
#define GYRO_CONFIG_USE_GYRO_2 1
#define GYRO_CONFIG_USE_GYRO_BOTH 2
typedef enum {
enum {
FILTER_LOWPASS = 0,
FILTER_LOWPASS2
} filterSlots;
};
typedef enum gyroDetectionFlags_e {
NO_GYROS_DETECTED = 0,
DETECTED_GYRO_1 = (1 << 0),
#if defined(USE_MULTI_GYRO)
DETECTED_GYRO_2 = (1 << 1),
DETECTED_BOTH_GYROS = (DETECTED_GYRO_1 | DETECTED_GYRO_2),
DETECTED_DUAL_GYROS = (1 << 7), // All gyros are of the same hardware type
#endif
} gyroDetectionFlags_t;
typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
@ -125,6 +135,7 @@ bool gyroOverflowDetected(void);
bool gyroYawSpinDetected(void);
uint16_t gyroAbsRateDps(int axis);
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
gyroDetectionFlags_t getGyroDetectionFlags(void);
#ifdef USE_DYN_LPF
float dynThrottle(float throttle);
void dynLpfGyroUpdate(float throttle);

16
src/main/target/common_defaults_post.h
View file

@ -293,14 +293,6 @@
#endif
#endif
// F4 and F7 single gyro boards
#if defined(USE_MULTI_GYRO) && !defined(GYRO_2_SPI_INSTANCE)
#define GYRO_2_SPI_INSTANCE GYRO_1_SPI_INSTANCE
#define GYRO_2_CS_PIN NONE
#define GYRO_2_ALIGN ALIGN_DEFAULT
#define GYRO_2_EXTI_PIN NONE
#endif
#if !defined(GYRO_1_SPI_INSTANCE)
#define GYRO_1_SPI_INSTANCE NULL
#endif
@ -317,6 +309,14 @@
#define GYRO_1_ALIGN ALIGN_DEFAULT
#endif
// F4 and F7 single gyro boards
#if defined(USE_MULTI_GYRO) && !defined(GYRO_2_SPI_INSTANCE)
#define GYRO_2_SPI_INSTANCE GYRO_1_SPI_INSTANCE
#define GYRO_2_CS_PIN NONE
#define GYRO_2_ALIGN ALIGN_DEFAULT
#define GYRO_2_EXTI_PIN NONE
#endif
#if defined(MPU_ADDRESS)
#define GYRO_I2C_ADDRESS MPU_ADDRESS
#else

1
src/test/unit/sensor_gyro_unittest.cc
View file

@ -59,7 +59,6 @@ TEST(SensorGyro, Detect)
{
const gyroHardware_e detected = gyroDetect(gyroDevPtr);
EXPECT_EQ(GYRO_FAKE, detected);
EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
}
TEST(SensorGyro, Init)