public override void Drive(FlightCtrlState s) { if (useSAS) { Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0); if (!part.vessel.ActionGroups[KSPActionGroup.SAS]) { part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true); part.vessel.VesselSAS.LockHeading(target); lastSAS = target; } else if (Quaternion.Angle(lastSAS, target) > 10) { part.vessel.VesselSAS.LockHeading(target); lastSAS = target; } else { part.vessel.VesselSAS.LockHeading(target, true); } } else { // Direction we want to be facing Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget; Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * target); Vector3d deltaEuler = new Vector3d( (delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x, -((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y), (delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z ); Vector3d torque = new Vector3d( vesselState.torqueAvailable.x + vesselState.torqueThrustPYAvailable * s.mainThrottle, vesselState.torqueAvailable.y, vesselState.torqueAvailable.z + vesselState.torqueThrustPYAvailable * s.mainThrottle ); Vector3d inertia = Vector3d.Scale( vesselState.angularMomentum.Sign(), Vector3d.Scale( Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum), Vector3d.Scale(torque, vesselState.MoI).Invert() ) ); // ( MoI / avaiable torque ) factor: Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132); // angular error: Vector3d err = deltaEuler * Math.PI / 180.0F; err += inertia.Reorder(132) / 2; err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(NormFactor); // angular velocity: Vector3d omega; omega.x = vessel.angularVelocity.x; omega.y = vessel.angularVelocity.z; // y <=> z omega.z = vessel.angularVelocity.y; // z <=> y omega.Scale(NormFactor); pidAction = pid.Compute(err, omega); // low pass filter, wf = 1/Tf: Vector3d act = lastAct + (pidAction - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1)); lastAct = act; SetFlightCtrlState(act, deltaEuler, s, 1); act = new Vector3d(s.pitch, s.yaw, s.roll); } }
// used to set PID parameters. public void setPIDParameters() { Vector3d TfV = new Vector3d(0.3, 0.3, 0.3); Vector3d invTf = TfV.Invert(); pid.Kd = kdFactor * invTf; pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * pid.Kd; pid.Kp.Scale(invTf); pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * pid.Kp; pid.Ki.Scale(invTf); pid.intAccum = pid.intAccum.Clamp(-5, 5); }
protected override void WindowGUI(int windowID) { GUILayout.BeginVertical(); core.GetComputerModule<MechJebModuleCustomWindowEditor>().registry.Find(i => i.id == "Toggle:AttitudeController.useSAS").DrawItem(); if (!core.attitude.useSAS) { core.attitude.Tf_autoTune = GUILayout.Toggle(core.attitude.Tf_autoTune, " Tf auto tunning"); if (!core.attitude.Tf_autoTune) { GUILayout.Label("Larger ship do better with a larger Tf"); GuiUtils.SimpleTextBox("Tf (s)", Tf); Tf = Math.Max(0.01, Tf); } else { // pid.Kd = kpFactor / Tf; // pid.Kp = pid.Kd / (kiFactor * Math.Sqrt(2) * Tf); // pid.Ki = pid.Kp / (kpFactor * Math.Sqrt(2) * Tf); GUILayout.BeginHorizontal(); GUILayout.Label("Tf", GUILayout.ExpandWidth(true)); GUILayout.Label(core.attitude.Tf.ToString("F3"), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("Tf range"); GuiUtils.SimpleTextBox("min", TfMin, "", 50); TfMin = Math.Max(TfMin, 0.01); GuiUtils.SimpleTextBox("max", TfMax, "", 50); TfMax = Math.Max(TfMax, 0.01); GUILayout.EndHorizontal(); GUILayout.Label("PID factors"); GuiUtils.SimpleTextBox("Kd = ", kdFactor, " / Tf", 50); kdFactor = Math.Max(kdFactor, 0.01); GuiUtils.SimpleTextBox("Kp = pid.Kd / (", kpFactor, " * Math.Sqrt(2) * Tf)", 50); kpFactor = Math.Max(kpFactor, 0.01); GuiUtils.SimpleTextBox("Ki = pid.Kp / (", kiFactor, " * Math.Sqrt(2) * Tf)", 50); kiFactor = Math.Max(kiFactor, 0.01); } core.attitude.RCS_auto = GUILayout.Toggle(core.attitude.RCS_auto, " RCS auto mode"); core.rcs.rcsThrottle = GUILayout.Toggle(core.rcs.rcsThrottle, " RCS throttle when 0k thrust"); GUILayout.BeginHorizontal(); GUILayout.Label("Kp, Ki, Kd", GUILayout.ExpandWidth(true)); GUILayout.Label(core.attitude.pid.Kp.ToString("F3") + ", " + core.attitude.pid.Ki.ToString("F3") + ", " + core.attitude.pid.Kd.ToString("F3"), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("prop. action.", GUILayout.ExpandWidth(true)); GUILayout.Label(MuUtils.PrettyPrint(core.attitude.pid.propAct), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("deriv. action", GUILayout.ExpandWidth(true)); GUILayout.Label(MuUtils.PrettyPrint(core.attitude.pid.derivativeAct), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("integral action.", GUILayout.ExpandWidth(true)); GUILayout.Label(MuUtils.PrettyPrint(core.attitude.pid.intAccum), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("PID Action", GUILayout.ExpandWidth(true)); GUILayout.Label(MuUtils.PrettyPrint(core.attitude.pidAction), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("AttitudeRollMatters ", GUILayout.ExpandWidth(true)); GUILayout.Label(core.attitude.attitudeRollMatters ? "true" : "false", GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); Vector3d torque = new Vector3d( vesselState.torqueAvailable.x + vesselState.torqueThrustPYAvailable * vessel.ctrlState.mainThrottle, vesselState.torqueAvailable.y, vesselState.torqueAvailable.z + vesselState.torqueThrustPYAvailable * vessel.ctrlState.mainThrottle ); GUILayout.BeginHorizontal(); GUILayout.Label("torque", GUILayout.ExpandWidth(true)); GUILayout.Label(MuUtils.PrettyPrint(torque), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("|torque|", GUILayout.ExpandWidth(true)); GUILayout.Label(torque.magnitude.ToString("F3"), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); Vector3d inertia = Vector3d.Scale( vesselState.angularMomentum.Sign(), Vector3d.Scale( Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum), Vector3d.Scale(torque, vesselState.MoI).Invert() ) ); GUILayout.BeginHorizontal(); GUILayout.Label("inertia", GUILayout.ExpandWidth(true)); GUILayout.Label(MuUtils.PrettyPrint(inertia), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("|inertia|", GUILayout.ExpandWidth(true)); GUILayout.Label(inertia.magnitude.ToString("F3"), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); Vector3d ratio = Vector3d.Scale(vesselState.MoI, torque.Invert()); GUILayout.BeginHorizontal(); GUILayout.Label("|MOI| / |Torque|", GUILayout.ExpandWidth(true)); GUILayout.Label(ratio.magnitude.ToString("F3"), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("fixedDeltaTime", GUILayout.ExpandWidth(true)); GUILayout.Label(TimeWarp.fixedDeltaTime.ToString("F3"), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); } GUILayout.EndVertical(); if (!core.attitude.Tf_autoTune) { if (core.attitude.Tf != Tf) { core.attitude.Tf = Tf; core.attitude.setPIDParameters(); } } else { if (core.attitude.TfMin != TfMin || core.attitude.TfMax != TfMax) { core.attitude.TfMin = TfMin; core.attitude.TfMax = TfMax; core.attitude.setPIDParameters(); } if (core.attitude.kpFactor != kpFactor || core.attitude.kiFactor != kiFactor || core.attitude.kdFactor != kdFactor) { core.attitude.kpFactor = kpFactor; core.attitude.kiFactor = kiFactor; core.attitude.kdFactor = kdFactor; core.attitude.setPIDParameters(); } } base.WindowGUI(windowID); }
public void HoldOrientation(FlightCtrlState s, Vessel v, Quaternion orientation, bool roll) { mVesselState.Update(v); // Used in the killRot activation calculation and drive_limit calculation double precision = Math.Max(0.5, Math.Min(10.0, (Math.Min(mVesselState.torqueAvailable.x, mVesselState.torqueAvailable.z) + mVesselState.torqueThrustPYAvailable * s.mainThrottle) * 20.0 / mVesselState.MoI.magnitude)); // Reset the PID controller during roll to keep pitch and yaw errors // from accumulating on the wrong axis. double rollDelta = Mathf.Abs((float)(mVesselState.vesselRoll - lastResetRoll)); if (rollDelta > 180) rollDelta = 360 - rollDelta; if (rollDelta > 5) { mPid.Reset(); lastResetRoll = mVesselState.vesselRoll; } // Direction we want to be facing Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(v.ReferenceTransform.rotation) * orientation); Vector3d deltaEuler = new Vector3d( (delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x, -((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y), (delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z ); Vector3d torque = new Vector3d( mVesselState.torqueAvailable.x + mVesselState.torqueThrustPYAvailable * s.mainThrottle, mVesselState.torqueAvailable.y, mVesselState.torqueAvailable.z + mVesselState.torqueThrustPYAvailable * s.mainThrottle ); Vector3d inertia = Vector3d.Scale( mVesselState.angularMomentum.Sign(), Vector3d.Scale( Vector3d.Scale(mVesselState.angularMomentum, mVesselState.angularMomentum), Vector3d.Scale(torque, mVesselState.MoI).Invert() ) ); Vector3d err = deltaEuler * Math.PI / 180.0F; err += inertia.Reorder(132); err.Scale(Vector3d.Scale(mVesselState.MoI, torque.Invert()).Reorder(132)); Vector3d act = mPid.Compute(err); float drive_limit = Mathf.Clamp01((float)(err.magnitude * drive_factor / precision)); act.x = Mathf.Clamp((float)act.x, drive_limit * -1, drive_limit); act.y = Mathf.Clamp((float)act.y, drive_limit * -1, drive_limit); act.z = Mathf.Clamp((float)act.z, drive_limit * -1, drive_limit); act = lastAct + (act - lastAct) * (TimeWarp.fixedDeltaTime / Tf); SetFlightCtrlState(act, deltaEuler, s, v, precision, drive_limit); act = new Vector3d(s.pitch, s.yaw, s.roll); lastAct = act; stress = Math.Abs(act.x) + Math.Abs(act.y) + Math.Abs(act.z); }
/// <summary> /// Automatically guides the ship to face a desired orientation /// </summary> /// <param name="target">The desired orientation</param> /// <param name="c">The FlightCtrlState for the current vessel.</param> /// <param name="fc">The flight computer carrying out the slew</param> /// <param name="ignoreRoll">[optional] to ignore the roll</param> public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll) { // Add support for roll-less targets later -- Starstrider42 bool fixedRoll = !ignoreRoll; Vessel vessel = fc.Vessel; Vector3d momentOfInertia = vessel.MOI; Transform vesselReference = vessel.GetTransform(); Vector3d torque = GetVesselTorque(vessel); // ----------------------------------------------- // Copied from MechJeb master on 18.04.2016 with some modifications to adapt to RemoteTech Vector3d _axisControl = new Vector3d(); _axisControl.x = true ? 1 : 0; _axisControl.y = true ? 1 : 0; _axisControl.z = fixedRoll ? 1 : 0; Vector3d inertia = Vector3d.Scale( new Vector3d(vessel.angularMomentum.x, vessel.angularMomentum.y, vessel.angularMomentum.z).Sign(), Vector3d.Scale( Vector3d.Scale(vessel.angularMomentum, vessel.angularMomentum), Vector3d.Scale(torque, momentOfInertia).Invert() ) ); Vector3d TfV = new Vector3d(0.3, 0.3, 0.3); double kpFactor = 3; double kiFactor = 6; double kdFactor = 0.5; double kWlimit = 0.15; double deadband = 0.0001; Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target); Vector3d deltaEuler = delta.DeltaEuler(); // ( MoI / available torque ) factor: Vector3d NormFactor = Vector3d.Scale(momentOfInertia, torque.Invert()).Reorder(132); // Find out the real shorter way to turn were we want to. // Thanks to HoneyFox Vector3d tgtLocalUp = vesselReference.rotation.Inverse() * target * Vector3d.forward; Vector3d curLocalUp = Vector3d.up; double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp)); Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z); rotDirection = rotDirection.normalized * turnAngle / 180.0; // And the lowest roll // Thanks to Crzyrndm Vector3 normVec = Vector3.Cross(target * Vector3.forward, vesselReference.up); Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * target; float rollError = Vector3.Angle(vesselReference.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselReference.forward)); var error = new Vector3d( -rotDirection.y * Math.PI, rotDirection.x * Math.PI, rollError * Mathf.Deg2Rad ); error.Scale(_axisControl); Vector3d err = error + inertia.Reorder(132) / 2d; err = new Vector3d( Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(NormFactor); // angular velocity: Vector3d omega; omega.x = vessel.angularVelocity.x; omega.y = vessel.angularVelocity.z; // y <=> z omega.z = vessel.angularVelocity.y; // z <=> y omega.Scale(NormFactor); //if (Tf_autoTune) // tuneTf(torque); Vector3d invTf = TfV.Invert(); fc.pid.Kd = kdFactor * invTf; fc.pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * fc.pid.Kd; fc.pid.Kp.Scale(invTf); fc.pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * fc.pid.Kp; fc.pid.Ki.Scale(invTf); fc.pid.intAccum = fc.pid.intAccum.Clamp(-5, 5); // angular velocity limit: var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit), Math.Sqrt(NormFactor.y * Math.PI * kWlimit), Math.Sqrt(NormFactor.z * Math.PI * kWlimit)); Vector3d pidAction = fc.pid.Compute(err, omega, Wlimit); // deadband pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0; pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0; pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0; // low pass filter, wf = 1/Tf: Vector3d act = fc.lastAct; act.x += (pidAction.x - fc.lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0)); act.y += (pidAction.y - fc.lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0)); act.z += (pidAction.z - fc.lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0)); fc.lastAct = act; // end MechJeb import //--------------------------------------- float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f); float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision)); act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit); act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit); act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit); c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit); c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit); c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit); }