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

Change gyro_overflow_detect to use filtered instead of raw gyro data 

Noise and momentary spikes in gyro data (particularly in 32K mode) were causing false triggering of the gyro overflow detection.

Also adjusted the trigger and reset rates slightly (approx. 1950dps and 1850dps respectively).

Optimized the gyro loop slightly by eliminating 3 floating point multiplies.

Fixed bug for gyros that may not be +-2000dps full scale.
This commit is contained in:
Bruce Luckcuck 2018-02-14 17:50:09 -05:00
parent 9da7416c30
commit b49a9ec928
3 changed files with 26 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,23 +183,6 @@ 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};

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

@ -211,7 +211,6 @@ typedef struct mpuDetectionResult_s {
struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro);
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);

42
src/main/sensors/gyro.c
View File

@ -153,6 +153,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
#define GYRO_SYNC_DENOM_DEFAULT 4
#endif
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps)
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
@ -728,13 +731,9 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
// 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 = gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale;
const float gyroRateY = gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale;
const float gyroRateZ = gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale;
static const int overflowResetThreshold = 1800;
if (abs(gyroRateX) < overflowResetThreshold
&& abs(gyroRateY) < overflowResetThreshold
&& abs(gyroRateZ) < overflowResetThreshold) {
if (abs(gyroSensor->gyroDev.gyroADC[X]) < GYRO_OVERFLOW_RESET_THRESHOLD
&& abs(gyroSensor->gyroDev.gyroADC[Y]) < GYRO_OVERFLOW_RESET_THRESHOLD
&& abs(gyroSensor->gyroDev.gyroADC[Z]) < GYRO_OVERFLOW_RESET_THRESHOLD) {
// 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) {
@ -744,14 +743,25 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
// not a consecutive OK value, so reset the overflow time
gyroSensor->overflowTimeUs = currentTimeUs;
}
}
} else {
#ifndef SIMULATOR_BUILD
// check for overflow in the axes set in overflowAxisMask
if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) & overflowAxisMask) {
gyroSensor->overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs;
}
// check for overflow in the axes set in overflowAxisMask
gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
if (abs(gyroSensor->gyroDev.gyroADC[X]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
overflowCheck |= GYRO_OVERFLOW_X;
}
if (abs(gyroSensor->gyroDev.gyroADC[Y]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
overflowCheck |= GYRO_OVERFLOW_Y;
}
if (abs(gyroSensor->gyroDev.gyroADC[Z]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
overflowCheck |= GYRO_OVERFLOW_Z;
}
if (overflowCheck & overflowAxisMask) {
gyroSensor->overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs;
}
#endif // SIMULATOR_BUILD
}
#else
UNUSED(gyroSensor);
UNUSED(currentTimeUs);
@ -799,9 +809,6 @@ 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
@ -869,6 +876,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
}
}
}
if (gyroConfig()->checkOverflow) {
checkForOverflow(gyroSensor, currentTimeUs);
}
}
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)