diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index eece833027..f6df5875a0 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -60,6 +60,7 @@ typedef struct gyroDev_s { int32_t gyroZero[XYZ_AXIS_COUNT]; int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment int16_t gyroADCRaw[XYZ_AXIS_COUNT]; + int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT]; int16_t temperature; uint8_t lpf; gyroRateKHz_e gyroRateKHz; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 74fff590bd..8a8c89c6d1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -19,6 +19,7 @@ #include #include #include +#include #include "platform.h" @@ -69,6 +70,10 @@ #include "hardware_revision.h" #endif +#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689))) +#define USE_GYRO_SLEW_LIMITER +#endif + gyro_t gyro; static uint8_t gyroDebugMode; @@ -365,6 +370,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align; } + gyroInitSensorFilters(gyroSensor); #ifdef USE_GYRO_DATA_ANALYSE gyroDataAnalyseInit(gyro.targetLooptime); @@ -437,6 +443,14 @@ static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notch return notchHz; } +#if defined(USE_GYRO_SLEW_LIMITER) +void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) { + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0; +} +#endif + static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) { gyroSensor->notchFilter1ApplyFn = nullFilterApply; @@ -484,6 +498,9 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { +#if defined(USE_GYRO_SLEW_LIMITER) + gyroInitSlewLimiter(gyroSensor); +#endif gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); @@ -582,6 +599,21 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t } +#if defined(USE_GYRO_SLEW_LIMITER) +int32_t gyroSlewLimiter(int axis, gyroSensor_t *gyroSensor) +{ + int32_t newRawGyro = (int32_t) gyroSensor->gyroDev.gyroADCRaw[axis]; + + if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) + newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; + else + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; + + return newRawGyro; +} +#endif + + void gyroUpdateSensor(gyroSensor_t *gyroSensor) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { @@ -590,10 +622,15 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyroSensor->gyroDev.dataReady = false; if (isGyroSensorCalibrationComplete(gyroSensor)) { - // move gyro data into 32-bit variables to avoid overflows in calculations - gyroSensor->gyroDev.gyroADC[X] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[X] - (int32_t)gyroSensor->gyroDev.gyroZero[X]; - gyroSensor->gyroDev.gyroADC[Y] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Y] - (int32_t)gyroSensor->gyroDev.gyroZero[Y]; - gyroSensor->gyroDev.gyroADC[Z] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Z] - (int32_t)gyroSensor->gyroDev.gyroZero[Z]; + // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations + + gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X]; + gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; +#if defined(USE_GYRO_SLEW_LIMITER) + gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(Z, gyroSensor) - gyroSensor->gyroDev.gyroZero[Z]; +#else + gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; +#endif alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); } else {