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