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

Revert "Change gyro_overflow_detect to use filtered instead of raw gyro data"

This commit is contained in:
Michael Keller 2018-02-18 09:25:04 +13:00 committed by GitHub
parent 3fbff5a700
commit e5364ee764
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 35 additions and 27 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};

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

@ -211,6 +211,7 @@ 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);

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

@ -153,9 +153,6 @@ 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,
@ -731,9 +728,13 @@ 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) {
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) {
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 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) {
@ -743,25 +744,14 @@ 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
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
}
#ifndef SIMULATOR_BUILD
// check for overflow in the axes set in overflowAxisMask
if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) & overflowAxisMask) {
gyroSensor->overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs;
}
#endif // SIMULATOR_BUILD
#else
UNUSED(gyroSensor);
UNUSED(currentTimeUs);
@ -809,6 +799,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
@ -876,9 +869,6 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
}
}
}
if (gyroConfig()->checkOverflow) {
checkForOverflow(gyroSensor, currentTimeUs);
}
}
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)