static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
{
static int32_t g[3];
static stdev_t var[3];
for (int 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 in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles();
return;
}
gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
}
}
if (isOnFinalGyroCalibrationCycle()) {
beeper(BEEPER_GYRO_CALIBRATED);
}
calibratingG--;
}
STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
gyroSensor->calibration.sum[axis] = 0.0f;
devClear(&gyroSensor->calibration.var[axis]);
// gyroZero is set to zero until calibration complete
gyroSensor->gyroDev.gyroZero[axis] = 0.0f;
}
// Sum up CALIBRATING_GYRO_TIME_US readings
gyroSensor->calibration.sum[axis] += gyroSensor->gyroDev.gyroADCRaw[axis];
devPush(&gyroSensor->calibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]);
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
// DEBUG_GYRO_CALIBRATION records the standard deviation of roll
// into the spare field - debug[3], in DEBUG_GYRO_RAW
if (axis == X) {
DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
}
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(gyroSensor);
return;
}
// please take care with exotic boardalignment !!
gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
if (axis == Z) {
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
}
}
}
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
beeper(BEEPER_GYRO_CALIBRATED);
}
}
--gyroSensor->calibration.cyclesRemaining;
}
static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
{
gyroSensor->calibration.cyclesRemaining = gyroCalculateCalibratingCycles();
}
static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
{
return gyroCalibration->cyclesRemaining == gyroCalculateCalibratingCycles();
}
void gyroSetCalibrationCycles(void)
{
calibratingG = gyroCalculateCalibratingCycles();
}
static bool isOnFirstGyroCalibrationCycle(void)
{
return calibratingG == gyroCalculateCalibratingCycles();
}
