diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 0480a88925..4b717ef25e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -70,7 +70,7 @@ #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))) +#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689))) #define USE_GYRO_SLEW_LIMITER #endif @@ -446,8 +446,9 @@ static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notch #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; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0; + } } #endif @@ -499,7 +500,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { #if defined(USE_GYRO_SLEW_LIMITER) - gyroInitSlewLimiter(gyroSensor); + gyroInitSlewLimiter(gyroSensor); #endif gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); @@ -600,20 +601,20 @@ 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 gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) { - int32_t newRawGyro = (int32_t) gyroSensor->gyroDev.gyroADCRaw[axis]; + 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; + if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) { + newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; + } else { + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; + } - return newRawGyro; + return newRawGyro; } #endif - void gyroUpdateSensor(gyroSensor_t *gyroSensor) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { @@ -622,14 +623,14 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyroSensor->gyroDev.dataReady = false; if (isGyroSensorCalibrationComplete(gyroSensor)) { - // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations + // 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]; + 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]; + gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z]; #else - gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; + gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; #endif alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);