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

Add option to check gyro overflow on yaw or all axes

Browse source
This commit is contained in:
Martin Budden 2017-12-20 19:39:12 +00:00
parent eccdf2d4a5
commit 2281c9ab8c
5 changed files with 46 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -77,6 +77,9 @@
FAST_RAM gyro_t gyro;
static FAST_RAM uint8_t gyroDebugMode;
#ifdef USE_GYRO_OVERFLOW_CHECK
static FAST_RAM uint8_t overflowAxisMask;
#endif
static FAST_RAM float accumulatedMeasurements[XYZ_AXIS_COUNT];
static FAST_RAM float gyroPrevious[XYZ_AXIS_COUNT];
static FAST_RAM timeUs_t accumulatedMeasurementTimeUs;
@ -145,7 +148,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_cutoff_1 = 300,
.gyro_soft_notch_hz_2 = 200,
.gyro_soft_notch_cutoff_2 = 100,
.checkOverflow = true
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES
);
@ -387,6 +390,16 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
bool gyroInit(void)
{
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
overflowAxisMask = GYRO_OVERFLOW_Z;
} else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
} else {
overflowAxisMask = 0;
}
#endif
switch (debugMode) {
case DEBUG_FFT:
case DEBUG_GYRO_NOTCH:
@ -627,6 +640,7 @@ FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
#ifdef USE_GYRO_OVERFLOW_CHECK
// 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.
@ -640,6 +654,7 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
&& 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
// reset requires good OK values on all axes
if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) {
gyroSensor->overflowDetected = false;
}
@ -649,11 +664,16 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
}
}
#ifndef SIMULATOR_BUILD
if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) != GYRO_OVERFLOW_NONE) {
// check for overflow in the axes set in overflowAxisMask
if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) & overflowAxisMask) {
gyroSensor->overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs;
}
#endif
#endif // SIMULATOR_BUILD
#else
UNUSED(gyroSensor);
UNUSED(currentTimeUs);
#endif // USE_GYRO_OVERFLOW_CHECK
}
static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)

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

@ -49,6 +49,12 @@ typedef struct gyro_s {
extern gyro_t gyro;
typedef enum {
GYRO_OVERFLOW_CHECK_NONE = 0,
GYRO_OVERFLOW_CHECK_YAW,
GYRO_OVERFLOW_CHECK_ALL_AXES
} gyroOverflowCheck_e;
typedef struct gyroConfig_s {
sensor_align_e gyro_align; // gyro alignment
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
@ -63,8 +69,7 @@ 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;
gyroOverflowCheck_e checkOverflow;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);