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 uint8_t gyroToUse = 0;
static FAST_RAM_ZERO_INIT bool overflowDetected;
#ifdef USE_GYRO_OVERFLOW_CHECK
static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
#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 gyroPrevious[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs;
static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs;
static FAST_RAM_ZERO_INIT int16_t gyroSensorTemperature;
static bool gyroHasOverflowProtection = true;
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)
{
const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale;
if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) {
if ((abs(gyroSensor->gyroDev.gyroADCf[X]) < gyroOverflowResetRate)
&& (abs(gyroSensor->gyroDev.gyroADCf[Y]) < 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
// reset requires good OK values on all axes
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
gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
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;
}
if (abs(gyro.gyroADCf[Y]) > gyroOverflowTriggerRate) {
if (abs(gyroSensor->gyroDev.gyroADCf[Y]) > gyroOverflowTriggerRate) {
overflowCheck |= GYRO_OVERFLOW_Y;
}
if (abs(gyro.gyroADCf[Z]) > gyroOverflowTriggerRate) {
if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroOverflowTriggerRate) {
overflowCheck |= GYRO_OVERFLOW_Z;
}
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)
{
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
if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) {
gyroSensor->yawSpinDetected = false;
@ -893,7 +901,7 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current
} else {
#ifndef SIMULATOR_BUILD
// 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->yawSpinTimeUs = currentTimeUs;
}
@ -941,10 +949,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
return;
}
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
checkForOverflow(gyroSensor, currentTimeUs);
@ -958,9 +962,9 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
#endif
if (gyroDebugMode == DEBUG_NONE) {
filterGyro(gyroSensor, sampleDeltaUs);
filterGyro(gyroSensor);
} else {
filterGyroDebug(gyroSensor, sampleDeltaUs);
filterGyroDebug(gyroSensor);
}
#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);
}
#endif
#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
UNUSED(currentTimeUs);
#endif
}
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
{
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
switch (gyroToUse) {
case GYRO_CONFIG_USE_GYRO_1:
gyroUpdateSensor(&gyroSensor1, currentTimeUs);
@ -979,6 +991,12 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
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, 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[Y] = gyroSensor2.gyroDev.gyroADCf[Y];
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, 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[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 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, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
@ -1026,6 +1057,15 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
break;
#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)
@ -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)
{
if (gyroSensor1.gyroDev.temperatureFn) {
gyroSensor1.gyroDev.temperatureFn(&gyroSensor1.gyroDev, &gyroSensor1.gyroDev.temperature);
switch (gyroToUse) {
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)
{
return gyroSensor1.gyroDev.temperature;
return gyroSensorTemperature;
}
int16_t gyroRateDps(int axis)
@ -1065,13 +1125,17 @@ int16_t gyroRateDps(int axis)
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
bool gyroYawSpinDetected(void)
{
return gyroSensor1.yawSpinDetected;
return yawSpinDetected;
}
#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++) {
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));
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;
}
}
}