void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{
static int32_t a[3];
uint8_t axis;
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (isOnFirstAccelerationCalibrationCycle())
a[axis] = 0;
// Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accADC[axis];
// Reset global variables to prevent other code from using un-calibrated data
accADC[axis] = 0;
accelerationTrims->raw[axis] = 0;
}
if (isOnFinalAccelerationCalibrationCycle()) {
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
accelerationTrims->raw[FD_ROLL] = (a[FD_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[FD_PITCH] = (a[FD_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[FD_YAW] = (a[FD_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
resetRollAndPitchTrims(rollAndPitchTrims);
saveAndReloadCurrentProfileToCurrentProfileSlot();
}
calibratingA--;
}
static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{
static int32_t a[3];
for (int axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (isOnFirstAccelerationCalibrationCycle())
a[axis] = 0;
// Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accSmooth[axis];
// Reset global variables to prevent other code from using un-calibrated data
accSmooth[axis] = 0;
accelerationTrims->raw[axis] = 0;
}
if (isOnFinalAccelerationCalibrationCycle()) {
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc.acc_1G;
resetRollAndPitchTrims(rollAndPitchTrims);
saveConfigAndNotify();
}
calibratingA--;
}
void performAcclerationCalibration(void)
{
int axisIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable
if (axisIndex < 0) {
return;
}
// Top-up and first calibration cycle, reset everything
if (axisIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
for (int axis = 0; axis < 6; axis++) {
calibratedAxis[axis] = false;
accSamples[axis][X] = 0;
accSamples[axis][Y] = 0;
accSamples[axis][Z] = 0;
}
calibratedAxisCount = 0;
sensorCalibrationResetState(&calState);
}
if (!calibratedAxis[axisIndex]) {
sensorCalibrationPushSampleForOffsetCalculation(&calState, accADC);
accSamples[axisIndex][X] += accADC[X];
accSamples[axisIndex][Y] += accADC[Y];
accSamples[axisIndex][Z] += accADC[Z];
if (isOnFinalAccelerationCalibrationCycle()) {
calibratedAxis[axisIndex] = true;
calibratedAxisCount++;
beeperConfirmationBeeps(2);
}
}
if (calibratedAxisCount == 6) {
float accTmp[3];
int32_t accSample[3];
/* Calculate offset */
sensorCalibrationSolveForOffset(&calState, accTmp);
for (int axis = 0; axis < 3; axis++) {
accZero->raw[axis] = lrintf(accTmp[axis]);
}
/* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
sensorCalibrationResetState(&calState);
for (int axis = 0; axis < 6; axis++) {
accSample[X] = accSamples[axis][X] / CALIBRATING_ACC_CYCLES - accZero->raw[X];
accSample[Y] = accSamples[axis][Y] / CALIBRATING_ACC_CYCLES - accZero->raw[Y];
accSample[Z] = accSamples[axis][Z] / CALIBRATING_ACC_CYCLES - accZero->raw[Z];
sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.acc_1G);
}
sensorCalibrationSolveForScale(&calState, accTmp);
for (int axis = 0; axis < 3; axis++) {
accGain->raw[axis] = lrintf(accTmp[axis] * 4096);
}
saveConfigAndNotify();
}
calibratingA--;
}
