void autotuneStart(void)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
tuneCurrent[axis].gainP = pidBank()->pid[axis].P;
tuneCurrent[axis].gainI = pidBank()->pid[axis].I;
tuneCurrent[axis].gainD = pidBank()->pid[axis].D;
tuneCurrent[axis].pidSaturated = false;
tuneCurrent[axis].stateEnterTime = millis();
tuneCurrent[axis].state = DEMAND_TOO_LOW;
}
memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
lastGainsUpdateTime = millis();
}
static long cmsx_menuPidAltMag_onEnter(void)
{
cmsx_ReadPidToArray(cmsx_pidPosZ, PID_POS_Z);
cmsx_ReadPidToArray(cmsx_pidVelZ, PID_VEL_Z);
cmsx_pidHead[0] = pidBank()->pid[PID_HEADING].P;
return 0;
}
static void cmsx_ReadPidToArray(uint8_t *dst, int pidIndex)
{
dst[0] = pidBank()->pid[pidIndex].P;
dst[1] = pidBank()->pid[pidIndex].I;
dst[2] = pidBank()->pid[pidIndex].D;
}
void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput)
{
const timeMs_t currentTimeMs = millis();
const float absDesiredRateDps = fabsf(desiredRateDps);
float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
pidAutotuneState_e newState;
// Use different max desired rate in ANGLE for pitch and roll
// Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet.
if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) {
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
maxDesiredRate = MIN(maxDesiredRate, maxDesiredRateInAngleMode);
}
if (fabsf(pidOutput) >= pidProfile()->pidSumLimit) {
// If we have saturated the pid output by P+FF don't increase the gain
tuneCurrent[axis].pidSaturated = true;
}
if (absDesiredRateDps < (pidAutotuneConfig()->fw_max_rate_threshold / 100.0f) * maxDesiredRate) {
// We can make decisions only when we are demanding at least 50% of max configured rate
newState = DEMAND_TOO_LOW;
}
else if (fabsf(reachedRateDps) > absDesiredRateDps) {
newState = DEMAND_OVERSHOOT;
}
else {
newState = DEMAND_UNDERSHOOT;
}
if (newState != tuneCurrent[axis].state) {
const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime;
bool gainsUpdated = false;
switch (tuneCurrent[axis].state) {
case DEMAND_TOO_LOW:
break;
case DEMAND_OVERSHOOT:
if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) {
tuneCurrent[axis].gainD = tuneCurrent[axis].gainD * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainD < AUTOTUNE_FIXED_WING_MIN_FF) {
tuneCurrent[axis].gainD = AUTOTUNE_FIXED_WING_MIN_FF;
}
gainsUpdated = true;
}
break;
case DEMAND_UNDERSHOOT:
if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) {
tuneCurrent[axis].gainD = tuneCurrent[axis].gainD * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainD > AUTOTUNE_FIXED_WING_MAX_FF) {
tuneCurrent[axis].gainD = AUTOTUNE_FIXED_WING_MAX_FF;
}
gainsUpdated = true;
}
break;
}
if (gainsUpdated) {
// Set P-gain to 10% of FF gain (quite agressive - FIXME)
tuneCurrent[axis].gainP = tuneCurrent[axis].gainD * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
// Set integrator gain to reach the same response as FF gain in 0.667 second
tuneCurrent[axis].gainI = (tuneCurrent[axis].gainD / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
autotuneUpdateGains(tuneCurrent);
switch (axis) {
case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainD);
break;
case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainD);
break;
case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainD);
break;
}
}
// Change state and reset saturation flag
tuneCurrent[axis].state = newState;
tuneCurrent[axis].stateEnterTime = currentTimeMs;
tuneCurrent[axis].pidSaturated = false;
}
}
