public void OnFixedUpdate() { if (Vessel != null) { UpdateStateVectors(Vessel, Target); SteeringHelper.AnalyzeParts(Vessel); Vector3 Torque = SteeringHelper.TorqueAvailable; var CoM = Vessel.CoM; var MoI = Vessel.MOI; phiTotal = calculatePhiTotal(); phiVector = calculatePhiVector();//deviation errors from orientation target for (int i = 0; i < 3; i++) { MaxOmega[i] = Mathf.Max(Torque[i] / MoI[i], 0.0001f); } 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], Vessel.MOI[0], Torque[0]); TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]); TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]); } }
public void OnFixedUpdate() { if (Vessel != null) { UpdateStateVectors(Vessel, Target); Vector3 Torque = SteeringHelper.GetVesselTorque(Vessel); var CoM = Vessel.CoM; var MoI = Vessel.MOI; phiTotal = PhiTotal(); phiVector = PhiVector(); for (int i = 0; i < 3; i++) { MaxOmega[i] = Torque[i] * MaxStoppingTime / 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], Vessel.MOI[0], Torque[0]); TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]); TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]); } }
public Vector3d GetActuation(Quaternion thisTarget) { Target = thisTarget; UpdateStateVectors(Vessel, Target); Vector3 Torque = SteeringHelper.GetVesselTorque(Vessel); for (int i = 0; i < 3; i++) { Actuation[i] = (Torque[i] == 0.0) ? 0.0 :TargetTorque[i] / Torque[i]; //removed the value modifications/clamps as the raw output is desired and can be post-processed. } return(Actuation); }
public Vector3d GetActuation(Quaternion thisTarget) { Target = thisTarget; UpdateStateVectors(Vessel, Target); SteeringHelper.UpdateRCSThrustAndTorque(Vessel); Vector3 Torque = SteeringHelper.TorqueAvailable; for (int i = 0; i < 3; i++) { Actuation[i] = TargetTorque[i] / Torque[i]; if (Math.Abs(Actuation[i]) < EPSILON || double.IsNaN(Actuation[i]) || double.IsInfinity(Actuation[i])) { Actuation[i] = 0; } } return(Actuation); }
// Calculations of Tf are not safe during FlightComputer constructor // Probably because the ship is only half-initialized... public void updatePIDParameters() { if (Vessel != null) { Vector3 torque = SteeringHelper.GetVesselTorque(Vessel); var CoM = Vessel.CoM; var MoI = Vessel.MOI; Vector3d ratio = new Vector3d( torque.x != 0 ? MoI.x / torque.x : 0, torque.y != 0 ? MoI.y / torque.y : 0, torque.z != 0 ? MoI.z / torque.z : 0 ); Tf = Mathf.Clamp((float)ratio.magnitude / 20f, 2 * TimeWarp.fixedDeltaTime, 1f); Tf = Mathf.Clamp((float)Tf, (float)TfMin, (float)TfMax); } setPIDParameters(); }
// Calculations of Tf are not safe during FlightComputer constructor // Probably because the ship is only half-initialized... public void updatePIDParameters() { if (Vessel != null) { Vector3d torque = SteeringHelper.GetTorque(Vessel, Vessel.ctrlState != null ? Vessel.ctrlState.mainThrottle : 0.0f); var CoM = Vessel.findWorldCenterOfMass(); var MoI = Vessel.findLocalMOI(CoM); Vector3d ratio = new Vector3d( torque.x != 0 ? MoI.x / torque.x : 0, torque.y != 0 ? MoI.y / torque.y : 0, torque.z != 0 ? MoI.z / torque.z : 0 ); Tf = Mathf.Clamp((float)ratio.magnitude / 20f, 2 * TimeWarp.fixedDeltaTime, 1f); Tf = Mathf.Clamp((float)Tf, (float)TfMin, (float)TfMax); } initPIDParameters(); }
public void OnFixedUpdate() { if (Vessel != null) { UpdateStateVectors(Vessel, Target); SteeringHelper.AnalyzeParts(Vessel); Vector3 Torque = SteeringHelper.TorqueAvailable; var CoM = Vessel.CoM; var MoI = Vessel.MOI; phiTotal = calculatePhiTotal(); phiVector = calculatePhiVector();//deviation errors from orientation target for (int i = 0; i < 3; i++) { //Edge case: Very low (torque/MoI) (like 0.0078!) rate so need to rise max acceleration artifically StoppingTime = (OmegaThreshold <= (Torque[i] / MoI[i])) ? 1.0f : (float)RTUtil.Clamp((1.0 / (Torque[i] / MoI[i])) * (Math.Abs(phiVector[i]) - Phi1FStoppingTime), 1.0, MaxStoppingTime); MaxOmega[i] = Mathf.Max((Torque[i] * StoppingTime) / MoI[i], 0.0001f); } 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], Vessel.MOI[0], Torque[0]); TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]); TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]); } }
public static void HoldOrientation(FlightCtrlState fs, FlightComputer f, Quaternion target) { f.Vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false); SteeringHelper.SteerShipToward(target, fs, f); }
/// <summary> /// Single entry point of all Flight Computer orientation holding, including maneuver node. /// </summary> public static void HoldOrientation(FlightCtrlState fs, FlightComputer f, Quaternion target, bool ignoreRoll = false, bool ignorePitch = false, bool ignoreHeading = false) { f.Vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false); SteeringHelper.SteerShipToward(target, fs, f, ignoreRoll, ignorePitch, ignoreHeading); }