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]); }