/// <summary>
/// Load infos into this object and create a new BaseEvent
/// </summary>
/// <returns>true - loaded successfull</returns>
public override bool Load(ConfigNode n, FlightComputer fc)
{
if(base.Load(n, fc))
{
// deprecated since 1.6.2, we need this for upgrading from 1.6.x => 1.6.2
int PartId = 0;
{
if (n.HasValue("PartId"))
PartId = int.Parse(n.GetValue("PartId"));
}
if (n.HasValue("flightID"))
this.flightID = uint.Parse(n.GetValue("flightID"));
this.Module = n.GetValue("Module");
this.GUIName = n.GetValue("GUIName");
this.Name = n.GetValue("Name");
RTLog.Notify("Try to load an EventCommand from persistent with {0},{1},{2},{3},{4}",
PartId, this.flightID, this.Module, this.GUIName, this.Name);
Part part = null;
var partlist = FlightGlobals.ActiveVessel.parts;
if (this.flightID == 0)
{
// only look with the partid if we've enough parts
if (PartId < partlist.Count)
part = partlist.ElementAt(PartId);
}
else
{
part = partlist.Where(p => p.flightID == this.flightID).FirstOrDefault();
}
if (part == null) return false;
PartModule partmodule = part.Modules[Module];
if (partmodule == null) return false;
BaseEventList eventlist = new BaseEventList(part, partmodule);
if (eventlist.Count <= 0) return false;
this.BaseEvent = eventlist.Where(ba => (ba.GUIName == this.GUIName || ba.name == this.Name)).FirstOrDefault();
return true;
}
return false;
}
public override bool Pop(FlightComputer f)
{
if (this.BaseEvent != null)
{
try
{
// invoke the baseevent
this.BaseEvent.Invoke();
}
catch (Exception invokeException)
{
RTLog.Notify("BaseEvent invokeException by '{0}' with message: {1}",
RTLogLevel.LVL1, this.BaseEvent.guiName, invokeException.Message);
}
}
return false;
}
public override bool Pop(FlightComputer f)
{
if (BaseField != null)
{
try
{
var field = (BaseField as WrappedField);
if (field == null) // we lost the Wrapped field instance, this is due to the fact that the command was loaded from a save
{
if (NewValue != null)
{
var newfield = new WrappedField(BaseField, WrappedField.KspFieldFromBaseField(BaseField));
if(newfield.NewValueFromString(NewValueString))
{
newfield.Invoke();
}
}
}
else
{
// invoke the field value change
field.Invoke();
}
if (UIPartActionController.Instance != null)
UIPartActionController.Instance.UpdateFlight();
}
catch (Exception invokeException)
{
RTLog.Notify("BaseField InvokeAction() by '{0}' with message: {1}",
RTLogLevel.LVL1, this.BaseField.guiName, invokeException.Message);
}
}
return false;
}
public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceFrame frame, FlightAttitude attitude, Quaternion extra)
{
var v = f.Vessel;
var forward = Vector3.zero;
var up = Vector3.zero;
bool ignoreRoll = false;
switch (frame)
{
case ReferenceFrame.Orbit:
ignoreRoll = true;
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.Surface:
ignoreRoll = true;
forward = v.GetSrfVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.North:
up = (v.mainBody.position - v.CoM);
forward = Vector3.ProjectOnPlane(v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM, up);
break;
case ReferenceFrame.Maneuver:
ignoreRoll = true;
if (f.Vessel.patchedConicSolver.maneuverNodes.Count != 0)
{
forward = f.Vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit);
up = (v.mainBody.position - v.CoM);
}
else
{
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
break;
case ReferenceFrame.TargetVelocity:
// f.DelayedTarget may be any ITargetable, including a planet
// Velocity matching only makes sense for vessels and part modules
// Can test for Vessel but not PartModule, so instead test that it's not the third case (CelestialBody)
if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody))
{
forward = v.GetObtVelocity() - f.DelayedTarget.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
else
{
up = (v.mainBody.position - v.CoM);
forward = v.GetObtVelocity();
}
break;
case ReferenceFrame.TargetParallel:
if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody))
{
forward = f.DelayedTarget.GetTransform().position - v.CoM;
up = (v.mainBody.position - v.CoM);
}
else
{
up = (v.mainBody.position - v.CoM);
forward = v.GetObtVelocity();
}
break;
}
Vector3.OrthoNormalize(ref forward, ref up);
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
switch (attitude)
{
case FlightAttitude.Prograde:
break;
case FlightAttitude.Retrograde:
rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up);
break;
case FlightAttitude.NormalPlus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.up);
break;
case FlightAttitude.NormalMinus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.down);
break;
case FlightAttitude.RadialPlus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.right);
break;
case FlightAttitude.RadialMinus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.left);
break;
case FlightAttitude.Surface:
rotationReference = rotationReference * extra;
break;
}
HoldOrientation(fs, f, rotationReference, ignoreRoll);
}
/// <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 = GetTrueMoI(vessel);
Transform vesselReference = vessel.GetTransform();
//---------------------------------------
// Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);
Vector3d torque = GetTorque(vessel, c.mainThrottle);
Vector3d spinMargin = GetStoppingAngle(vessel, torque);
// Allow for zero torque around some but not all axes
Vector3d normFactor;
normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0);
normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0);
normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0);
normFactor = SwapYZ(normFactor);
// Find out the real shorter way to turn were we want to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0f;
var err = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
fixedRoll ?
((delta.eulerAngles.z > 180) ?
(delta.eulerAngles.z - 360.0F) :
delta.eulerAngles.z) * Math.PI / 180.0F
: 0F
);
err += SwapYZ(spinMargin);
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 = SwapYZ(vessel.angularVelocity);
omega.Scale(normFactor);
Vector3d pidAction = fc.pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1));
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);
}
public static void HoldOrientation(FlightCtrlState fs, FlightComputer f, Quaternion target, bool ignoreRoll = false)
{
f.Vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false);
SteeringHelper.SteerShipToward(target, fs, f, ignoreRoll);
}
/// <summary>
/// Save the BaseEvent to the persistent
/// </summary>
public override void Save(ConfigNode n, FlightComputer fc)
{
PartModule pm = (BaseField.host as PartModule);
if(pm == null)
{
RTLog.Notify("On PartActionCommand.Save(): Can't save because BaseField.host is not a PartModule instance. Type is: {0}", BaseField.host.GetType());
return;
}
GUIName = BaseField.guiName;
flightID = pm.part.flightID;
Module = pm.ClassName.ToString();
Name = BaseField.name;
n.AddValue("NewValue", this.NewValue);
base.Save(n, fc);
}
private static void SetPIDParameters(FlightComputer fc, Vector3d TfV, double kdFactor, double kpFactor, double kiFactor)
{
var pid = fc.pid;
Vector3d invTf = TfV.InvertNoNaN();
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);
}
/// <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);
}
/// <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)
{
f.Vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false);
SteeringHelper.SteerShipToward(target, fs, f, ignoreRoll);
}
public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceFrame frame, FlightAttitude attitude, Quaternion extra)
{
var v = f.Vessel;
var forward = Vector3.zero;
var up = Vector3.zero;
bool ignoreRoll = false;
switch (frame)
{
case ReferenceFrame.Orbit:
ignoreRoll = true;
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.Surface:
ignoreRoll = true;
forward = v.GetSrfVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.North:
up = (v.mainBody.position - v.CoM);
forward = Vector3.ProjectOnPlane(v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM, up);
break;
case ReferenceFrame.Maneuver:
ignoreRoll = true;
if (f.Vessel.patchedConicSolver == null) //scenario: two vessels within physical range with FC attitude hold cmds. Unloaded one doesn't have solver instance
{
f.Vessel.AttachPatchedConicsSolver();
f.Vessel.patchedConicSolver.Update();
}
if (f.Vessel.patchedConicSolver.maneuverNodes.Count != 0)
{
forward = f.Vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit);
up = (v.mainBody.position - v.CoM);
}
else
{
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
break;
case ReferenceFrame.TargetVelocity:
// f.DelayedTarget may be any ITargetable, including a planet
// Velocity matching only makes sense for vessels and part modules
// Can test for Vessel but not PartModule, so instead test that it's not the third case (CelestialBody)
if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody))
{
forward = v.GetObtVelocity() - f.DelayedTarget.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
else
{
up = (v.mainBody.position - v.CoM);
forward = v.GetObtVelocity();
}
break;
case ReferenceFrame.TargetParallel:
if (f.DelayedTarget != null) // either target vessel or celestial body
{
forward = f.DelayedTarget.GetTransform().position - v.CoM;
up = (v.mainBody.position - v.CoM);
}
else // no target to aim; default to orbital prograde
{
up = (v.mainBody.position - v.CoM);
forward = v.GetObtVelocity();
}
break;
}
Vector3.OrthoNormalize(ref forward, ref up);
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
switch (attitude)
{
case FlightAttitude.Prograde:
break;
case FlightAttitude.Retrograde:
rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up);
break;
case FlightAttitude.NormalPlus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.up);
break;
case FlightAttitude.NormalMinus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.down);
break;
case FlightAttitude.RadialPlus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.right);
break;
case FlightAttitude.RadialMinus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.left);
break;
case FlightAttitude.Surface:
rotationReference = rotationReference * extra;
break;
}
HoldOrientation(fs, f, rotationReference, ignoreRoll);
}
public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceFrame frame, FlightAttitude attitude, Quaternion extra)
{
var v = f.Vessel;
var forward = Vector3.zero;
var up = Vector3.zero;
switch (frame)
{
case ReferenceFrame.Orbit:
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.Surface:
forward = v.GetSrfVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.North:
up = (v.mainBody.position - v.CoM);
forward = Vector3.Exclude(up,
v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM
);
break;
case ReferenceFrame.Maneuver:
if (f.Vessel.patchedConicSolver.maneuverNodes.Count != 0)
{
forward = f.Vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit);
up = (v.mainBody.position - v.CoM);
}
else
{
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
break;
case ReferenceFrame.TargetVelocity:
// f.DelayedTarget may be any ITargetable, including a planet
// Velocity matching only makes sense for vessels and part modules
// Can test for Vessel but not PartModule, so instead test that it's not the third case (CelestialBody)
if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody))
{
forward = v.GetObtVelocity() - f.DelayedTarget.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
else
{
up = (v.mainBody.position - v.CoM);
forward = v.GetObtVelocity();
}
break;
case ReferenceFrame.TargetParallel:
if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody))
{
forward = f.DelayedTarget.GetTransform().position - v.CoM;
up = (v.mainBody.position - v.CoM);
}
else
{
up = (v.mainBody.position - v.CoM);
forward = v.GetObtVelocity();
}
break;
}
Vector3.OrthoNormalize(ref forward, ref up);
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
switch (attitude)
{
case FlightAttitude.Prograde:
break;
case FlightAttitude.Retrograde:
rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up);
break;
case FlightAttitude.NormalPlus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.up);
break;
case FlightAttitude.NormalMinus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.down);
break;
case FlightAttitude.RadialPlus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.right);
break;
case FlightAttitude.RadialMinus:
rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.left);
break;
case FlightAttitude.Surface:
rotationReference = rotationReference * extra;
break;
}
HoldOrientation(fs, f, rotationReference);
}
/// <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>
public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc)
{
// Add support for roll-less targets later -- Starstrider42
bool fixedRoll = true;
Vessel vessel = fc.Vessel;
Vector3d momentOfInertia = GetTrueMoI(vessel);
Transform vesselReference = vessel.GetTransform();
//---------------------------------------
// Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);
Vector3d torque = GetTorque(vessel, c.mainThrottle);
Vector3d spinMargin = GetStoppingAngle(vessel, torque);
// Allow for zero torque around some but not all axes
Vector3d normFactor;
normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0);
normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0);
normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0);
normFactor = SwapYZ(normFactor);
// Find out the real shorter way to turn were we want to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0f;
var err = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
fixedRoll ?
((delta.eulerAngles.z > 180) ?
(delta.eulerAngles.z - 360.0F) :
delta.eulerAngles.z) * Math.PI / 180.0F
: 0F
);
err += SwapYZ(spinMargin);
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 = SwapYZ(vessel.angularVelocity);
omega.Scale(normFactor);
Vector3d pidAction = fc.pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1));
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);
}
/// <summary>
/// Save the BaseEvent to the persistent
/// </summary>
public override void Save(ConfigNode n, FlightComputer fc)
{
this.GUIName = this.BaseEvent.GUIName;
this.flightID = this.BaseEvent.listParent.module.part.flightID;
this.Module = this.BaseEvent.listParent.module.ClassName.ToString();
this.Name = this.BaseEvent.name;
base.Save(n, fc);
}
/// <summary>
/// Load infos into this object and create a new BaseEvent
/// </summary>
/// <returns>true - loaded successfull</returns>
public override bool Load(ConfigNode n, FlightComputer fc)
{
if (base.Load(n, fc))
{
// deprecated since 1.6.2, we need this for upgrading from 1.6.x => 1.6.2
int PartId = 0;
{
if (n.HasValue("PartId"))
{
PartId = int.Parse(n.GetValue("PartId"));
}
}
if (n.HasValue("flightID"))
{
this.flightID = uint.Parse(n.GetValue("flightID"));
}
this.Module = n.GetValue("Module");
this.GUIName = n.GetValue("GUIName");
this.Name = n.GetValue("Name");
RTLog.Notify("Try to load an EventCommand from persistent with {0},{1},{2},{3},{4}",
PartId, this.flightID, this.Module, this.GUIName, this.Name);
Part part = null;
var partlist = FlightGlobals.ActiveVessel.parts;
if (this.flightID == 0)
{
// only look with the partid if we've enough parts
if (PartId < partlist.Count)
{
part = partlist.ElementAt(PartId);
}
}
else
{
part = partlist.Where(p => p.flightID == this.flightID).FirstOrDefault();
}
if (part == null)
{
return(false);
}
PartModule partmodule = part.Modules[Module];
if (partmodule == null)
{
return(false);
}
BaseEventList eventlist = new BaseEventList(part, partmodule);
if (eventlist.Count <= 0)
{
return(false);
}
this.BaseEvent = eventlist.Where(ba => (ba.GUIName == this.GUIName || ba.name == this.Name)).FirstOrDefault();
return(true);
}
return(false);
}
