void handleInflightCalibrationStickPosition(void)
{
if (AccInflightCalibrationMeasurementDone) {
// trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = false;
AccInflightCalibrationSavetoEEProm = true;
} else {
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
if (AccInflightCalibrationArmed) {
queueConfirmationBeep(2);
} else {
queueConfirmationBeep(3);
}
}
}
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();
}
}
void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position);
applied = true;
}
break;
}
if (applied) {
queueConfirmationBeep(position + 1);
}
}
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue;
float newFloatValue;
if (delta > 0) {
queueConfirmationBeep(2);
} else {
queueConfirmationBeep(1);
}
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
newValue = (int)controlRateConfig->rcRate8 + delta;
controlRateConfig->rcRate8 = constrain(newValue, 0, 250); // FIXME magic numbers repeated in serial_cli.c
generatePitchRollCurve(controlRateConfig);
break;
case ADJUSTMENT_RC_EXPO:
newValue = (int)controlRateConfig->rcExpo8 + delta;
controlRateConfig->rcExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
generatePitchRollCurve(controlRateConfig);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = (int)controlRateConfig->thrExpo8 + delta;
controlRateConfig->thrExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
generateThrottleCurve(controlRateConfig, escAndServoConfig);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = (int)controlRateConfig->rates[FD_PITCH] + delta;
controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = (int)controlRateConfig->rates[FD_ROLL] + delta;
controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
break;
case ADJUSTMENT_YAW_RATE:
newValue = (int)controlRateConfig->rates[FD_YAW] + delta;
controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
break;
case ADJUSTMENT_PITCH_ROLL_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f);
pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f);
pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->P8[PIDPITCH] + delta;
pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->P8[PIDROLL] + delta;
pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_PITCH_ROLL_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f);
pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f);
pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->I8[PIDPITCH] + delta;
pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->I8[PIDROLL] + delta;
pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_PITCH_ROLL_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->D8[PIDPITCH] + delta;
pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->D8[PIDROLL] + delta;
pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_YAW_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f);
pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->P8[PIDYAW] + delta;
pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_YAW_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f);
pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->I8[PIDYAW] + delta;
pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_YAW_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->D8[PIDYAW] + delta;
pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
default:
break;
};
}
