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