public Apoapsis(Processor p)
: base(p)
{
}
public override void rawInvoke(FlightCtrlState fcs, Processor p, bool b)
{
p.parentVessel.ActionGroups.SetGroup(KSPActionGroup.Abort, b);
}
public static void HoldOrientation(FlightCtrlState fcs, Processor p, Quaternion target, bool ignoreRoll = false)
{
p.parentVessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false);
SteeringHelper.SteerShipToward(target, fcs, p, ignoreRoll);
}
public void considerWakeup(Processor p)
{
try
{
if (mAST.mToken.Value != Tokens.TOK_KEYWORD || !mAST.mToken.Key.Equals("ON"))
throw new ExecError("expected ON, saw " + mAST.mToken.Key);
if (mAST.mChildren.Count < 2)
throw new ExecError("ON missing HIBERNATE");
ASTNode h = mAST.mChildren[1];
if (h.mToken.Value != Tokens.TOK_KEYWORD || !h.mToken.Key.Equals("HIBERNATE"))
throw new ExecError("expected HIBERNATE, saw " + h.mToken.Key);
if (h.mChildren.Count < 1)
throw new ExecError("HIBERNATE missing wakeup condition");
Value v = evalRecursive(h.mChildren[0], p);
assertType(h, "wakeup", Type.BOOLEAN, v);
if (v.b)
p.wakeup();
}
catch (EvalError exc)
{
Logging.Message("EvalError: " + exc.ToString());
p.error = true;
p.wakeup(); // leave hibernation
skip = true; // and don't let this line put us back in
}
catch (ExecError exc)
{
Logging.Message("ExecError: " + exc.ToString());
p.error = true;
p.wakeup(); // leave hibernation
skip = true; // and don't let this line put us back in
}
}
public Value evalRecursive(ASTNode n, Processor p)
{
Value left, right;
switch(n.mToken.Value)
{
case Tokens.TOK_ARITH_OP: // + - * /
left = evalRecursive(n.mChildren[0], p);
right = evalRecursive(n.mChildren[1], p);
if ((left.typ == Type.ANGLE) && (right.typ == Type.ANGLE))
{
if (n.mToken.Key.Equals("+"))
return new Value((left.d + right.d) % 360.0, true);
else if (n.mToken.Key.Equals("-"))
return new Value((left.d + 360.0 - right.d) % 360.0, true);
else if (n.mToken.Key.Equals("*"))
throw new EvalError("'*' not valid on angles");
else if (n.mToken.Key.Equals("/"))
throw new EvalError("'/' not valid on angles");
else // can't happen
throw new EvalError(n.mToken.Key);
}
else
{
if (left.typ == Type.ANGLE)
left = new Value(left.d);
else if (right.typ == Type.ANGLE)
right = new Value(right.d);
assertType(n, "left", Type.DOUBLE, left);
assertType(n, "right", Type.DOUBLE, right);
if (n.mToken.Key.Equals("+"))
return new Value(left.d + right.d);
else if (n.mToken.Key.Equals("-"))
return new Value(left.d - right.d);
else if (n.mToken.Key.Equals("*"))
return new Value(left.d * right.d);
else if (n.mToken.Key.Equals("/"))
return new Value(left.d / right.d);
else // can't happen
throw new EvalError(n.mToken.Key);
}
case Tokens.TOK_AT:
left = evalRecursive(n.mChildren[0], p);
right = evalRecursive(n.mChildren[1], p);
assertType(n, "left", Type.NAME, left);
return exec(left.n, right, p);
case Tokens.TOK_COMMA:
left = evalRecursive(n.mChildren[0], p);
right = evalRecursive(n.mChildren[1], p);
return new Value(left, right);
case Tokens.TOK_COMP_OP: // < >
left = evalRecursive(n.mChildren[0], p);
right = evalRecursive(n.mChildren[1], p);
if (left.typ == Type.ANGLE && right.typ == Type.ANGLE)
{
double diff = left.d - right.d;
if (diff > 180.0)
diff -= 360.0;
else if (diff < -180.0)
diff += 360.0;
if (n.mToken.Key.Equals("<"))
return new Value(diff < 0);
else if (n.mToken.Key.Equals(">"))
return new Value(diff > 0);
else // can't happen
throw new EvalError(n.mToken.Key);
}
else
{
if (left.typ == Type.ANGLE)
left = new Value(left.d);
else if (right.typ == Type.ANGLE)
right = new Value(right.d);
assertType(n, "left", Type.DOUBLE, left);
assertType(n, "right", Type.DOUBLE, right);
if (n.mToken.Key.Equals("<"))
return new Value(left.d < right.d);
else if (n.mToken.Key.Equals(">"))
return new Value(left.d > right.d);
else // can't happen
throw new EvalError(n.mToken.Key);
}
case Tokens.TOK_IDENT:
if (n.mToken.Key.Equals("true"))
return new Value(true);
if (n.mToken.Key.Equals("false"))
return new Value(false);
if (n.mToken.Key.Equals("error"))
{
bool b = p.error;
p.error = false;
return new Value(b);
}
if (p.inputValues.ContainsKey(n.mToken.Key))
{
return new Value(p.inputValues[n.mToken.Key]);
}
return new Value(n.mToken.Key);
case Tokens.TOK_KEYWORD: // ON DO IF THEN
if (n.mToken.Key.Equals("ON"))
{
Value cond = evalRecursive(n.mChildren[0], p);
assertType(n, "cond", Type.BOOLEAN, cond);
if (mGlitched)
cond = new Value(!cond.b);
if (cond.b && !lastValue)
{
Logging.Log("edge fired! " + mText);
if (TimeWarp.CurrentRateIndex > 0)
TimeWarp.SetRate(0, true); // force 1x speed
try
{
evalRecursive(n.mChildren[1], p);
}
catch(Exception ex)
{
Logging.Message(ex.ToString());
}
}
lastValue = cond.b;
return new Value();
}
if (n.mToken.Key.Equals("IF"))
{
Value cond = evalRecursive(n.mChildren[0], p);
assertType(n, "cond", Type.BOOLEAN, cond);
if (mGlitched)
cond = new Value(!cond.b);
if (cond.b)
{
evalRecursive(n.mChildren[1], p);
}
return new Value();
}
if (n.mToken.Key.Equals("DO"))
return evalRecursive(n.mChildren[0], p);
if (n.mToken.Key.Equals("THEN"))
return evalRecursive(n.mChildren[0], p);
if (n.mToken.Key.Equals("HIBERNATE"))
{
p.hibernate(this);
return new Value();
}
// can't happen
throw new EvalError(n.mToken.Key);
case Tokens.TOK_LITERAL:
try
{
if (n.mToken.Key.EndsWith("~"))
return new Value(Double.Parse(n.mToken.Key.TrimEnd('~')), true);
else
return new Value(Double.Parse(n.mToken.Key));
}
catch (Exception) // can't happen?
{
throw new EvalError(n.mToken.Key);
}
case Tokens.TOK_LOG_OP: // AND OR
left = evalRecursive(n.mChildren[0], p);
assertType(n, "left", Type.BOOLEAN, left);
if (n.mToken.Key.Equals("AND") && !left.b)
return new Value(false);
else if (n.mToken.Key.Equals("OR") && left.b)
return new Value(true);
right = evalRecursive(n.mChildren[1], p);
assertType(n, "right", Type.BOOLEAN, right);
if (n.mToken.Key.Equals("AND"))
return new Value(left.b && right.b);
else if (n.mToken.Key.Equals("OR"))
return new Value(left.b || right.b);
else // can't happen
throw new EvalError(n.mToken.Key);
case Tokens.TOK_SEMI:
evalRecursive(n.mChildren[0], p);
evalRecursive(n.mChildren[1], p);
return new Value();
case Tokens.TOK_UN_OP: // !
if (!n.mToken.Key.Equals("!")) // can't happen
throw new EvalError(n.mToken.Key);
left = evalRecursive(n.mChildren[0], p);
assertType(n, "child", Type.BOOLEAN, left);
return new Value(!left.b);
case Tokens.TOK_COMMENT:
return new Value();
default: // can't happen
throw new EvalError(n.mToken.Value.ToString());
}
}
public virtual void rawInvoke(FlightCtrlState fcs, Processor p, bool b)
{
}
public Heading(Processor p)
{
mProc = p;
}
public Batteries(Processor p)
{
mProc = p;
}
public SrfHeight(Processor p)
: base(p)
{
}
public SensorDriven(Processor p)
{
mProc = p;
}
public override void rawInvoke(FlightCtrlState fcs, Processor p, bool b)
{
foreach (Part part in p.parentVessel.Parts)
{
foreach (ModuleDeployableSolarPanel sp in part.FindModulesImplementing<ModuleDeployableSolarPanel>())
{
if (b)
sp.Extend();
else
sp.Retract();
}
}
}
public SensorDouble(Processor p)
: base(p)
{
}
public override void rawInvoke(FlightCtrlState fcs, Processor p, bool b)
{
foreach (Part part in p.parentVessel.Parts)
{
foreach (RemoteTech.Modules.ModuleRTAntenna ant in part.FindModulesImplementing<RemoteTech.Modules.ModuleRTAntenna>())
{
if (b)
ant.ActionOpen(new KSPActionParam(KSPActionGroup.None, KSPActionType.Activate));
else
ant.ActionClose(new KSPActionParam(KSPActionGroup.None, KSPActionType.Deactivate));
}
}
}
public void Invoke(FlightCtrlState fcs, Processor p)
{
setTo(mValue + mSlewRate * TimeWarp.deltaTime);
fcs.wheelSteer = (float)mValue / 100.0f;
mSlewRate = 0;
}
public void Invoke(FlightCtrlState fcs, Processor p)
{
if (mChange)
rawInvoke(fcs, p, mValue);
mChange = false;
}
public SrfSpeed(Processor p)
: base(p)
{
}
public virtual bool rawGet(FlightCtrlState fcs, Processor p)
{
return false;
}
public SrfVerticalSpeed(Processor p)
: base(p)
{
}
public Gear(Processor p)
: base(p)
{
}
public void Invoke(FlightCtrlState fcs, Processor p)
{
if (mQueued > 0)
{
Staging.ActivateNextStage();
mQueued--;
}
}
public Inclination(Processor p)
: base(p)
{
}
public void Invoke(FlightCtrlState fcs, Processor p)
{
setTo(mValue + mSlewRate * TimeWarp.deltaTime);
fcs.mainThrottle = Math.Max((float)mValue / 100.0f, fcs.mainThrottle);
mSlewRate = 0;
}
public void eval(Processor p)
{
if (!skip)
{
try
{
evalRecursive(mAST, p);
}
catch (EvalError exc)
{
Logging.Message("EvalError: " + exc.ToString());
p.error = true;
skip = true;
}
catch (ExecError exc)
{
Logging.Message("ExecError: " + exc.ToString());
p.error = true;
skip = true;
}
}
mGlitched = false;
}
public TimerIO(Processor p, int index)
{
mIndex = index;
}
public Value exec(string name, Value arglist, Processor p)
{
List<Value> args = arglist.l;
var match = System.Text.RegularExpressions.Regex.Match(name, "^(.+)\\.([^.]+)");
string error;
if (match.Success)
{
string mainName = match.Groups[1].Value;
string method = match.Groups[2].Value;
if (p.outputs.ContainsKey(mainName))
{
IOutputData output = p.outputs[mainName];
if (method.Equals("set"))
{
output.setValue(arglist);
return new Value();
}
if (method.Equals("incr") && arglist.typ == Type.DOUBLE)
{
output.slewValue(arglist);
return new Value();
}
if (method.Equals("decr") && arglist.typ == Type.DOUBLE)
{
output.slewValue(new Value(-arglist.d));
return new Value();
}
error = "no such method";
}
else
{
error = "no such output";
}
}
else
{
error = "method expected";
}
string s = error + ": " + name + ":";
foreach (Value arg in args)
s += " " + arg.ToString();
throw new ExecError(s);
}
public void Invoke(FlightCtrlState fcs, Processor p)
{
}
public static void HoldAttitude(FlightCtrlState fcs, Processor p, FlightAttitude fa)
{
Vessel v = p.parentVessel;
var forward = Vector3.zero;
var up = Vector3.zero;
bool ignoreRoll = false;
switch (fa.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.Maneuver:
ignoreRoll = true;
if (v.patchedConicSolver.maneuverNodes.Count != 0)
{
forward = v.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.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;
}
Vector3.OrthoNormalize(ref forward, ref up);
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
switch (fa.attitude)
{
case RelativeAttitude.Prograde:
break;
case RelativeAttitude.Retrograde:
rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up);
break;
case RelativeAttitude.Vertical:
ignoreRoll = true;
up = v.mainBody.transform.up;
forward = (v.CoM - v.mainBody.position);
Vector3.OrthoNormalize(ref forward, ref up);
rotationReference = Quaternion.LookRotation(forward, up);
if (fa.frame == ReferenceFrame.Surface)
{
forward = rotationReference * v.terrainNormal;
up = v.mainBody.transform.up;
Vector3.OrthoNormalize(ref forward, ref up);
rotationReference = Quaternion.LookRotation(forward, up);
}
break;
case RelativeAttitude.CustomHP:
Quaternion hp = Quaternion.Euler(new Vector3d(Double.IsNaN(fa.Pitch) ? 0 : fa.Pitch,
Double.IsNaN(fa.Hdg) ? 0 : -fa.Hdg,
0));
rotationReference = rotationReference * hp;
ignoreRoll = true;
break;
case RelativeAttitude.CustomHPR:
Quaternion hpr = Quaternion.Euler(new Vector3d(Double.IsNaN(fa.Pitch) ? 0 : fa.Pitch,
Double.IsNaN(fa.Hdg) ? 0 : -fa.Hdg,
Double.IsNaN(fa.Roll) ? 0 : 180 - fa.Roll));
rotationReference = rotationReference * hpr;
break;
}
HoldOrientation(fcs, p, rotationReference, ignoreRoll);
}
public VesselTMR(Processor p)
{
mProc = p;
}
/// <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="p">The Processor carrying out the slew</param>
/// <param name="ignoreRoll">[optional] to ignore the roll</param>
public static void SteerShipToward(Quaternion target, FlightCtrlState c, Processor p, bool ignoreRoll)
{
Vessel vessel = p.parentVessel;
// Add support for roll-less targets later -- Starstrider42
bool fixedRoll = !ignoreRoll;
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 = Quaternion.Inverse(vesselReference.transform.rotation) * 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 = p.pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = p.lastAct + (pidAction - p.lastAct) * (1 / ((p.Tf / TimeWarp.fixedDeltaTime) + 1));
p.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 PeriLongitude(Processor p)
: base(p)
{
}
