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

Added gyro overflow checking and handling. Helps avoid YSTTM

Browse source
This commit is contained in:
Martin Budden 2017-12-14 14:19:07 +00:00
parent c3de899d47
commit b26ff88fd9
6 changed files with 112 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -110,6 +110,8 @@ typedef struct gyroSensor_s {
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
timeUs_t overflowTimeUs;
bool overflowDetected;
} gyroSensor_t;
static FAST_RAM gyroSensor_t gyroSensor1;
@ -127,6 +129,16 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
#define GYRO_SYNC_DENOM_DEFAULT 4
#endif
#if (defined(USE_GYRO_SPI_MPU6500) \
|| defined(USE_GYRO_SPI_MPU9250) \
|| defined(USE_GYRO_SPI_ICM20601) \
|| defined(USE_GYRO_SPI_ICM20649) \
|| defined(USE_GYRO_SPI_ICM20689))
#define GYRO_CHECK_OVERFLOW_DEFAULT true
#else
#define GYRO_CHECK_OVERFLOW_DEFAULT false
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
@ -142,7 +154,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_hz_1 = 400,
.gyro_soft_notch_cutoff_1 = 300,
.gyro_soft_notch_hz_2 = 200,
.gyro_soft_notch_cutoff_2 = 100
.gyro_soft_notch_cutoff_2 = 100,
.checkOverflow = GYRO_CHECK_OVERFLOW_DEFAULT
);
@ -607,16 +620,52 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
#if defined(USE_GYRO_SLEW_LIMITER)
FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int 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;
int32_t ret = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
if (gyroConfig()->checkOverflow) {
// don't use the slew limiter if overflow checking is on
return ret;
}
return newRawGyro;
if (abs(ret - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
// there has been a large change in value, so assume overflow has occurred and return the previous value
ret = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
} else {
gyroSensor->gyroDev.gyroADCRawPrevious[axis] = ret;
}
return ret;
}
#endif
static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
// check for overflow to handle Yaw Spin To The Moon (YSTTM)
// ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
// This can cause an overflow and sign reversal in the output.
// Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
if (gyroSensor->overflowDetected) {
const float gyroRateX = (float)gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale;
const float gyroRateY = (float)gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale;
const float gyroRateZ = (float)gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale;
static const int overflowResetThreshold = 1800;
if (abs(gyroRateX) < overflowResetThreshold
&& abs(gyroRateY) < overflowResetThreshold
&& abs(gyroRateZ) < overflowResetThreshold) {
// if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected
if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) {
gyroSensor->overflowDetected = false;
}
} else {
// not a consecutive OK value, so reset the overflow time
gyroSensor->overflowTimeUs = currentTimeUs;
}
}
#ifndef SIMULATOR_BUILD
if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) != GYRO_OVERFLOW_NONE) {
gyroSensor->overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs;
}
#endif
}
static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
@ -654,6 +703,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
if (gyroConfig()->checkOverflow) {
checkForOverflow(gyroSensor, currentTimeUs);
}
if (gyroDebugMode == DEBUG_NONE) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
@ -665,9 +717,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
gyroPrevious[axis] = gyroADCf;
if (!gyroSensor->overflowDetected) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
gyroPrevious[axis] = gyroADCf;
}
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -699,9 +753,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
gyroPrevious[axis] = gyroADCf;
if (!gyroSensor->overflowDetected) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
gyroPrevious[axis] = gyroADCf;
}
}
}
}
@ -745,3 +801,8 @@ int16_t gyroRateDps(int axis)
{
return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale);
}
bool gyroOverflowDetected(void)
{
return gyroSensor1.overflowDetected;
}

3
src/main/sensors/gyro.h
View file

@ -63,6 +63,8 @@ typedef struct gyroConfig_s {
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
uint16_t overflowResetThreshold;
bool checkOverflow;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);
@ -83,3 +85,4 @@ bool isGyroCalibrationComplete(void);
void gyroReadTemperature(void);
int16_t gyroGetTemperature(void);
int16_t gyroRateDps(int axis);
bool gyroOverflowDetected(void);