diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 059ae0313..6da30982f 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -26,6 +26,10 @@ #define MPU_I2C_INSTANCE I2C_DEVICE #endif +#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) +#define GYRO_SUPPORTS_32KHZ +#endif + #define GYRO_LPF_256HZ 0 #define GYRO_LPF_188HZ 1 #define GYRO_LPF_98HZ 2 @@ -35,6 +39,12 @@ #define GYRO_LPF_5HZ 6 #define GYRO_LPF_NONE 7 +typedef enum { + GYRO_RATE_1_kHz, + GYRO_RATE_8_kHz, + GYRO_RATE_32_kHz, +} gyroRateKHz_e; + typedef struct gyroDev_s { sensorGyroInitFuncPtr init; // initialize function sensorGyroReadFuncPtr read; // read 3 axis data function @@ -44,7 +54,9 @@ typedef struct gyroDev_s { extiCallbackRec_t exti; float scale; // scalefactor int16_t gyroADCRaw[XYZ_AXIS_COUNT]; - uint16_t lpf; + uint8_t lpf; + gyroRateKHz_e gyroRateKHz; + uint8_t mpuDividerDrops; volatile bool dataReady; sensor_align_e gyroAlign; mpuDetectionResult_t mpuDetectionResult; diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index ccbcc4e58..631f7ec80 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -82,7 +82,7 @@ static void mpu6050GyroInit(gyroDev_t *gyro) gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index 74521e81f..f9a46fbfc 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -63,13 +63,14 @@ void mpu6500GyroInit(gyroDev_t *gyro) delay(100); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); + gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops + gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 69af5cb01..29e22d574 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -138,13 +138,14 @@ void icm20689GyroInit(gyroDev_t *gyro) // delay(100); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); + gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops + gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 44ba01262..20fa4df8d 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -45,7 +45,7 @@ #include "accgyro_spi_mpu6000.h" -static void mpu6000AccAndGyroInit(void); +static void mpu6000AccAndGyroInit(gyroDev_t *gyro); static bool mpuSpi6000InitDone = false; @@ -128,7 +128,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - mpu6000AccAndGyroInit(); + mpu6000AccAndGyroInit(gyro); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); @@ -201,7 +201,7 @@ bool mpu6000SpiDetect(void) return false; } -static void mpu6000AccAndGyroInit(void) +static void mpu6000AccAndGyroInit(gyroDev_t *gyro) { if (mpuSpi6000InitDone) { return; @@ -229,7 +229,7 @@ static void mpu6000AccAndGyroInit(void) // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled - mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); + mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index d7e4d6482..9cd3993cf 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -46,7 +46,7 @@ #include "accgyro_mpu.h" #include "accgyro_spi_mpu9250.h" -static void mpu9250AccAndGyroInit(uint8_t lpf); +static void mpu9250AccAndGyroInit(gyroDev_t *gyro); static bool mpuSpi9250InitDone = false; @@ -100,7 +100,7 @@ void mpu9250SpiGyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - mpu9250AccAndGyroInit(gyro->lpf); + mpu9250AccAndGyroInit(gyro); spiResetErrorCounter(MPU9250_SPI_INSTANCE); @@ -140,7 +140,7 @@ bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) return false; } -static void mpu9250AccAndGyroInit(uint8_t lpf) { +static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { if (mpuSpi9250InitDone) { return; @@ -153,17 +153,19 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) { verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); - verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11 + //Fchoice_b defaults to 00 which makes fchoice 11 + const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); + verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData); - if (lpf == 4) { + if (gyro->lpf == 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF - } else if (lpf < 4) { + } else if (gyro->lpf < 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF - } else if (lpf > 4) { + } else if (gyro->lpf > 4) { verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF } - verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops + verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index c1df19077..c0c7cc507 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -14,7 +14,6 @@ #include "accgyro.h" #include "gyro_sync.h" -static uint8_t mpuDividerDrops; bool gyroSyncCheckUpdate(gyroDev_t *gyro) { @@ -23,24 +22,35 @@ bool gyroSyncCheckUpdate(gyroDev_t *gyro) return gyro->intStatus(gyro); } -uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) +uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz) { +#ifndef GYRO_SUPPORTS_32KHZ + if (gyro_use_32khz) { + gyro_use_32khz = false; + } +#endif int gyroSamplePeriod; if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) { + gyro->gyroRateKHz = GYRO_RATE_8_kHz; gyroSamplePeriod = 125; } else { + gyro->gyroRateKHz = GYRO_RATE_1_kHz; gyroSamplePeriod = 1000; gyroSyncDenominator = 1; // Always full Sampling 1khz } // calculate gyro divider and targetLooptime (expected cycleTime) - mpuDividerDrops = gyroSyncDenominator - 1; - const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod; + gyro->mpuDividerDrops = gyroSyncDenominator - 1; + uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod; + if (gyro_use_32khz) { + gyro->gyroRateKHz = GYRO_RATE_32_kHz; + targetLooptime /= 4; + } return targetLooptime; } -uint8_t gyroMPU6xxxGetDividerDrops(void) +uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro) { - return mpuDividerDrops; + return gyro->mpuDividerDrops; } diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index 1b0787894..7bd7d375e 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -5,8 +5,8 @@ * Author: borisb */ -struct gyroDev_s; +#include "drivers/accgyro.h" -bool gyroSyncCheckUpdate(struct gyroDev_s *gyro); -uint8_t gyroMPU6xxxGetDividerDrops(void); -uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator); +bool gyroSyncCheckUpdate(gyroDev_t *gyro); +uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro); +uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz); diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index ff053dbc0..6b646435f 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -6,9 +6,9 @@ #define NVIC_PRIO_MAX NVIC_BUILD_PRIORITY(0, 1) #define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_BARO_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f) -#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increate slightly +#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increase slightly #define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0) -#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f) +#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(2, 0) #define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?) #define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d7984caf2..64fad6bb5 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -1043,6 +1043,14 @@ void validateAndFixGyroConfig(void) float samplingTime = 0.000125f; + if (gyroConfig()->gyro_use_32khz) { +#ifdef GYRO_SUPPORTS_32KHZ + samplingTime = 0.00003125; +#else + gyroConfig()->gyro_use_32khz = false; +#endif + } + if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed gyroConfig()->gyro_sync_denom = 1; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 843a2484f..75daf1ed0 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1140,6 +1140,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, motorConfig()->motorPwmProtocol); sbufWriteU16(dst, motorConfig()->motorPwmRate); sbufWriteU16(dst, (uint16_t)(motorConfig()->digitalIdleOffsetPercent * 100)); + //!!TODO gyro_isr_update and gyro_use_32khz to be added once we decide to add them to configurator + //sbufWriteU8(dst, gyroConfig()->gyro_isr_update); + //sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); break; case MSP_FILTER_CONFIG : @@ -1483,6 +1486,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (dataSize > 7) { motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; } + //!!TODO gyro_isr_update and gyro_use_32khz to be added once we decide to add them to configurator + /*if (sbufBytesRemaining(src)) { + gyroConfig()->gyro_isr_update = sbufReadU8(src); + } + if (sbufBytesRemaining(src)) { + gyroConfig()->gyro_use_32khz = sbufReadU8(src); + }*/ + validateAndFixGyroConfig(); break; case MSP_SET_FILTER_CONFIG: diff --git a/src/main/fc/serial_cli.c b/src/main/fc/serial_cli.c index 71c047bd5..4b95ad1c0 100755 --- a/src/main/fc/serial_cli.c +++ b/src/main/fc/serial_cli.c @@ -600,9 +600,12 @@ const clivalue_t valueTable[] = { { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &boardAlignment()->yawDegrees, .config.minmax = { -180, 360 } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, - { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 8 } }, + { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 32 } }, #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) { "gyro_isr_update", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_isr_update, .config.lookup = { TABLE_OFF_ON } }, +#ifdef GYRO_SUPPORTS_32KHZ + { "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_use_32khz, .config.lookup = { TABLE_OFF_ON } }, +#endif #endif { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, @@ -3226,7 +3229,7 @@ static void cliTasks(char *cmdline) int averageLoadSum = 0; #ifndef CLI_MINIMAL_VERBOSITY - cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n"); + cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n"); #endif for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { cfTaskInfo_t taskInfo; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a280eae45..8cd8fd8fe 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -240,7 +240,8 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) if (!gyroDetect(&gyro.dev)) { return false; } - gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation + // Must set gyro sample rate before initialisation + gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz); gyro.dev.lpf = gyroConfig->gyro_lpf; gyro.dev.init(&gyro.dev); gyroInitFilters(); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 4dcc594b5..1fdf8820a 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -52,6 +52,7 @@ typedef struct gyroConfig_s { uint8_t gyro_soft_lpf_type; uint8_t gyro_soft_lpf_hz; bool gyro_isr_update; + bool gyro_use_32khz; uint16_t gyro_soft_notch_hz_1; uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2;