diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 8af8f1c762..fe7560e136 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -27,6 +27,7 @@ #include "drivers/bus.h" #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_mpu.h" +#include "sensors/gyro.h" #pragma GCC diagnostic push #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) #include @@ -81,7 +82,7 @@ typedef struct gyroDev_s { uint8_t mpuDividerDrops; ioTag_t mpuIntExtiTag; uint8_t gyroHasOverflowProtection; - uint8_t filler[1]; + gyroSensor_e gyroHardware; } gyroDev_t; typedef struct accDev_s { diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 2b042adba6..234c8268fc 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -2374,11 +2374,27 @@ static void cliGpsPassthrough(char *cmdline) #endif #if defined(USE_GYRO_REGISTER_DUMP) && !defined(SIMULATOR_BUILD) +static void cliPrintGyroRegisters(uint8_t whichSensor) +{ + tfp_printf("# WHO_AM_I 0x%X\r\n", gyroReadRegister(whichSensor, MPU_RA_WHO_AM_I)); + tfp_printf("# CONFIG 0x%X\r\n", gyroReadRegister(whichSensor, MPU_RA_CONFIG)); + tfp_printf("# GYRO_CONFIG 0x%X\r\n", gyroReadRegister(whichSensor, MPU_RA_GYRO_CONFIG)); +} + static void cliDumpGyroRegisters(char *cmdline) { - tfp_printf("# WHO_AM_I 0x%X\r\n", gyroReadRegister(MPU_RA_WHO_AM_I)); - tfp_printf("# CONFIG 0x%X\r\n", gyroReadRegister(MPU_RA_CONFIG)); - tfp_printf("# GYRO_CONFIG 0x%X\r\n", gyroReadRegister(MPU_RA_GYRO_CONFIG)); +#ifdef USE_DUAL_GYRO + if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) { + tfp_printf("\r\n# Gyro 1\r\n"); + cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1); + } + if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) { + tfp_printf("\r\n# Gyro 2\r\n"); + cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_2); + } +#else + cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1); +#endif // USE_DUAL_GYRO UNUSED(cmdline); } #endif diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4af4a0971f..d68cf9e0c3 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -223,6 +223,22 @@ const busDevice_t *gyroSensorBus(void) #endif } +#ifdef USE_GYRO_REGISTER_DUMP +const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor) +{ +#ifdef USE_DUAL_GYRO + if (whichSensor == GYRO_CONFIG_USE_GYRO_2) { + return &gyroSensor2.gyroDev.bus; + } else { + return &gyroSensor1.gyroDev.bus; + } +#else + UNUSED(whichSensor); + return &gyroSensor1.gyroDev.bus; +#endif +} +#endif // USE_GYRO_REGISTER_DUMP + const mpuConfiguration_t *gyroMpuConfiguration(void) { #ifdef USE_DUAL_GYRO @@ -433,6 +449,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) #endif const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); + gyroSensor->gyroDev.gyroHardware = gyroHardware; if (gyroHardware == GYRO_NONE) { return false; } @@ -599,7 +616,21 @@ bool gyroInit(void) } gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection; } -#endif +#endif // USE_DUAL_GYRO + +#ifdef USE_DUAL_GYRO + // 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); + + } + } +#endif // USE_DUAL_GYRO return ret; } @@ -1257,8 +1288,8 @@ uint16_t gyroAbsRateDps(int axis) } #ifdef USE_GYRO_REGISTER_DUMP -uint8_t gyroReadRegister(uint8_t reg) +uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg) { - return mpuGyroReadRegister(gyroSensorBus(), reg); + return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg); } -#endif +#endif // USE_GYRO_REGISTER_DUMP diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 16b3a31ab9..f26d7673d8 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -128,4 +128,4 @@ int16_t gyroRateDps(int axis); bool gyroOverflowDetected(void); bool gyroYawSpinDetected(void); uint16_t gyroAbsRateDps(int axis); -uint8_t gyroReadRegister(uint8_t reg); +uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg); diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index e498b70e33..3efcb7bbb5 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -697,7 +697,10 @@ void initRcProcessing(void) {} void changePidProfile(uint8_t) {} void pidInitConfig(const pidProfile_t *) {} void accSetCalibrationCycles(uint16_t) {} -void gyroStartCalibration(void) {} +void gyroStartCalibration(bool isFirstArmingCalibration) +{ + UNUSED(isFirstArmingCalibration); +} void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} void handleInflightCalibrationStickPosition(void) {} bool feature(uint32_t) { return false;}