Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Merge pull request #2773 from martinbudden/bf_gyro_reentrant_calibration

Browse source 
Further work on supporting 2 gyros
This commit is contained in:
Martin Budden 2017-05-02 10:57:15 +01:00 • committed by GitHub
commit 3e4325f484
1 changed files with 33 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

66
src/main/sensors/gyro.c
View file

@ -74,7 +74,13 @@ gyro_t gyro;
STATIC_UNIT_TESTED gyroDev_t gyroDev0; STATIC_UNIT_TESTED gyroDev_t gyroDev0;
static int16_t gyroTemperature0; static int16_t gyroTemperature0;
static uint16_t calibratingG = 0; typedef struct gyroCalibration_s {
int32_t g[XYZ_AXIS_COUNT];
stdev_t var[XYZ_AXIS_COUNT];
uint16_t calibratingG;
} gyroCalibration_t;
STATIC_UNIT_TESTED gyroCalibration_t gyroCalibration;
static filterApplyFnPtr softLpfFilterApplyFn; static filterApplyFnPtr softLpfFilterApplyFn;
static void *softLpfFilter[3]; static void *softLpfFilter[3];
@ -412,12 +418,12 @@ void gyroInitFilters(void)
bool isGyroCalibrationComplete(void) bool isGyroCalibrationComplete(void)
{ {
return calibratingG == 0; return gyroCalibration.calibratingG == 0;
} }
static bool isOnFinalGyroCalibrationCycle(void) static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
{ {
return calibratingG == 1; return gyroCalibration->calibratingG == 1;
} }
static uint16_t gyroCalculateCalibratingCycles(void) static uint16_t gyroCalculateCalibratingCycles(void)
@ -425,56 +431,52 @@ static uint16_t gyroCalculateCalibratingCycles(void)
return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
} }
static bool isOnFirstGyroCalibrationCycle(void) static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
{ {
return calibratingG == gyroCalculateCalibratingCycles(); return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles();
} }
void gyroSetCalibrationCycles(void) void gyroSetCalibrationCycles(void)
{ {
calibratingG = gyroCalculateCalibratingCycles(); gyroCalibration.calibratingG = gyroCalculateCalibratingCycles();
} }
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, uint8_t gyroMovementCalibrationThreshold) STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold)
{ {
static int32_t g[3];
static stdev_t var[3];
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration // Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle()) { if (isOnFirstGyroCalibrationCycle(gyroCalibration)) {
g[axis] = 0; gyroCalibration->g[axis] = 0;
devClear(&var[axis]); devClear(&gyroCalibration->var[axis]);
} }
// Sum up CALIBRATING_GYRO_CYCLES readings // Sum up CALIBRATING_GYRO_CYCLES readings
g[axis] += gyroDev->gyroADC[axis]; gyroCalibration->g[axis] += gyroDev->gyroADC[axis];
devPush(&var[axis], gyroDev->gyroADC[axis]); devPush(&gyroCalibration->var[axis], gyroDev->gyroADC[axis]);
// Reset global variables to prevent other code from using un-calibrated data // Reset global variables to prevent other code from using un-calibrated data
gyroDev->gyroADC[axis] = 0; gyroDev->gyroADC[axis] = 0;
gyroDev->gyroZero[axis] = 0; gyroDev->gyroZero[axis] = 0;
if (isOnFinalGyroCalibrationCycle()) { if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
const float dev = devStandardDeviation(&var[axis]); const float stddev = devStandardDeviation(&gyroCalibration->var[axis]);
DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(dev)); DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
// check deviation and startover in case the model was moved // check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(); gyroSetCalibrationCycles();
return; return;
} }
gyroDev->gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); gyroDev->gyroZero[axis] = (gyroCalibration->g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
} }
} }
if (isOnFinalGyroCalibrationCycle()) { if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
beeper(BEEPER_GYRO_CALIBRATED); beeper(BEEPER_GYRO_CALIBRATED);
} }
calibratingG--; gyroCalibration->calibratingG--;
} }
@ -489,14 +491,13 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev)
#endif #endif
gyroDev->dataReady = false; gyroDev->dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations // move gyro data into 32-bit variables to avoid overflows in calculations
gyroDev->gyroADC[X] = gyroDev->gyroADCRaw[X]; gyroDev->gyroADC[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X];
gyroDev->gyroADC[Y] = gyroDev->gyroADCRaw[Y]; gyroDev->gyroADC[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y];
gyroDev->gyroADC[Z] = gyroDev->gyroADCRaw[Z]; gyroDev->gyroADC[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z];
alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign); alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroDev->gyroADC[axis] -= gyroDev->gyroZero[axis];
// scale gyro output to degrees per second // scale gyro output to degrees per second
float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale; float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale;
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
@ -523,9 +524,9 @@ void gyroUpdate(void)
} }
gyroDev0.dataReady = false; gyroDev0.dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations // move gyro data into 32-bit variables to avoid overflows in calculations
gyroDev0.gyroADC[X] = gyroDev0.gyroADCRaw[X]; gyroDev0.gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
gyroDev0.gyroADC[Y] = gyroDev0.gyroADCRaw[Y]; gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
gyroDev0.gyroADC[Z] = gyroDev0.gyroADCRaw[Z]; gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign); alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign);
@ -542,11 +543,10 @@ void gyroUpdate(void)
debug[3] = (uint16_t)(micros() & 0xffff); debug[3] = (uint16_t)(micros() & 0xffff);
#endif #endif
} else { } else {
performGyroCalibration(&gyroDev0, gyroConfig()->gyroMovementCalibrationThreshold); performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
} }
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroDev0.gyroADC[axis] -= gyroDev0.gyroZero[axis];
// scale gyro output to degrees per second // scale gyro output to degrees per second
float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale; float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale;