diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 2d1620ef14..6da30982f2 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,15 +39,24 @@ #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 sensorGyroReadDataFuncPtr temperature; // read temperature if available sensorGyroInterruptStatusFuncPtr intStatus; + sensorGyroUpdateFuncPtr update; 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_mpu.c b/src/main/drivers/accgyro_mpu.c index 8c60a64171..b452afc774 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -22,6 +22,7 @@ #include "platform.h" +#include "build/atomic.h" #include "build/build_config.h" #include "build/debug.h" @@ -46,7 +47,6 @@ #include "accgyro_spi_mpu9250.h" #include "accgyro_mpu.h" -//#define DEBUG_MPU_DATA_READY_INTERRUPT mpuResetFuncPtr mpuReset; @@ -220,15 +220,20 @@ static void mpu6050FindRevision(gyroDev_t *gyro) #if defined(MPU_INT_EXTI) static void mpuIntExtiHandler(extiCallbackRec_t *cb) { +#ifdef DEBUG_MPU_DATA_READY_INTERRUPT + static uint32_t lastCalledAtUs = 0; + const uint32_t nowUs = micros(); + debug[0] = (uint16_t)(nowUs - lastCalledAtUs); + lastCalledAtUs = nowUs; +#endif gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); gyro->dataReady = true; - + if (gyro->update) { + gyro->update(gyro); + } #ifdef DEBUG_MPU_DATA_READY_INTERRUPT - static uint32_t lastCalledAt = 0; - uint32_t now = micros(); - uint32_t callDelta = now - lastCalledAt; - debug[0] = callDelta; - lastCalledAt = now; + const uint32_t now2Us = micros(); + debug[1] = (uint16_t)(now2Us - nowUs); #endif } #endif @@ -296,6 +301,13 @@ bool mpuAccRead(accDev_t *acc) return true; } +void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn) +{ + ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) { + gyro->update = updateFn; + } +} + bool mpuGyroRead(gyroDev_t *gyro) { uint8_t data[6]; diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index bf15fcd4b2..8ba01540a3 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -18,6 +18,13 @@ #pragma once #include "exti.h" +#include "sensor.h" + +//#define DEBUG_MPU_DATA_READY_INTERRUPT + +#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) +#define GYRO_USES_SPI +#endif // MPU6050 #define MPU_RA_WHO_AM_I 0x75 @@ -190,3 +197,5 @@ bool mpuAccRead(struct accDev_s *acc); bool mpuGyroRead(struct gyroDev_s *gyro); mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro); bool mpuCheckDataReady(struct gyroDev_s *gyro); +void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn); + diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index ccbcc4e586..631f7ec805 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 74521e81fa..f9a46fbfc1 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 69af5cb015..29e22d5743 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 44ba012620..20fa4df8db 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 d7e4d6482f..9cd3993cf6 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 c1df190779..2415c2158f 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,37 @@ 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) { - int gyroSamplePeriod; +#ifndef GYRO_SUPPORTS_32KHZ + if (gyro_use_32khz) { + gyro_use_32khz = false; + } +#endif + + float gyroSamplePeriod; if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) { - gyroSamplePeriod = 125; + if (gyro_use_32khz) { + gyro->gyroRateKHz = GYRO_RATE_32_kHz; + gyroSamplePeriod = 31.5f; + } else { + gyro->gyroRateKHz = GYRO_RATE_8_kHz; + gyroSamplePeriod = 125.0f; + } } else { - gyroSamplePeriod = 1000; + gyro->gyroRateKHz = GYRO_RATE_1_kHz; + gyroSamplePeriod = 1000.0f; 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; + const uint32_t targetLooptime = (uint32_t)(gyroSyncDenominator * gyroSamplePeriod); 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 1b07878946..7bd7d375e5 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 ff053dbc0b..6b646435f4 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/drivers/sensor.h b/src/main/drivers/sensor.h index b9763695c3..1f50a9a1df 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -38,5 +38,6 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc); struct gyroDev_s; typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro); typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro); +typedef bool (*sensorGyroUpdateFuncPtr)(struct gyroDev_s *gyro); typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data); typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c2f6c1c3a7..32bbc19069 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -1046,6 +1046,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 bbaa5eb9e7..8d2aa09e22 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)); + sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); + //!!TODO gyro_isr_update to be added pending decision + //sbufWriteU8(dst, gyroConfig()->gyro_isr_update); 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; } + if (sbufBytesRemaining(src)) { + gyroConfig()->gyro_use_32khz = sbufReadU8(src); + } + //!!TODO gyro_isr_update to be added pending decision + /*if (sbufBytesRemaining(src)) { + gyroConfig()->gyro_isr_update = 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 2c09ab827c..92777f49d5 100755 --- a/src/main/fc/serial_cli.c +++ b/src/main/fc/serial_cli.c @@ -600,7 +600,13 @@ 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 } }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, @@ -3223,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/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 12c6419521..83a130a0d2 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -185,6 +185,23 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId) } } +void schedulerResetTaskStatistics(cfTaskId_e taskId) +{ +#ifdef SKIP_TASK_STATISTICS + UNUSED(taskId); +#else + if (taskId == TASK_SELF) { + currentTask->movingSumExecutionTime = 0; + currentTask->totalExecutionTime = 0; + currentTask->maxExecutionTime = 0; + } else if (taskId < TASK_COUNT) { + cfTasks[taskId].movingSumExecutionTime = 0; + cfTasks[taskId].totalExecutionTime = 0; + cfTasks[taskId].totalExecutionTime = 0; + } +#endif +} + void schedulerInit(void) { queueClear(); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 06014f01bb..8fbcd8c246 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -144,6 +144,7 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo); void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros); void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState); uint32_t getTaskDeltaTime(cfTaskId_e taskId); +void schedulerResetTaskStatistics(cfTaskId_e taskId); void schedulerInit(void); void scheduler(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8400c0f28e..53be29c818 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -54,6 +54,8 @@ #include "io/beeper.h" #include "io/statusindicator.h" +#include "scheduler/scheduler.h" + #include "sensors/sensors.h" #include "sensors/boardalignment.h" #include "sensors/gyro.h" @@ -240,7 +242,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(); @@ -358,26 +361,69 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold) } if (isOnFinalGyroCalibrationCycle()) { + schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics beeper(BEEPER_GYRO_CALIBRATED); } calibratingG--; } +#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) +static bool gyroUpdateISR(gyroDev_t* gyroDev) +{ + if (!gyroDev->dataReady || !gyroDev->read(gyroDev)) { + return false; + } +#ifdef DEBUG_MPU_DATA_READY_INTERRUPT + debug[2] = (uint16_t)(micros() & 0xffff); +#endif + gyroDev->dataReady = false; + // move gyro data into 32-bit variables to avoid overflows in calculations + gyroADC[X] = gyroDev->gyroADCRaw[X]; + gyroADC[Y] = gyroDev->gyroADCRaw[Y]; + gyroADC[Z] = gyroDev->gyroADCRaw[Z]; + + alignSensors(gyroADC, gyroDev->gyroAlign); + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroADC[axis] -= gyroZero[axis]; + // scale gyro output to degrees per second + float gyroADCf = (float)gyroADC[axis] * gyroDev->scale; + gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); + gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); + gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf); + } + return true; +} +#endif + void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec - if (!gyro.dev.read(&gyro.dev)) { + if (!gyro.dev.dataReady || !gyro.dev.read(&gyro.dev)) { return; } + const bool calibrationComplete = isGyroCalibrationComplete(); + if (calibrationComplete) { +#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) + // SPI-based gyro so can read and update in ISR + if (gyroConfig->gyro_isr_update) { + mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR); + return; + } +#endif +#ifdef DEBUG_MPU_DATA_READY_INTERRUPT + debug[3] = (uint16_t)(micros() & 0xffff); +#endif + } gyro.dev.dataReady = false; + // move gyro data into 32-bit variables to avoid overflows in calculations gyroADC[X] = gyro.dev.gyroADCRaw[X]; gyroADC[Y] = gyro.dev.gyroADCRaw[Y]; gyroADC[Z] = gyro.dev.gyroADCRaw[Z]; alignSensors(gyroADC, gyro.dev.gyroAlign); - const bool calibrationComplete = isGyroCalibrationComplete(); if (!calibrationComplete) { performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); } @@ -385,17 +431,17 @@ void gyroUpdate(void) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[axis] -= gyroZero[axis]; // scale gyro output to degrees per second - gyro.gyroADCf[axis] = (float)gyroADC[axis] * gyro.dev.scale; + float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale; - DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyro.gyroADCf[axis])); + DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); - gyro.gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyro.gyroADCf[axis]); + gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); - DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyro.gyroADCf[axis])); + DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf)); - gyro.gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyro.gyroADCf[axis]); + gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); - gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyro.gyroADCf[axis]); + gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf); if (!calibrationComplete) { gyroADC[axis] = lrintf(gyro.gyroADCf[axis] / gyro.dev.scale); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 9244fcc638..1fdf8820a6 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -51,6 +51,8 @@ typedef struct gyroConfig_s { uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. 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;