private void pidApplyFixedWingRateController(PIDState pidState, int axis, float dT)
{
float rateError = pidState.rateTarget - pidState.gyroRate;
float newPTerm = rateError * pidState.kP;
float newFFTerm = pidState.rateTarget * pidState.kFF;
pidState.errorGyroIf += rateError * pidState.kI * dT;
pidState.errorGyroIf = constrainf(pidState.errorGyroIf, -pidState.errorGyroIfLimit, pidState.errorGyroIfLimit);
int pidSumLimit = 0; //Set manually
pidState.output = constrainf(newPTerm + newFFTerm + pidState.errorGyroIf, -pidSumLimit, pidSumLimit);
}
private void pidApplySetpointRateLimiting(PIDState pidState, int axis)
{
}
private void pidLevel(PIDState pidState, int axis, float stick, float stab_rate, int mode)
{
float angleTarget = pidRcCommandToAngle((int)(stick*500), MAX_ANGLE);
float rateTarget = pidRcCommandToRate((int)(stick*500), (int)stab_rate);
float angleRateTarget = 0;
if (mode == ANGLE_MODE)
{
holding_angle = 0;
}
if (mode == HORIZON_MODE && axis == FD_ROLL)
{
if (attitude[FD_ROLL] > MAX_ANGLE)
{
holding_angle = 1;
}
else if (attitude[FD_ROLL] < -MAX_ANGLE)
{
holding_angle = 1;
}
if (holding_angle == 1 && stick > 0)
{
holding_angle = 0;
}
else if (holding_angle == -1 && stick < 0)
{
holding_angle = 0;
}
if (holding_angle != 0)
{
angleTarget = MAX_ANGLE * holding_angle;
}
}
float angleErrorDeg;
if (mode == HORIZON_MODE && axis == FD_ROLL)
{
angleErrorDeg = attitude[FD_ROLL] - angleTarget;
}
else
{
angleErrorDeg = angleTarget - attitude[FD_ROLL];
}
if (mode == ANGLE_MODE || holding_angle != 0) {
angleRateTarget = constrainf(angleErrorDeg * PID_LEVEL_P, -stab_rate, stab_rate);
if (PID_LEVEL_I != 0)
{
angleRateTarget = pidState.angleFilterState.pt1FilterApply4(angleRateTarget, PID_LEVEL_I, dT);
}
}
else
{
angleRateTarget = rateTarget;
}
pidState.rateTarget = angleRateTarget;
}