rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4754 from martinbudden/bfa_ysttm_gyro_threshold 

Added YSTTM gyro threshold
This commit is contained in:
Michael Keller 2017-12-18 19:17:42 +13:00 committed by GitHub
parent 379382036f b26ff88fd9
commit ddf0246ecd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 112 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/main/drivers/accgyro/accgyro_mpu.c
View File

@ -183,6 +183,23 @@ bool mpuGyroRead(gyroDev_t *gyro)
return true;
}
gyroOverflow_e mpuGyroCheckOverflow(const gyroDev_t *gyro)
{
// we cannot detect overflow directly, so assume overflow if absolute gyro rate is large
gyroOverflow_e ret = GYRO_OVERFLOW_NONE;
const int16_t overflowValue = 0x7C00; // this is a slightly conservative value, could probably be as high as 0x7FF0
if (gyro->gyroADCRaw[X] > overflowValue || gyro->gyroADCRaw[X] < -overflowValue) {
ret |= GYRO_OVERFLOW_X;
}
if (gyro->gyroADCRaw[Y] > overflowValue || gyro->gyroADCRaw[Y] < -overflowValue) {
ret |= GYRO_OVERFLOW_Y;
}
if (gyro->gyroADCRaw[Z] > overflowValue || gyro->gyroADCRaw[Z] < -overflowValue) {
ret |= GYRO_OVERFLOW_Z;
}
return ret;
}
bool mpuGyroReadSPI(gyroDev_t *gyro)
{
static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

13
src/main/drivers/accgyro/accgyro_mpu.h
View File

@ -176,6 +176,13 @@ enum accel_fsr_e {
NUM_ACCEL_FSR
};
typedef enum {
GYRO_OVERFLOW_NONE = 0x00,
GYRO_OVERFLOW_X = 0x01,
GYRO_OVERFLOW_Y = 0x02,
GYRO_OVERFLOW_Z = 0x04
} gyroOverflow_e;
typedef enum {
MPU_NONE,
MPU_3050,
@ -204,8 +211,10 @@ typedef struct mpuDetectionResult_s {
struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro);
struct accDev_s;
bool mpuAccRead(struct accDev_s *acc);
gyroOverflow_e mpuGyroCheckOverflow(const struct gyroDev_s *gyro);
bool mpuGyroRead(struct gyroDev_s *gyro);
bool mpuGyroReadSPI(struct gyroDev_s *gyro);
void mpuDetect(struct gyroDev_s *gyro);
struct accDev_s;
bool mpuAccRead(struct accDev_s *acc);

23
src/main/flight/pid.c
View File

@ -440,21 +440,7 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT
BEEP_ON;
}
if (axis == FD_YAW) {
// on yaw axis, prevent "yaw spin to the moon" after crash by constraining errorRate
#if !(defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_ICM20649))
#define GYRO_POTENTIAL_OVERFLOW_RATE 1990.0f
if (gyroRate > GYRO_POTENTIAL_OVERFLOW_RATE || gyroRate < -GYRO_POTENTIAL_OVERFLOW_RATE) {
// 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 there is a sign reversal we will actually increase crash-induced yaw spin
// so best thing to do is set error to zero.
errorRate = 0.0f;
} else
#endif
{
errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw);
}
} else {
// on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
if (sensors(SENSOR_ACC)) {
@ -522,8 +508,9 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT
previousRateError[axis] = rD;
// if crash recovery is on and accelerometer enabled then check for a crash
if (pidProfile->crash_recovery) {
// if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
// no point in trying to recover if the crash is so severe that the gyro overflows
if (pidProfile->crash_recovery && !gyroOverflowDetected()) {
if (ARMING_FLAG(ARMED)) {
if (motorMixRange >= 1.0f && !inCrashRecoveryMode
&& ABS(delta) > crashDtermThreshold
@ -545,8 +532,8 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT
axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
}
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) {
// Disable PID control if at zero throttle or if gyro overflow detected
if (!pidStabilisationEnabled || gyroOverflowDetected()) {
axisPID_P[axis] = 0;
axisPID_I[axis] = 0;
axisPID_D[axis] = 0;

1
src/main/interface/settings.c
View File

@ -335,6 +335,7 @@ const clivalue_t valueTable[] = {
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
{ "gyro_overflow_detect", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },
#if defined(GYRO_USES_SPI)
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
{ "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) },

75
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,10 +717,12 @@ 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;
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++) {
DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]);
@ -699,12 +753,14 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
if (!gyroSensor->overflowDetected) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
gyroPrevious[axis] = gyroADCf;
}
}
}
}
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
{
@ -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);