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

Merge pull request #6844 from etracer65/dual_gyro_fixes 

Dual gyro fixes - fix non-reentrant code in gyro sensor update and hardcoded gyroSensor1 logic
This commit is contained in:
Michael Keller 2018-10-01 23:39:51 +13:00 committed by GitHub
parent 9bdd41d473 6656780205
commit 8980ba1065
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 84 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -86,15 +86,23 @@ FAST_RAM_ZERO_INIT gyro_t gyro;
static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode; static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode;
static uint8_t gyroToUse = 0; static uint8_t gyroToUse = 0;
static FAST_RAM_ZERO_INIT bool overflowDetected;
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask; static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
#endif #endif
#ifdef USE_YAW_SPIN_RECOVERY
static FAST_RAM_ZERO_INIT bool yawSpinDetected;
#endif
static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs; static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs;
static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs; static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs;
static FAST_RAM_ZERO_INIT int16_t gyroSensorTemperature;
static bool gyroHasOverflowProtection = true; static bool gyroHasOverflowProtection = true;
typedef struct gyroCalibration_s { typedef struct gyroCalibration_s {
@ -815,9 +823,9 @@ FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
static FAST_CODE_NOINLINE void handleOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) static FAST_CODE_NOINLINE void handleOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{ {
const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale; const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale;
if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate) if ((abs(gyroSensor->gyroDev.gyroADCf[X]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate) && (abs(gyroSensor->gyroDev.gyroADCf[Y]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) { && (abs(gyroSensor->gyroDev.gyroADCf[Z]) < gyroOverflowResetRate)) {
// if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected // 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 // reset requires good OK values on all axes
if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) { if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) {
@ -842,13 +850,13 @@ static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t curren
// check for overflow in the axes set in overflowAxisMask // check for overflow in the axes set in overflowAxisMask
gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE; gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyroSensor->gyroDev.scale; const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyroSensor->gyroDev.scale;
if (abs(gyro.gyroADCf[X]) > gyroOverflowTriggerRate) { if (abs(gyroSensor->gyroDev.gyroADCf[X]) > gyroOverflowTriggerRate) {
overflowCheck |= GYRO_OVERFLOW_X; overflowCheck |= GYRO_OVERFLOW_X;
} }
if (abs(gyro.gyroADCf[Y]) > gyroOverflowTriggerRate) { if (abs(gyroSensor->gyroDev.gyroADCf[Y]) > gyroOverflowTriggerRate) {
overflowCheck |= GYRO_OVERFLOW_Y; overflowCheck |= GYRO_OVERFLOW_Y;
} }
if (abs(gyro.gyroADCf[Z]) > gyroOverflowTriggerRate) { if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroOverflowTriggerRate) {
overflowCheck |= GYRO_OVERFLOW_Z; overflowCheck |= GYRO_OVERFLOW_Z;
} }
if (overflowCheck & overflowAxisMask) { if (overflowCheck & overflowAxisMask) {
@ -867,7 +875,7 @@ static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t curren
static FAST_CODE_NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) static FAST_CODE_NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{ {
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f; const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) { if (abs(gyroSensor->gyroDev.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough // testing whether 20ms of consecutive OK gyro yaw values is enough
if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) { if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) {
gyroSensor->yawSpinDetected = false; gyroSensor->yawSpinDetected = false;
@ -893,7 +901,7 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current
} else { } else {
#ifndef SIMULATOR_BUILD #ifndef SIMULATOR_BUILD
// check for spin on yaw axis only // check for spin on yaw axis only
if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) { if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
gyroSensor->yawSpinDetected = true; gyroSensor->yawSpinDetected = true;
gyroSensor->yawSpinTimeUs = currentTimeUs; gyroSensor->yawSpinTimeUs = currentTimeUs;
} }
@ -941,10 +949,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
return; return;
} }
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) { if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
checkForOverflow(gyroSensor, currentTimeUs); checkForOverflow(gyroSensor, currentTimeUs);
@ -958,9 +962,9 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
#endif #endif
if (gyroDebugMode == DEBUG_NONE) { if (gyroDebugMode == DEBUG_NONE) {
filterGyro(gyroSensor, sampleDeltaUs); filterGyro(gyroSensor);
} else { } else {
filterGyroDebug(gyroSensor, sampleDeltaUs); filterGyroDebug(gyroSensor);
} }
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
@ -968,10 +972,18 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn); gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
} }
#endif #endif
#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
UNUSED(currentTimeUs);
#endif
} }
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
{ {
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
switch (gyroToUse) { switch (gyroToUse) {
case GYRO_CONFIG_USE_GYRO_1: case GYRO_CONFIG_USE_GYRO_1:
gyroUpdateSensor(&gyroSensor1, currentTimeUs); gyroUpdateSensor(&gyroSensor1, currentTimeUs);
@ -979,6 +991,12 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X]; gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y]; gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z]; gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z];
#ifdef USE_GYRO_OVERFLOW_CHECK
overflowDetected = gyroSensor1.overflowDetected;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor1.yawSpinDetected;
#endif
} }
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
@ -994,6 +1012,12 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X]; gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X];
gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y]; gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y];
gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z]; gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z];
#ifdef USE_GYRO_OVERFLOW_CHECK
overflowDetected = gyroSensor2.overflowDetected;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor2.yawSpinDetected;
#endif
} }
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
@ -1009,7 +1033,14 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f; gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f;
gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f; gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f;
gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f; gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f;
#ifdef USE_GYRO_OVERFLOW_CHECK
overflowDetected = gyroSensor1.overflowDetected || gyroSensor2.overflowDetected;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor1.yawSpinDetected || gyroSensor2.yawSpinDetected;
#endif
} }
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
@ -1026,6 +1057,15 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
break; break;
#endif #endif
} }
if (!overflowDetected) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * sampleDeltaUs;
gyroPrevious[axis] = gyro.gyroADCf[axis];
}
}
} }
bool gyroGetAccumulationAverage(float *accumulationAverage) bool gyroGetAccumulationAverage(float *accumulationAverage)
@ -1046,16 +1086,36 @@ bool gyroGetAccumulationAverage(float *accumulationAverage)
} }
} }
int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor)
{
if (gyroSensor.gyroDev.temperatureFn) {
gyroSensor.gyroDev.temperatureFn(&gyroSensor.gyroDev, &gyroSensor.gyroDev.temperature);
}
return gyroSensor.gyroDev.temperature;
}
void gyroReadTemperature(void) void gyroReadTemperature(void)
{ {
if (gyroSensor1.gyroDev.temperatureFn) { switch (gyroToUse) {
gyroSensor1.gyroDev.temperatureFn(&gyroSensor1.gyroDev, &gyroSensor1.gyroDev.temperature); case GYRO_CONFIG_USE_GYRO_1:
gyroSensorTemperature = gyroReadSensorTemperature(gyroSensor1);
break;
#ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2:
gyroSensorTemperature = gyroReadSensorTemperature(gyroSensor2);
break;
case GYRO_CONFIG_USE_GYRO_BOTH:
gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyroSensor1), gyroReadSensorTemperature(gyroSensor2));
break;
#endif // USE_MULTI_GYRO
} }
} }
int16_t gyroGetTemperature(void) int16_t gyroGetTemperature(void)
{ {
return gyroSensor1.gyroDev.temperature; return gyroSensorTemperature;
} }
int16_t gyroRateDps(int axis) int16_t gyroRateDps(int axis)
@ -1065,13 +1125,17 @@ int16_t gyroRateDps(int axis)
bool gyroOverflowDetected(void) bool gyroOverflowDetected(void)
{ {
return gyroSensor1.overflowDetected; #ifdef USE_GYRO_OVERFLOW_CHECK
return overflowDetected;
#else
return false;
#endif // USE_GYRO_OVERFLOW_CHECK
} }
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
bool gyroYawSpinDetected(void) bool gyroYawSpinDetected(void)
{ {
return gyroSensor1.yawSpinDetected; return yawSpinDetected;
} }
#endif // USE_YAW_SPIN_RECOVERY #endif // USE_YAW_SPIN_RECOVERY

7
src/main/sensors/gyro_filter_impl.h
View File

@ -1,4 +1,4 @@
static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDelta_t sampleDeltaUs) static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
{ {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]); GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]);
@ -44,10 +44,5 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
if (!gyroSensor->overflowDetected) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
gyroPrevious[axis] = gyroADCf;
}
} }
} }