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--; }