diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index b2728bac8..142d63540 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 38eb04f35..70c9dc07e 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 16b3a31ab..f26d7673d 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);