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--; }
void updateAutotuneState(void) { static bool landedAfterAutoTuning = false; static bool autoTuneWasUsed = false; if (rcOptions[BOXAUTOTUNE]) { if (!f.AUTOTUNE_MODE) { if (f.ARMED) { if (isAutotuneIdle() || landedAfterAutoTuning) { autotuneReset(); landedAfterAutoTuning = false; } autotuneBeginNextPhase(¤tProfile.pidProfile, currentProfile.pidController); f.AUTOTUNE_MODE = 1; autoTuneWasUsed = true; } else { if (havePidsBeenUpdatedByAutotune()) { saveAndReloadCurrentProfileToCurrentProfileSlot(); autotuneReset(); } } } return; } if (f.AUTOTUNE_MODE) { autotuneEndPhase(); f.AUTOTUNE_MODE = 0; } if (!f.ARMED && autoTuneWasUsed) { landedAfterAutoTuning = true; } }
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { currentProfile.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; currentProfile.accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; saveAndReloadCurrentProfileToCurrentProfileSlot(); }
void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { uint8_t axis; static int32_t b[3]; static int16_t accZero_saved[3] = { 0, 0, 0 }; static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } }; // Saving old zeropoints before measurement if (InflightcalibratingA == 50) { accZero_saved[FD_ROLL] = accelerationTrims->raw[FD_ROLL]; accZero_saved[FD_PITCH] = accelerationTrims->raw[FD_PITCH]; accZero_saved[FD_YAW] = accelerationTrims->raw[FD_YAW]; angleTrim_saved.values.roll = rollAndPitchTrims->values.roll; angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch; } if (InflightcalibratingA > 0) { for (axis = 0; axis < 3; axis++) { // Reset a[axis] at start of calibration if (InflightcalibratingA == 50) b[axis] = 0; // Sum up 50 readings b[axis] += accADC[axis]; // Clear global variables for next reading accADC[axis] = 0; accelerationTrims->raw[axis] = 0; } // all values are measured if (InflightcalibratingA == 1) { AccInflightCalibrationActive = false; AccInflightCalibrationMeasurementDone = true; queueConfirmationBeep(2); // buzzer to indicatiing the end of calibration // recover saved values to maintain current flight behavior until new values are transferred accelerationTrims->raw[FD_ROLL] = accZero_saved[FD_ROLL]; accelerationTrims->raw[FD_PITCH] = accZero_saved[FD_PITCH]; accelerationTrims->raw[FD_YAW] = accZero_saved[FD_YAW]; rollAndPitchTrims->values.roll = angleTrim_saved.values.roll; rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch; } InflightcalibratingA--; } // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again AccInflightCalibrationSavetoEEProm = false; accelerationTrims->raw[FD_ROLL] = b[FD_ROLL] / 50; accelerationTrims->raw[FD_PITCH] = b[FD_PITCH] / 50; accelerationTrims->raw[FD_YAW] = b[FD_YAW] / 50 - acc_1G; // for nunchuk 200=1G resetRollAndPitchTrims(rollAndPitchTrims); saveAndReloadCurrentProfileToCurrentProfileSlot(); } }
int compassGetADC(flightDynamicsTrims_t *magZero) { static uint32_t t, tCal = 0; static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; uint32_t axis; if ((int32_t)(currentTime - t) < 0) return 0; //each read is spaced by 100ms t = currentTime + 100000; // Read mag sensor hmc5883lRead(magADC); alignSensors(magADC, magADC, magAlign); if (f.CALIBRATE_MAG) { tCal = t; for (axis = 0; axis < 3; axis++) { magZero->raw[axis] = 0; magZeroTempMin.raw[axis] = magADC[axis]; magZeroTempMax.raw[axis] = magADC[axis]; } f.CALIBRATE_MAG = 0; } if (magInit) { // we apply offset only once mag calibration is done magADC[X] -= magZero->raw[X]; magADC[Y] -= magZero->raw[Y]; magADC[Z] -= magZero->raw[Z]; } if (tCal != 0) { if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions LED0_TOGGLE; for (axis = 0; axis < 3; axis++) { if (magADC[axis] < magZeroTempMin.raw[axis]) magZeroTempMin.raw[axis] = magADC[axis]; if (magADC[axis] > magZeroTempMax.raw[axis]) magZeroTempMax.raw[axis] = magADC[axis]; } } else { tCal = 0; for (axis = 0; axis < 3; axis++) { magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets } saveAndReloadCurrentProfileToCurrentProfileSlot(); } } return 1; }