From ff59c37fdb878c7896d4601df4c451cb85b553df Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 3 May 2018 10:18:17 -0400 Subject: [PATCH 1/2] Only allow gyro_to_use = BOTH if both detected gyros are the same type Since we currently don't have per gyro configuration, trying to use two different gyro types simultaneously may not provide good results or lead to other unforeseen situations like attempting to initialize with settings not applicable to both hardware types. This logic looks at the detected gyro types and resets to use only the first one if the user selected "BOTH" and they're different hardware types. --- src/main/drivers/accgyro/accgyro.h | 3 ++- src/main/sensors/gyro.c | 17 ++++++++++++++++- src/test/unit/rc_controls_unittest.cc | 5 ++++- 3 files changed, 22 insertions(+), 3 deletions(-) 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/sensors/gyro.c b/src/main/sensors/gyro.c index 8c903c721a..38eb04f356 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -437,6 +437,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; } @@ -603,7 +604,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; } 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;} From 34bdfc00e75044abc08b1786c1101554c5891f43 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Fri, 4 May 2018 11:46:05 -0400 Subject: [PATCH 2/2] Update gyroregisters cli command to display both gyros If gyro_to_use = BOTH, then display the registers for both active gyros. --- src/main/interface/cli.c | 22 +++++++++++++++++++--- src/main/sensors/gyro.c | 22 +++++++++++++++++++--- src/main/sensors/gyro.h | 2 +- 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index b2728bac89..142d63540f 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -2373,11 +2373,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 38eb04f356..70c9dc07e8 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -227,6 +227,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 @@ -1276,8 +1292,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);