public float Update(float angleError, float angularVelocity, float maxAA)
{
var aaNorm = Utils.ClampH(maxAngularAcceleration / maxAA, 1);
var aaCap = Mathf.Min(maxAngularAcceleration, maxAA);
var tau = avFilter * TimeWarp.fixedDeltaTime;
avActionFilter.Tau = tau;
pid.setTau(tau);
var errorDecreases = angleError < 0 && angularVelocity > 0 ||
angleError > 0 && angularVelocity < 0;
if (Mathf.Abs(angleError) < angularErrorTolerance)
{
pid.Update(Utils.Clamp(angularVelocity / aaCap, -1, 1) * aaNorm);
}
else if (errorDecreases)
{
var accelToStop = angleError.Equals(0)
? 0
: angularVelocity * angularVelocity / 2 / angleError;
var accelToStopAbs = Mathf.Abs(accelToStop);
var decelerateThreshold = Mathf.Abs(pid.Action) < upperLowerActionThreshold
? decelerateThresholdUpper
: decelerateThresholdLower;
if (accelToStopAbs > decelerateThreshold * aaCap)
{
pid.Update(-Utils.Clamp(accelToStop, -aaCap, aaCap) / maxAA);
}
else if (accelToStopAbs < accelerateThreshold * aaCap &&
Math.Abs(angularVelocity) < maxAngularVelocity)
{
pid.Update(Utils.Clamp(angleError * angleErrorToActionP, -1, 1) * aaNorm);
}
else
{
pid.Update(0);
}
}
else
{
pid.Update(Utils.Clamp(angleError * angleErrorToActionP + angularVelocity / aaCap,
-1,
1)
* aaNorm);
}
var action = tau > 0 ? avActionFilter.Update(pid.Action) : pid.Action;
if (odFilter > 0)
{
action *= 1 - odFilter * OD.Update(pid.Action, TimeWarp.fixedDeltaTime);
}
// DebugUtils.CSV($"AxisAttitudeCascade-{name}.csv",
// Time.timeSinceLevelLoad,
// angleError,
// angularVelocity,
// pid.Action,
// action); //debug
return(-action);
}
public float UpdateAV(float avError)
{
var tau = ATCB.avAction_Filter * TimeWarp.fixedDeltaTime;
avFilter.Tau = tau;
avPID.setTau(tau);
avPID.Update(avError);
avPID.Action = avFilter.Update(avPID.Action);
return(avPID.Action);
}
