public static string ToStringDecimal(double latitude, double longitude, bool newline = false, int precision = 3)
{
double clampedLongitude = MuUtils.ClampDegrees180(longitude);
double latitudeAbs = Math.Abs(latitude);
double longitudeAbs = Math.Abs(clampedLongitude);
return(latitudeAbs.ToString("F" + precision) + "° " + (latitude > 0 ? "N" : "S") + (newline ? "\n" : ", ")
+ longitudeAbs.ToString("F" + precision) + "° " + (clampedLongitude > 0 ? "E" : "W"));
}
private void UpdatePredictionPI()
{
// velcity relative to the target is the minus of the velocity relative to the vessel
// (The PID moves the vessel to zero in the target frame)
Omega = -ac.vessel.angularVelocity;
UpdateError();
// see https://archive.is/NqoUm and the "Alt Hold Controller", the acceleration PID is not implemented so we only
// have the first two PIDs in the cascade.
for (int i = 0; i < 3; i++)
{
MaxAlpha[i] = ControlTorque[i] / ac.vesselState.MoI[i];
// scale the LD the user inputs to get the actual LD we use
// (this was determined entirely empirically by seeing that vessels with appx 1000x range of MaxAlpha
// needed a roughly 10x different LD, so by scaling here, we produce a user tunable which should be
// fairly constant across a large range of vessels)
double effLD = LD * Math.Pow(MaxAlpha[i], 1.0 / 3.0);
double Gain = Math.Sqrt(0.5 * MaxAlpha[i] / effLD);
if (Math.Abs(errorVector[i]) <= 2 * effLD)
{
// linear ramp down of acceleration
TargetOmega[i] = Gain * errorVector[i];
}
else
{
// v = - sqrt(2 * F * x / m) is target stopping velocity based on distance
TargetOmega[i] = Math.Sqrt(2 * MaxAlpha[i] * (Math.Abs(errorVector[i]) - effLD)) * Math.Sign(errorVector[i]);
}
if (useStoppingTime)
{
MaxOmega[i] = MaxAlpha[i] * maxStoppingTime;
if (useFlipTime)
{
MaxOmega[i] = Math.Max(MaxOmega[i], Math.PI / minFlipTime);
}
TargetOmega[i] = MuUtils.Clamp(TargetOmega[i], -MaxOmega[i], MaxOmega[i]);
}
}
if (useControlRange && errorTotal * Mathf.Rad2Deg > rollControlRange)
{
TargetOmega[1] = 0;
Pid[1].ResetI();
}
for (int i = 0; i < 3; i++)
{
Pid[i].Ki = Ki;
Pid[i].Kp = Kp / TimeWarp.CurrentRate;
Pid[i].Kd = Kd;
Actuation[i] = Pid[i].Update(Omega[i] / MaxAlpha[i], TargetOmega[i] / MaxAlpha[i], 1.0);
TargetTorque[i] = Actuation[i] * ControlTorque[i]; // for display
}
}
public override AutopilotStep Drive(FlightCtrlState s)
{
// primary directions come from trajectories, but we may change them in descend phase to stay on course
targetInfo.update();
switch (phase)
{
case Phase.waitForEntry:
status = "Holding entry attitude, no corrections";
//just hold attitude in this phase
core.attitude.attitudeTo(TrajectoriesConnector.API.PlannedOrientation().Value, AttitudeReference.SURFACE_VELOCITY, this);
if (vesselState.altitudeASL < mainBody.atmosphereDepth)
{
phase = Phase.descend;
}
break;
case Phase.descend:
descend();
if (core.landing.deployChutes &&
vesselState.parachutes.Any(p => p.deploymentSafeState == ModuleParachute.deploymentSafeStates.SAFE && p.deploymentState == ModuleParachute.deploymentStates.STOWED))
{
phase = Phase.parachutes;
core.attitude.attitudeTo(TrajectoriesConnector.API.PlannedOrientation().Value, AttitudeReference.SURFACE_VELOCITY, this);
core.thrust.targetThrottle = 0;
}
break;
case Phase.parachutes:
parachuteInfo.update(vesselState.speedSurface, vesselState.atmosphericDensity);
double dist = Vector3d.Dot(targetInfo.distanceTarget, vesselState.horizontalSurface) - parachuteInfo.breakDistance;
Debug.Log(String.Format("Waiting for parachutes to open Speed:{0:F0} dist:{1:F0} break:{2:F0} maxSpeed:{3:F0} ", vesselState.speedSurface, targetInfo.forwardDistance, dist, parachuteInfo.maxSpeed));
status = "Waiting " + MuUtils.ToSI(dist) + "m with parachutes deploy to hit target";
// control parachute opening
if (dist <= 0)
{
deployParachutes();
}
if (vesselState.parachutes.All(p => p.deploymentState != ModuleParachute.deploymentStates.STOWED))
{
return(new FinalDescent(core));
}
break;
}
if (vesselState.altitudeTrue < 1000 || vesselState.speedSurface < 200)
{
if (core.landing.deployChutes)
{
deployParachutes();
}
return(new FinalDescent(core));
}
return(this);
}
public EditableAngle(double angle)
{
angle = MuUtils.ClampDegrees180(angle);
negative = (angle < 0);
angle = Math.Abs(angle);
degrees = (int)angle;
angle -= degrees;
minutes = (int)(60 * angle);
angle -= minutes / 60;
seconds = Math.Round(3600 * angle);
}
public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
{
double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);
if (o.referenceBody.Radius + newApA < o.Radius(UT))
{
string burnAltitude = MuUtils.ToSI(o.Radius(UT) - o.referenceBody.Radius) + "m";
throw new OperationException("new apoapsis cannot be lower than the altitude of the burn (" + burnAltitude + ")");
}
return(new ManeuverParameters(OrbitalManeuverCalculator.DeltaVToChangeApoapsis(o, UT, newApA + o.referenceBody.Radius), UT));
}
public override AutopilotStep OnFixedUpdate()
{
if (!vessel.patchedConicsUnlocked() || vessel.patchedConicSolver.maneuverNodes.Count == 0)
{
return(doAfterExecution);
}
node = vessel.patchedConicSolver.maneuverNodes[0];
double dVLeft = node.GetBurnVector(orbit).magnitude;
if (dVLeft < core.node.tolerance && core.attitude.attitudeAngleFromTarget() > 5)
{
burnTriggered = false;
node.RemoveSelf();
return(this); // we are done for this frame, continue in next
}
double halfBurnTime;
double burnTime = core.node.BurnTime(dVLeft, out halfBurnTime);
double timeToNode = node.UT - vesselState.time;
status = "Moving to node";
if ((!double.IsInfinity(halfBurnTime) && halfBurnTime > 0 && timeToNode < halfBurnTime) || timeToNode < 0)
{
burnTriggered = true;
status = "Executing node";
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp();
}
}
//autowarp, but only if we're already aligned with the node
if (core.node.autowarp && !burnTriggered)
{
if ((core.attitude.attitudeAngleFromTarget() < 1 && core.vessel.angularVelocity.magnitude < 0.01) || (core.attitude.attitudeAngleFromTarget() < 10 && !MuUtils.PhysicsRunning()))
{
core.warp.WarpToUT(node.UT - halfBurnTime - core.node.leadTime);
}
else if (!MuUtils.PhysicsRunning() && core.attitude.attitudeAngleFromTarget() > 10 && timeToNode < 600)
{
//realign
core.warp.MinimumWarp();
}
}
return(this);
}
public override void GUI()
{
useInertia = GUILayout.Toggle(useInertia, "useInertia");
GUILayout.BeginHorizontal();
GUILayout.Label("MaxStoppingTime", GUILayout.ExpandWidth(false));
maxStoppingTime.text = GUILayout.TextField(maxStoppingTime.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
useControlRange = GUILayout.Toggle(useControlRange, "RollControlRange", GUILayout.ExpandWidth(false));
rollControlRange.text = GUILayout.TextField(rollControlRange.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
// Not used yet
//GuiUtils.SimpleTextBox("Maximum Relative Angular Velocity", kWlimit, "%");
//double tmp_kWlimit = kWlimit;
//tmp_kWlimit = (EditableDouble)GUILayout.HorizontalSlider((float)tmp_kWlimit, 0.0F, 1.0F);
//
//const int sliderPrecision = 3;
//if (Math.Round(Math.Abs(tmp_kWlimit - kWlimit), sliderPrecision) > 0)
//{
// kWlimit.val = Math.Round(tmp_kWlimit, sliderPrecision);
//}
GUILayout.BeginHorizontal();
GUILayout.Label("Actuation", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(Actuation), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("phiVector", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(phiVector), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("TargetTorque", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(TargetTorque), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("ControlTorque", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(ControlTorque), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Inertia", GUILayout.ExpandWidth(true));
GUILayout.Label("|" + ac.inertia.magnitude.ToString("F3") + "| " + MuUtils.PrettyPrint(ac.inertia), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
{
double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);
if (o.referenceBody.Radius + newPeA > o.Radius(UT))
{
string burnAltitude = MuUtils.ToSI(o.Radius(UT) - o.referenceBody.Radius) + "m";
throw new OperationException("new periapsis cannot be higher than the altitude of the burn (" + burnAltitude + ")");
}
else if (newPeA < -o.referenceBody.Radius)
{
throw new OperationException("new periapsis cannot be lower than minus the radius of " + o.referenceBody.displayName + "(-" + MuUtils.ToSI(o.referenceBody.Radius, 3) + "m)");
}
return(new ManeuverParameters(OrbitalManeuverCalculator.DeltaVToChangePeriapsis(o, UT, newPeA + o.referenceBody.Radius), UT));
}
public static Coordinates GetMouseCoordinates(CelestialBody body)
{
Ray mouseRay = PlanetariumCamera.Camera.ScreenPointToRay(Input.mousePosition);
mouseRay.origin = ScaledSpace.ScaledToLocalSpace(mouseRay.origin);
Vector3d relOrigin = mouseRay.origin - body.position;
Vector3d relSurfacePosition;
double curRadius = body.pqsController.radiusMax;
double lastRadius = 0;
double error = 0;
int loops = 0;
float st = Time.time;
while (loops < 50)
{
if (PQS.LineSphereIntersection(relOrigin, mouseRay.direction, curRadius, out relSurfacePosition))
{
Vector3d surfacePoint = body.position + relSurfacePosition;
double alt = body.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(body.GetLongitude(surfacePoint), Vector3d.down) * QuaternionD.AngleAxis(body.GetLatitude(surfacePoint), Vector3d.forward) * Vector3d.right);
error = Math.Abs(curRadius - alt);
if (error < (body.pqsController.radiusMax - body.pqsController.radiusMin) / 100)
{
return(new Coordinates(body.GetLatitude(surfacePoint), MuUtils.ClampDegrees180(body.GetLongitude(surfacePoint))));
}
else
{
lastRadius = curRadius;
curRadius = alt;
loops++;
}
}
else
{
if (loops == 0)
{
break;
}
else
{ // Went too low, needs to try higher
curRadius = (lastRadius * 9 + curRadius) / 10;
loops++;
}
}
}
return(null);
}
public static Coordinates GetMouseCoordinates(CelestialBody body)
{
Ray mouseRay = PlanetariumCamera.Camera.ScreenPointToRay(Input.mousePosition);
mouseRay.origin = ScaledSpace.ScaledToLocalSpace(mouseRay.origin);
Vector3d relOrigin = mouseRay.origin - body.position;
Vector3d relSurfacePosition;
if (PQS.LineSphereIntersection(relOrigin, mouseRay.direction, body.Radius, out relSurfacePosition))
{
Vector3d surfacePoint = body.position + relSurfacePosition;
return(new Coordinates(body.GetLatitude(surfacePoint), MuUtils.ClampDegrees180(body.GetLongitude(surfacePoint))));
}
else
{
return(null);
}
}
public override AutopilotStep OnFixedUpdate()
{
Vector3d targetRadialVector = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0);
Vector3d currentRadialVector = vesselState.CoM - mainBody.position;
double angleToTarget = Vector3d.Angle(targetRadialVector, currentRadialVector);
bool approaching = Vector3d.Dot(targetRadialVector - currentRadialVector, vesselState.orbitalVelocity) > 0;
if (!planeChangeTriggered && approaching && (angleToTarget > 80) && (angleToTarget < 90))
{
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp(true);
}
planeChangeTriggered = true;
}
if (planeChangeTriggered)
{
Vector3d horizontalToTarget = ComputePlaneChange();
Vector3d finalVelocity = Quaternion.FromToRotation(vesselState.horizontalOrbit, horizontalToTarget) * vesselState.orbitalVelocity;
Vector3d deltaV = finalVelocity - vesselState.orbitalVelocity;
//burn normal+ or normal- to avoid dropping the Pe:
Vector3d burnDir = Vector3d.Exclude(vesselState.up, Vector3d.Exclude(vesselState.orbitalVelocity, deltaV));
planeChangeDVLeft = UtilMath.Deg2Rad * Vector3d.Angle(finalVelocity, vesselState.orbitalVelocity) * vesselState.speedOrbitHorizontal;
core.attitude.attitudeTo(burnDir, AttitudeReference.INERTIAL, core.landing);
status = "Executing low orbit plane change of about " + planeChangeDVLeft.ToString("F0") + " m/s";
if (planeChangeDVLeft < 0.1F)
{
return(new LowDeorbitBurn(core));
}
}
else
{
if (core.node.autowarp)
{
core.warp.WarpRegularAtRate((float)(orbit.period / 6));
}
status = "Moving to low orbit plane change burn point";
}
return(this);
}
public void UpdatePhi()
{
Transform vesselTransform = ac.vessel.ReferenceTransform;
// 1. The Euler(-90) here is because the unity transform puts "up" as the pointy end, which is wrong. The rotation means that
// "forward" becomes the pointy end, and "up" and "right" correctly define e.g. AoA/pitch and AoS/yaw. This is just KSP being KSP.
// 2. We then use the inverse ship rotation to transform the requested attitude into the ship frame.
Quaternion deltaRotation = Quaternion.Inverse(vesselTransform.transform.rotation * Quaternion.Euler(-90, 0, 0)) * ac.RequestedAttitude;
// get us some euler angles for the target transform
Vector3d ea = deltaRotation.eulerAngles;
double pitch = ea[0] * UtilMath.Deg2Rad;
double yaw = ea[1] * UtilMath.Deg2Rad;
double roll = ea[2] * UtilMath.Deg2Rad;
// law of cosines for the "distance" of the miss in radians
phiTotal = Math.Acos(MuUtils.Clamp(Math.Cos(pitch) * Math.Cos(yaw), -1, 1));
// this is the initial direction of the great circle route of the requested transform
// (pitch is latitude, yaw is -longitude, and we are "navigating" from 0,0)
Vector3d temp = new Vector3d(Math.Sin(pitch), Math.Cos(pitch) * Math.Sin(-yaw), 0);
temp = temp.normalized * phiTotal;
// we assemble phi in the pitch, roll, yaw basis that vessel.MOI uses (right handed basis)
Vector3d phi = new Vector3d(
MuUtils.ClampRadiansPi(temp[0]), // pitch distance around the geodesic
MuUtils.ClampRadiansPi(roll),
MuUtils.ClampRadiansPi(temp[1]) // yaw distance around the geodesic
);
phi.Scale(ac.AxisState);
if (useInertia)
{
phi -= ac.inertia;
}
phiVector = phi;
}
public override void GUI()
{
if (GUILayout.Button(Localizer.Format("#MechJeb_adv_reset_button")))//"Reset"
{
ResetConfig();
}
Tf_autoTune = GUILayout.Toggle(Tf_autoTune, Localizer.Format("#MechJeb_AttitudeController_checkbox1"));//" Auto-tuning"
GUILayout.BeginHorizontal();
GUILayout.Space(20);
GUILayout.BeginVertical();
if (!Tf_autoTune)
{
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label1"));//"Larger ship do better with a larger Tf"
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label2"), GUILayout.ExpandWidth(true)); //"Tf (s)"
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label3"), GUILayout.ExpandWidth(false)); //"P"
UI_TfX.text = GUILayout.TextField(UI_TfX.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label4"), GUILayout.ExpandWidth(false)); //"Y"
UI_TfY.text = GUILayout.TextField(UI_TfY.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label5"), GUILayout.ExpandWidth(false)); //"R"
UI_TfZ.text = GUILayout.TextField(UI_TfZ.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
GUILayout.EndHorizontal();
UI_TfX = Math.Max(0.01, UI_TfX);
UI_TfY = Math.Max(0.01, UI_TfY);
UI_TfZ = Math.Max(0.01, UI_TfZ);
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label6"), GUILayout.ExpandWidth(true));//"Tf"
GUILayout.Label(MuUtils.PrettyPrint(TfV), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label7"), GUILayout.ExpandWidth(true)); //"Tf range"
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_AttitudeController_label8"), UI_TfMin, "", 50); //"min"
UI_TfMin = Math.Max(UI_TfMin, 0.01);
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_AttitudeController_label9"), UI_TfMax, "", 50); //"max"
UI_TfMax = Math.Max(UI_TfMax, 0.01);
GUILayout.EndHorizontal();
}
GUILayout.EndVertical();
GUILayout.EndHorizontal();
bool newLowPassFilter = GUILayout.Toggle(lowPassFilter, Localizer.Format("#MechJeb_AttitudeController_checkbox2"));//" Low Pass Filter"
if (lowPassFilter != newLowPassFilter)
{
setPIDParameters();
lowPassFilter = newLowPassFilter;
}
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_PIDF")); //"PID factors"
GuiUtils.SimpleTextBox("Kd = ", UI_kdFactor, " / Tf", 50); //
UI_kdFactor = Math.Max(UI_kdFactor, 0.01);
GuiUtils.SimpleTextBox("Kp = pid.Kd / (", UI_kpFactor, " * Math.Sqrt(2) * Tf)", 50);
UI_kpFactor = Math.Max(UI_kpFactor, 0.01);
GuiUtils.SimpleTextBox("Ki = pid.Kp / (", UI_kiFactor, " * Math.Sqrt(2) * Tf)", 50);
UI_kiFactor = Math.Max(UI_kiFactor, 0.01);
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_AttitudeController_PIDFactor1"), UI_deadband, "", 50);//"Deadband = "
deadband = Math.Max(UI_deadband, 0.0);
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_AttitudeController_label11"), kWlimit, "%");//"Maximum Relative Angular Velocity"
double tmp_kWlimit = kWlimit;
tmp_kWlimit = (EditableDouble)GUILayout.HorizontalSlider((float)tmp_kWlimit, 0.0F, 1.0F);
const int sliderPrecision = 3;
if (Math.Round(Math.Abs(tmp_kWlimit - kWlimit), sliderPrecision) > 0)
{
kWlimit = Math.Round(tmp_kWlimit, sliderPrecision);
}
//showInfos = GUILayout.Toggle(showInfos, "Show Numbers");
//if (showInfos)
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label12"), GUILayout.ExpandWidth(true));//"Kp"
GUILayout.Label(MuUtils.PrettyPrint(pid.Kp), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label13"), GUILayout.ExpandWidth(true));//"Ki"
GUILayout.Label(MuUtils.PrettyPrint(pid.Ki), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label14"), GUILayout.ExpandWidth(true));//"Kd"
GUILayout.Label(MuUtils.PrettyPrint(pid.Kd), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label15"), GUILayout.ExpandWidth(true));//"Error"
GUILayout.Label(MuUtils.PrettyPrint(error * Mathf.Rad2Deg), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label16"), GUILayout.ExpandWidth(true));//"prop. action."
GUILayout.Label(MuUtils.PrettyPrint(pid.propAct), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label17"), GUILayout.ExpandWidth(true));//"deriv. action"
GUILayout.Label(MuUtils.PrettyPrint(pid.derivativeAct), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label18"), GUILayout.ExpandWidth(true));//"integral action."
GUILayout.Label(MuUtils.PrettyPrint(pid.intAccum), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label19"), GUILayout.ExpandWidth(true));//"PID Action"
GUILayout.Label(MuUtils.PrettyPrint(pidAction), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label20"), GUILayout.ExpandWidth(true));//"Inertia"
GUILayout.Label("|" + ac.inertia.magnitude.ToString("F3") + "| " + MuUtils.PrettyPrint(ac.inertia),
GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
if (!Tf_autoTune)
{
if (TfV.x != UI_TfX || TfV.y != UI_TfY || TfV.z != UI_TfZ)
{
TfV.x = UI_TfX;
TfV.y = UI_TfY;
TfV.z = UI_TfZ;
setPIDParameters();
}
}
else
{
if (TfMin != UI_TfMin || TfMax != UI_TfMax)
{
TfMin = UI_TfMin;
TfMax = UI_TfMax;
setPIDParameters();
}
}
if (kpFactor != UI_kpFactor || kiFactor != UI_kiFactor || kdFactor != UI_kdFactor)
{
kpFactor = UI_kpFactor;
kiFactor = UI_kiFactor;
kdFactor = UI_kdFactor;
setPIDParameters();
}
}
public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
{
double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);
if (2 * newSMA > o.Radius(UT) + o.referenceBody.sphereOfInfluence)
{
errorMessage = "Warning: new Semi-Major Axis is very large, and may result in a hyberbolic orbit";
}
if (o.Radius(UT) > 2 * newSMA)
{
throw new OperationException("cannot make Semi-Major Axis less than twice the burn altitude plus the radius of " + o.referenceBody.theName + "(" + MuUtils.ToSI(o.referenceBody.Radius, 3) + "m)");
}
return(new ManeuverParameters(OrbitalManeuverCalculator.DeltaVForSemiMajorAxis(o, UT, newSMA), UT));
}
public override AutopilotStep OnFixedUpdate()
{
//Decide when we will start the deorbit burn:
double stoppingDistance = Math.Pow(vesselState.speedSurfaceHorizontal, 2) / (2 * vesselState.limitedMaxThrustAccel);
double triggerDistance = lowDeorbitBurnTriggerFactor * stoppingDistance;
double heightAboveTarget = vesselState.altitudeASL - core.landing.DecelerationEndAltitude();
if (triggerDistance < heightAboveTarget)
{
triggerDistance = heightAboveTarget;
}
//See if it's time to start the deorbit burn:
double rangeToTarget = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).magnitude;
if (!deorbitBurnTriggered && rangeToTarget < triggerDistance)
{
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp(true);
}
deorbitBurnTriggered = true;
}
if (deorbitBurnTriggered)
{
status = Localizer.Format("#MechJeb_LandingGuidance_Status11"); //"Executing low deorbit burn"
}
else
{
status = Localizer.Format("#MechJeb_LandingGuidance_Status12"); //"Moving to low deorbit burn point"
}
//Warp toward deorbit burn if it hasn't been triggerd yet:
if (!deorbitBurnTriggered && core.node.autowarp && rangeToTarget > 2 * triggerDistance)
{
core.warp.WarpRegularAtRate((float)(orbit.period / 6));
}
if (rangeToTarget < triggerDistance && !MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp();
}
//By default, thrust straight back at max throttle
Vector3d thrustDirection = -vesselState.surfaceVelocity.normalized;
lowDeorbitBurnMaxThrottle = 1;
//If we are burning, we watch the predicted landing site and switch to the braking
//burn when the predicted landing site crosses the target. We also use the predictions
//to steer the predicted landing site toward the target
if (deorbitBurnTriggered && core.landing.PredictionReady)
{
//angle slightly left or right to fix any cross-range error in the predicted landing site:
Vector3d horizontalToLandingSite = Vector3d.Exclude(vesselState.up, core.landing.LandingSite - vesselState.CoM).normalized;
Vector3d horizontalToTarget = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).normalized;
const double angleGain = 4;
Vector3d angleCorrection = angleGain * (horizontalToTarget - horizontalToLandingSite);
if (angleCorrection.magnitude > 0.1)
{
angleCorrection *= 0.1 / angleCorrection.magnitude;
}
thrustDirection = (thrustDirection + angleCorrection).normalized;
double rangeToLandingSite = Vector3d.Exclude(vesselState.up, core.landing.LandingSite - vesselState.CoM).magnitude;
double maxAllowedSpeed = core.landing.MaxAllowedSpeed();
if (!lowDeorbitEndConditionSet && Vector3d.Distance(core.landing.LandingSite, vesselState.CoM) < mainBody.Radius + vesselState.altitudeASL)
{
lowDeorbitEndOnLandingSiteNearer = rangeToLandingSite > rangeToTarget;
lowDeorbitEndConditionSet = true;
}
lowDeorbitBurnMaxThrottle = 1;
if (orbit.PeA < 0)
{
if (rangeToLandingSite > rangeToTarget)
{
if (lowDeorbitEndConditionSet && !lowDeorbitEndOnLandingSiteNearer)
{
core.thrust.targetThrottle = 0;
return(new DecelerationBurn(core));
}
double maxAllowedSpeedAfterDt = core.landing.MaxAllowedSpeedAfterDt(vesselState.deltaT);
double speedAfterDt = vesselState.speedSurface + vesselState.deltaT * Vector3d.Dot(vesselState.gravityForce, vesselState.surfaceVelocity.normalized);
double throttleToMaintainLandingSite;
if (vesselState.speedSurface < maxAllowedSpeed)
{
throttleToMaintainLandingSite = 0;
}
else
{
throttleToMaintainLandingSite = (speedAfterDt - maxAllowedSpeedAfterDt) / (vesselState.deltaT * vesselState.maxThrustAccel);
}
lowDeorbitBurnMaxThrottle = throttleToMaintainLandingSite + 1 * (rangeToLandingSite / rangeToTarget - 1) + 0.2;
}
else
{
if (lowDeorbitEndConditionSet && lowDeorbitEndOnLandingSiteNearer)
{
core.thrust.targetThrottle = 0;
return(new DecelerationBurn(core));
}
else
{
lowDeorbitBurnMaxThrottle = 0;
status = Localizer.Format("#MechJeb_LandingGuidance_Status13");//"Deorbit burn complete: waiting for the right moment to start braking"
}
}
}
}
core.attitude.attitudeTo(thrustDirection, AttitudeReference.INERTIAL, core.landing);
return(this);
}
public override AutopilotStep OnFixedUpdate()
{
if (vesselState.altitudeASL < core.landing.DecelerationEndAltitude() + 5)
{
core.warp.MinimumWarp();
if (core.landing.UseAtmosphereToBrake())
{
return(new FinalDescent(core));
}
else
{
return(new KillHorizontalVelocity(core));
}
}
double decelerationStartTime = (core.landing.prediction.trajectory.Any() ? core.landing.prediction.trajectory.First().UT : vesselState.time);
if (!(core.landing.minThrust > 0 && core.thrust.targetThrottle > 0) && decelerationStartTime - vesselState.time > 5)
{
core.thrust.targetThrottle = 0;
status = "Warping to start of braking burn.";
//warp to deceleration start
Vector3d decelerationStartAttitude = -orbit.SwappedOrbitalVelocityAtUT(decelerationStartTime);
decelerationStartAttitude += mainBody.getRFrmVel(orbit.SwappedAbsolutePositionAtUT(decelerationStartTime));
decelerationStartAttitude = decelerationStartAttitude.normalized;
core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, core.landing);
bool warpReady = core.attitude.attitudeAngleFromTarget() < 5;
if (warpReady && core.node.autowarp)
{
core.warp.WarpToUT(decelerationStartTime - 5);
}
else if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp();
}
return(this);
}
Vector3d desiredThrustVector = -vesselState.surfaceVelocity.normalized;
Vector3d courseCorrection = core.landing.ComputeCourseCorrection(false);
double correctionAngle = courseCorrection.magnitude / (2.0 * vesselState.limitedMaxThrustAccel);
correctionAngle = Math.Min(0.1, correctionAngle);
desiredThrustVector = (desiredThrustVector + correctionAngle * courseCorrection.normalized).normalized;
if (Vector3d.Dot(vesselState.surfaceVelocity, vesselState.up) > 0 ||
Vector3d.Dot(vesselState.forward, desiredThrustVector) < 0.75)
{
core.thrust.targetThrottle = (float)core.landing.minThrust;
status = "Braking (wrongdir)";
}
else
{
double controlledSpeed = vesselState.speedSurface * Math.Sign(Vector3d.Dot(vesselState.surfaceVelocity, vesselState.up)); //positive if we are ascending, negative if descending
double desiredSpeed = -core.landing.MaxAllowedSpeed();
double desiredSpeedAfterDt = -core.landing.MaxAllowedSpeedAfterDt(vesselState.deltaT);
double minAccel = -vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.surfaceVelocity.normalized, vesselState.up));
double maxAccel = vesselState.maxThrustAccel * Vector3d.Dot(vesselState.forward, -vesselState.surfaceVelocity.normalized) - vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.surfaceVelocity.normalized, vesselState.up));
const double speedCorrectionTimeConstant = 0.3;
double speedError = desiredSpeed - controlledSpeed;
double desiredAccel = speedError / speedCorrectionTimeConstant + (desiredSpeedAfterDt - desiredSpeed) / vesselState.deltaT;
if (maxAccel - minAccel > 0)
{
core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), (float)core.landing.minThrust, 1.0F);
}
else
{
core.thrust.targetThrottle = (float)core.landing.minThrust;
}
status = "Braking: target speed = " + Math.Abs(desiredSpeed).ToString("F1") + " m/s";
}
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, core.landing);
return(this);
}
public override AutopilotStep OnFixedUpdate()
{
//if we don't want to deorbit but we're already on a reentry trajectory, we can't wait until the ideal point
//in the orbit to deorbt; we already have deorbited.
if (orbit.ApA < mainBody.RealMaxAtmosphereAltitude())
{
core.thrust.targetThrottle = 0;
return(new CourseCorrection(core));
}
//We aim for a trajectory that
// a) has the same vertical speed as our current trajectory
// b) has a horizontal speed that will give it a periapsis of -10% of the body's radius
// c) has a heading that points toward where the target will be at the end of free-fall, accounting for planetary rotation
Vector3d horizontalDV = OrbitalManeuverCalculator.DeltaVToChangePeriapsis(orbit, vesselState.time, 0.9 * mainBody.Radius); //Imagine we are going to deorbit now. Find the burn that would lower our periapsis to -10% of the planet's radius
Orbit forwardDeorbitTrajectory = orbit.PerturbedOrbit(vesselState.time, horizontalDV); //Compute the orbit that would put us on
double freefallTime = forwardDeorbitTrajectory.NextTimeOfRadius(vesselState.time, mainBody.Radius) - vesselState.time; //Find how long that orbit would take to impact the ground
double planetRotationDuringFreefall = 360 * freefallTime / mainBody.rotationPeriod; //Find how many degrees the planet will rotate during that time
Vector3d currentTargetRadialVector = mainBody.GetWorldSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0) - mainBody.position; //Find the current vector from the planet center to the target landing site
Quaternion freefallPlanetRotation = Quaternion.AngleAxis((float)planetRotationDuringFreefall, mainBody.angularVelocity); //Construct a quaternion representing the rotation of the planet found above
Vector3d freefallEndTargetRadialVector = freefallPlanetRotation * currentTargetRadialVector; //Use this quaternion to find what the vector from the planet center to the target will be when we hit the ground
Vector3d freefallEndTargetPosition = mainBody.position + freefallEndTargetRadialVector; //Then find the actual position of the target at that time
Vector3d freefallEndHorizontalToTarget = Vector3d.Exclude(vesselState.up, freefallEndTargetPosition - vesselState.CoM).normalized; //Find a horizontal unit vector that points toward where the target will be when we hit the ground
Vector3d currentHorizontalVelocity = Vector3d.Exclude(vesselState.up, vesselState.orbitalVelocity); //Find our current horizontal velocity
double finalHorizontalSpeed = (currentHorizontalVelocity + horizontalDV).magnitude; //Find the desired horizontal speed after the deorbit burn
Vector3d finalHorizontalVelocity = finalHorizontalSpeed * freefallEndHorizontalToTarget; //Combine the desired speed and direction to get the desired velocity after the deorbi burn
//Compute the angle between the location of the target at the end of freefall and the normal to our orbit:
Vector3d currentRadialVector = vesselState.CoM - mainBody.position;
double targetAngleToOrbitNormal = Vector3d.Angle(orbit.SwappedOrbitNormal(), freefallEndTargetRadialVector);
targetAngleToOrbitNormal = Math.Min(targetAngleToOrbitNormal, 180 - targetAngleToOrbitNormal);
double targetAheadAngle = Vector3d.Angle(currentRadialVector, freefallEndTargetRadialVector); //How far ahead the target is, in degrees
double planeChangeAngle = Vector3d.Angle(currentHorizontalVelocity, freefallEndHorizontalToTarget); //The plane change required to get onto the deorbit trajectory, in degrees
//If the target is basically almost normal to our orbit, it doesn't matter when we deorbit; might as well do it now
//Otherwise, wait until the target is ahead
if (targetAngleToOrbitNormal < 10 ||
(targetAheadAngle < 90 && targetAheadAngle > 60 && planeChangeAngle < 90))
{
deorbitBurnTriggered = true;
}
if (deorbitBurnTriggered)
{
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp();
} //get out of warp
Vector3d deltaV = finalHorizontalVelocity - currentHorizontalVelocity;
core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, core.landing);
if (deltaV.magnitude < 2.0)
{
return(new CourseCorrection(core));
}
status = "Doing high deorbit burn";
}
else
{
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT, core.landing);
if (core.node.autowarp)
{
core.warp.WarpRegularAtRate((float)(orbit.period / 10));
}
status = "Moving to high deorbit burn point";
}
return(this);
}
public override void GUI()
{
GUILayout.BeginHorizontal();
useStoppingTime = GUILayout.Toggle(useStoppingTime, "Maximum Stopping Time", GUILayout.ExpandWidth(false));
maxStoppingTime.text = GUILayout.TextField(maxStoppingTime.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
useFlipTime = GUILayout.Toggle(useFlipTime, "Minimum Flip Time", GUILayout.ExpandWidth(false));
minFlipTime.text = GUILayout.TextField(minFlipTime.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
if (!useStoppingTime)
{
useFlipTime = false;
}
GUILayout.BeginHorizontal();
useControlRange = GUILayout.Toggle(useControlRange, Localizer.Format("#MechJeb_HybridController_checkbox2"), GUILayout.ExpandWidth(false));//"RollControlRange"
rollControlRange.text = GUILayout.TextField(rollControlRange.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("LD", GUILayout.ExpandWidth(false));
LD.text = GUILayout.TextField(LD.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
Kp.text = GUILayout.TextField(Kp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Ki", GUILayout.ExpandWidth(false));
Ki.text = GUILayout.TextField(Ki.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Kd", GUILayout.ExpandWidth(false));
Kd.text = GUILayout.TextField(Kd.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
GUILayout.EndHorizontal();
// Not used yet
//GuiUtils.SimpleTextBox("Maximum Relative Angular Velocity", kWlimit, "%");
//double tmp_kWlimit = kWlimit;
//tmp_kWlimit = (EditableDouble)GUILayout.HorizontalSlider((float)tmp_kWlimit, 0.0F, 1.0F);
//
//const int sliderPrecision = 3;
//if (Math.Round(Math.Abs(tmp_kWlimit - kWlimit), sliderPrecision) > 0)
//{
// kWlimit.val = Math.Round(tmp_kWlimit, sliderPrecision);
//}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_HybridController_label2"), GUILayout.ExpandWidth(true));//"Actuation"
GUILayout.Label(MuUtils.PrettyPrint(Actuation), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Error", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(errorVector), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Omega", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(Omega), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("MaxOmega", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(MaxOmega), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("TargetOmega", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(TargetOmega), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_HybridController_label4"), GUILayout.ExpandWidth(true));//"TargetTorque"
GUILayout.Label(MuUtils.PrettyPrint(TargetTorque), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_HybridController_label5"), GUILayout.ExpandWidth(true));//"ControlTorque"
GUILayout.Label(MuUtils.PrettyPrint(ControlTorque), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("MaxAlpha", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(MaxAlpha), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
void descend()
{
Quaternion courseCorrection = Quaternion.identity;
// we aim for parachute break point with half deploy time for opening, which we can anticipate
breakDiff = targetInfo.backwardDifference - (core.landing.deployChutes ? 0.5D * parachuteInfo.undeployedDistance : 0);
status = String.Format("Holding planned descent attitude, break diff={0:F1}", breakDiff);
String logs = String.Format("Atmo Correction alt:{0:F0}, speed hor:{1:F0}, AoA: {2:F1}, dist:{3:F0}", vesselState.altitudeASL.value, vesselState.speedSurfaceHorizontal.value, vesselState.AoA.value, targetInfo.forwardDistance);
double backwardCorrection = breakDiff / targetInfo.forwardDistance;
logs += String.Format(", target back:{0:F0} corr:{1:F4} break:{2:F0}", targetInfo.backwardDifference, backwardCorrection, breakDiff);
if (targetInfo.isValid)
{
double AoA = TrajectoriesConnector.API.AoA.Value;
//if we are less than 0°-60° turned on entry or 0-30° otherwise, simply turn more or less to hit target
if (!TrajectoriesConnector.API.isBelowEntry() && Math.Abs(backwardCorrection) > 0.02)
{
AoA = MuUtils.Clamp(AoA - Math.Sign(backwardCorrection) * 0.5, 120d, 180d, out aeroClamp, out _);
if (Math.Abs(TrajectoriesConnector.API.AoA.Value - AoA) > 0.1)
{
TrajectoriesConnector.API.AoA = AoA;
logs += String.Format(", changed AoA=>{0:F1}", AoA);
}
}
else if (Math.Abs(backwardCorrection) > 0.02)
{
AoA = MuUtils.Clamp(AoA - Math.Sign(backwardCorrection) * 0.5, 150d, 180d, out aeroClamp, out _);
if (TrajectoriesConnector.API.AoA != AoA)
{
TrajectoriesConnector.API.AoA = AoA;
logs += String.Format(", changed AoA=>{0:F1}", AoA);
}
}
if (aeroClamp && (core.thrust.targetThrottle != 0 || backwardCorrection > 0.03))
{
core.thrust.targetThrottle = Mathf.Clamp01((float)(gee / vesselState.limitedMaxThrustAccel * backwardCorrection)); // set thrust at 1G for 10% over target => early corrections, but slow enough for Trajectories
logs += String.Format(", reverse thrust=>{0:P0}", core.thrust.targetThrottle);
status += ", +THRUST";
}
else if (!aeroClamp)
{
core.thrust.targetThrottle = 0; // safeguard, actually backwardCorrection should be 0 before Angle gets reduced
}
}
float rollForTarget = 0;
// roll plannedOrientation towards target if we have signifikant lift
if (vesselState.lift > 1)
{
rollForTarget = -Mathf.Asin((float)(2.0 * targetInfo.normalDifference / targetInfo.distanceTarget.magnitude
* vesselState.mass * vesselState.speedSurface / (targetInfo.TimeTillImpact * vesselState.lift))) * Mathf.Rad2Deg;
rollForTarget = Mathf.Clamp(rollForTarget, -30, 30);
}
logs += String.Format(", normalDiff: {0:F0}, time: {1:F0}s, rollAngle={2:F1}", targetInfo.normalDifference, targetInfo.TimeTillImpact, rollForTarget);
if (Mathf.Abs(rollForTarget) < 0.0005 || targetInfo.normalDifference < 50)
{
rollForTarget = 0; //neglect very small differences
}
if (Mathf.Abs(rollForTarget) > 0 && TrajectoriesConnector.API.isBelowEntry())
{
status += String.Format(", roll towards target with {0:F1}", rollForTarget);
courseCorrection = Quaternion.AngleAxis(rollForTarget, Vector3.forward);
}
Debug.Log(logs);
Quaternion plannedOrientation = (Quaternion)TrajectoriesConnector.API.PlannedOrientation();
core.attitude.attitudeTo(courseCorrection * plannedOrientation, AttitudeReference.SURFACE_VELOCITY, this);
}
public override void GUI()
{
if (GUILayout.Button("Reset"))
{
ResetConfig();
}
Tf_autoTune = GUILayout.Toggle(Tf_autoTune, " Auto-tuning");
GUILayout.BeginHorizontal();
GUILayout.Space(20);
GUILayout.BeginVertical();
if (!Tf_autoTune)
{
GUILayout.Label("Larger ship do better with a larger Tf");
GUILayout.BeginHorizontal();
GUILayout.Label("Tf (s)", GUILayout.ExpandWidth(true));
GUILayout.Label("P", GUILayout.ExpandWidth(false));
UI_TfX.text = GUILayout.TextField(UI_TfX.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
GUILayout.Label("Y", GUILayout.ExpandWidth(false));
UI_TfY.text = GUILayout.TextField(UI_TfY.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
GUILayout.Label("R", GUILayout.ExpandWidth(false));
UI_TfZ.text = GUILayout.TextField(UI_TfZ.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
GUILayout.EndHorizontal();
UI_TfX = Math.Max(0.01, UI_TfX);
UI_TfY = Math.Max(0.01, UI_TfY);
UI_TfZ = Math.Max(0.01, UI_TfZ);
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label("Tf", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(TfV), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Tf range", GUILayout.ExpandWidth(true));
GuiUtils.SimpleTextBox("min", UI_TfMin, "", 50);
UI_TfMin = Math.Max(UI_TfMin, 0.01);
GuiUtils.SimpleTextBox("max", UI_TfMax, "", 50);
UI_TfMax = Math.Max(UI_TfMax, 0.01);
GUILayout.EndHorizontal();
}
GUILayout.EndVertical();
GUILayout.EndHorizontal();
bool newLowPassFilter = GUILayout.Toggle(lowPassFilter, " Low Pass Filter");
if (lowPassFilter != newLowPassFilter)
{
setPIDParameters();
lowPassFilter = newLowPassFilter;
}
GUILayout.Label("PID factors");
GuiUtils.SimpleTextBox("Kd = ", UI_kdFactor, " / Tf", 50);
UI_kdFactor = Math.Max(UI_kdFactor, 0.01);
GuiUtils.SimpleTextBox("Kp = pid.Kd / (", UI_kpFactor, " * Math.Sqrt(2) * Tf)", 50);
UI_kpFactor = Math.Max(UI_kpFactor, 0.01);
GuiUtils.SimpleTextBox("Ki = pid.Kp / (", UI_kiFactor, " * Math.Sqrt(2) * Tf)", 50);
UI_kiFactor = Math.Max(UI_kiFactor, 0.01);
GuiUtils.SimpleTextBox("Deadband = ", UI_deadband, "", 50);
deadband = Math.Max(UI_deadband, 0.0);
GuiUtils.SimpleTextBox("Maximum Relative Angular Velocity", kWlimit, "%");
double tmp_kWlimit = kWlimit;
tmp_kWlimit = (EditableDouble)GUILayout.HorizontalSlider((float)tmp_kWlimit, 0.0F, 1.0F);
const int sliderPrecision = 3;
if (Math.Round(Math.Abs(tmp_kWlimit - kWlimit), sliderPrecision) > 0)
{
kWlimit = Math.Round(tmp_kWlimit, sliderPrecision);
}
//showInfos = GUILayout.Toggle(showInfos, "Show Numbers");
//if (showInfos)
{
GUILayout.BeginHorizontal();
GUILayout.Label("Kp", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(pid.Kp), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Ki", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(pid.Ki), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Kd", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(pid.Kd), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Error", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(error * Mathf.Rad2Deg), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("prop. action.", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(pid.propAct), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("deriv. action", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(pid.derivativeAct), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("integral action.", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(pid.intAccum), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("PID Action", GUILayout.ExpandWidth(true));
GUILayout.Label(MuUtils.PrettyPrint(pidAction), GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Inertia", GUILayout.ExpandWidth(true));
GUILayout.Label("|" + ac.inertia.magnitude.ToString("F3") + "| " + MuUtils.PrettyPrint(ac.inertia),
GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
if (!Tf_autoTune)
{
if (TfV.x != UI_TfX || TfV.y != UI_TfY || TfV.z != UI_TfZ)
{
TfV.x = UI_TfX;
TfV.y = UI_TfY;
TfV.z = UI_TfZ;
setPIDParameters();
}
}
else
{
if (TfMin != UI_TfMin || TfMax != UI_TfMax)
{
TfMin = UI_TfMin;
TfMax = UI_TfMax;
setPIDParameters();
}
}
if (kpFactor != UI_kpFactor || kiFactor != UI_kiFactor || kdFactor != UI_kdFactor)
{
kpFactor = UI_kpFactor;
kiFactor = UI_kiFactor;
kdFactor = UI_kdFactor;
setPIDParameters();
}
}