Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

Remove gyro overflow protection on MPU6500 and MPU9250

Browse source
This commit is contained in:
Martin Budden 2017-09-07 21:55:24 +01:00
parent 2f674972e3
commit 534b447b6f
1 changed files with 18 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

35
src/main/sensors/gyro.c
View file

@ -70,7 +70,7 @@
#include "hardware_revision.h" #include "hardware_revision.h"
#endif #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 #define USE_GYRO_SLEW_LIMITER
#endif #endif
@ -446,8 +446,9 @@ static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notch
#if defined(USE_GYRO_SLEW_LIMITER) #if defined(USE_GYRO_SLEW_LIMITER)
void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) { void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0; gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0;
}
} }
#endif #endif
@ -499,7 +500,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
{ {
#if defined(USE_GYRO_SLEW_LIMITER) #if defined(USE_GYRO_SLEW_LIMITER)
gyroInitSlewLimiter(gyroSensor); gyroInitSlewLimiter(gyroSensor);
#endif #endif
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); 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) #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)) if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
else } else {
gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro;
}
return newRawGyro; return newRawGyro;
} }
#endif #endif
void gyroUpdateSensor(gyroSensor_t *gyroSensor) void gyroUpdateSensor(gyroSensor_t *gyroSensor)
{ {
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
@ -622,14 +623,14 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
gyroSensor->gyroDev.dataReady = false; gyroSensor->gyroDev.dataReady = false;
if (isGyroSensorCalibrationComplete(gyroSensor)) { 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[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y];
#if defined(USE_GYRO_SLEW_LIMITER) #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 #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 #endif
alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);