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

BUGFIX - gyro calibration too short when movement detected during 

calibration.

When the model is moved during the gyro calibration period too much the
code that detects the movement (gyroMovementCalibrationThreshold aka
'moron threshold') did not restart the calibration correctly and one too
few calibration cycles were used when restarting.

In addition to this there was unnecessary calibration reset code - the
reset of calibration variables is supposed to happen at the start of
calibration; due to the bug the first calibration cycle could not be
detected when restarting so the old reset code was required. 

This commit also cleans up the Gyro code so the it is in a similar style
to the recently cleaned acceleration code.
This commit is contained in:
Dominic Clifton 2014-04-22 20:39:44 +01:00
parent db9042757d
commit c08d0ac1b3
1 changed files with 56 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
src/sensors_gyro.c
View File

@ -24,44 +24,63 @@ void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
calibratingG = calibrationCyclesRequired;
}
static void gyroCommon(uint8_t gyroMovementCalibrationThreshold)
bool isGyroCalibrationComplete(void)
{
int axis;
return calibratingG == 0;
}
bool isOnFinalGyroCalibrationCycle(void)
{
return calibratingG == 1;
}
bool isOnFirstGyroCalibrationCycle(void)
{
return calibratingG == CALIBRATING_GYRO_CYCLES;
}
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
{
int8_t axis;
static int32_t g[3];
static stdev_t var[3];
if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == CALIBRATING_GYRO_CYCLES) {
g[axis] = 0;
devClear(&var[axis]);
}
// Sum up 1000 readings
g[axis] += gyroADC[axis];
devPush(&var[axis], gyroADC[axis]);
// Clear global variables for next reading
gyroADC[axis] = 0;
gyroZero[axis] = 0;
if (calibratingG == 1) {
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
calibratingG = CALIBRATING_GYRO_CYCLES;
devClear(&var[0]);
devClear(&var[1]);
devClear(&var[2]);
g[0] = g[1] = g[2] = 0;
continue;
}
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
blinkLedAndSoundBeeper(10, 15, 1);
}
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle()) {
g[axis] = 0;
devClear(&var[axis]);
}
// Sum up CALIBRATING_GYRO_CYCLES readings
g[axis] += gyroADC[axis];
devPush(&var[axis], gyroADC[axis]);
// Reset global variables to prevent other code from using un-calibrated data
gyroADC[axis] = 0;
gyroZero[axis] = 0;
if (isOnFinalGyroCalibrationCycle()) {
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
return;
}
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
blinkLedAndSoundBeeper(10, 15, 1);
}
calibratingG--;
}
for (axis = 0; axis < 3; axis++)
calibratingG--;
}
static void applyGyroZero(void)
{
int8_t axis;
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
}
}
void gyroGetADC(uint8_t gyroMovementCalibrationThreshold)
@ -69,5 +88,10 @@ void gyroGetADC(uint8_t gyroMovementCalibrationThreshold)
// range: +/- 8192; +/- 2000 deg/sec
gyro.read(gyroADC);
alignSensors(gyroADC, gyroADC, gyroAlign);
gyroCommon(gyroMovementCalibrationThreshold);
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroMovementCalibrationThreshold);
}
applyGyroZero();
}