private void UpdatePredictionPI()
{
phiTotal = PhiTotal();
phiVector = PhiVector();
for (int i = 0; i < 3; i++)
{
MaxOmega[i] = ControlTorque[i] * maxStoppingTime / ac.vessel.MOI[i];
}
TargetOmega[0] = pitchRatePI.Update(-phiVector[0], 0, MaxOmega[0]);
TargetOmega[1] = rollRatePI.Update(-phiVector[1], 0, MaxOmega[1]);
TargetOmega[2] = yawRatePI.Update(-phiVector[2], 0, MaxOmega[2]);
if (Math.Abs(phiTotal) > rollControlRange)
{
TargetOmega[1] = 0;
rollRatePI.ResetI();
}
TargetTorque[0] = pitchPI.Update(Omega[0], TargetOmega[0], ac.vessel.MOI[0], ControlTorque[0]);
TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], ac.vessel.MOI[1], ControlTorque[1]);
TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], ac.vessel.MOI[2], ControlTorque[2]);
}
private void UpdatePredictionPI()
{
Omega = -ac.vessel.angularVelocity;
UpdatePhi();
for (int i = 0; i < 3; i++)
{
MaxOmega[i] = ControlTorque[i] * maxStoppingTime / ac.vesselState.MoI[i];
}
TargetOmega[0] = pitchRatePI.Update(phiVector[0], 0, MaxOmega[0]);
TargetOmega[1] = rollRatePI.Update(phiVector[1], 0, MaxOmega[1]);
TargetOmega[2] = yawRatePI.Update(phiVector[2], 0, MaxOmega[2]);
if (useControlRange && Math.Abs(phiTotal) > rollControlRange)
{
TargetOmega[1] = 0;
rollRatePI.ResetI();
}
TargetTorque[0] = pitchPI.Update(Omega[0], TargetOmega[0], ac.vesselState.MoI[0], ControlTorque[0]);
TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], ac.vesselState.MoI[1], ControlTorque[1]);
TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], ac.vesselState.MoI[2], ControlTorque[2]);
}
public override void Reset()
{
pitchPI.ResetI();
yawPI.ResetI();
rollPI.ResetI();
pitchRatePI.ResetI();
yawRatePI.ResetI();
rollRatePI.ResetI();
}