public string SuicideBurnCountdown()
{
if (orbit.PeA > 0)
{
return("N/A");
}
double angleFromHorizontal = 90 - Vector3d.Angle(-vesselState.velocityVesselSurface, vesselState.up);
angleFromHorizontal = MuUtils.Clamp(angleFromHorizontal, 0, 90);
double sine = Math.Sin(angleFromHorizontal * Math.PI / 180);
double g = vesselState.localg;
double T = vesselState.limitedMaxThrustAccel;
double effectiveDecel = 0.5 * (-2 * g * sine + Math.Sqrt((2 * g * sine) * (2 * g * sine) + 4 * (T * T - g * g)));
double decelTime = vesselState.speedSurface / effectiveDecel;
Vector3d estimatedLandingSite = vesselState.CoM + 0.5 * decelTime * vesselState.velocityVesselSurface;
double terrainRadius = mainBody.Radius + mainBody.TerrainAltitude(estimatedLandingSite);
double impactTime = 0;
try
{
impactTime = orbit.NextTimeOfRadius(vesselState.time, terrainRadius);
}
catch (ArgumentException)
{
return(GuiUtils.TimeToDHMS(0));
}
return(GuiUtils.TimeToDHMS(impactTime - decelTime / 2 - vesselState.time));
}
//Time during a's next orbit at which object a comes nearest to object b.
//If a is hyperbolic, the examined interval is the next 100 units of mean anomaly.
//This is quite a large segment of the hyperbolic arc. However, for extremely high
//hyperbolic eccentricity it may not find the actual closest approach.
public static double NextClosestApproachTime(this Orbit a, Orbit b, double UT)
{
double closestApproachTime = UT;
double closestApproachDistance = Double.MaxValue;
double minTime = UT;
double interval = a.period;
if (a.eccentricity > 1)
{
interval = 100 / a.MeanMotion(); //this should be an interval of time that covers a large chunk of the hyperbolic arc
}
double maxTime = UT + interval;
const int numDivisions = 20;
for (int iter = 0; iter < 8; iter++)
{
double dt = (maxTime - minTime) / numDivisions;
for (int i = 0; i < numDivisions; i++)
{
double t = minTime + i * dt;
double distance = a.Separation(b, t);
if (distance < closestApproachDistance)
{
closestApproachDistance = distance;
closestApproachTime = t;
}
}
minTime = MuUtils.Clamp(closestApproachTime - dt, UT, UT + interval);
maxTime = MuUtils.Clamp(closestApproachTime + dt, UT, UT + interval);
}
return(closestApproachTime);
}
public static double SuicideBurnCountdown(Orbit orbit, VesselState vesselState, Vessel vessel)
{
if (vesselState.mainBody == null)
{
return(0);
}
if (orbit.PeA > 0)
{
return(Double.PositiveInfinity);
}
double angleFromHorizontal = 90 - Vector3d.Angle(-vessel.srf_velocity, vesselState.up);
angleFromHorizontal = MuUtils.Clamp(angleFromHorizontal, 0, 90);
double sine = Math.Sin(angleFromHorizontal * Math.PI / 180);
double g = vesselState.localg;
double T = vesselState.limitedMaxThrustAccel;
double effectiveDecel = 0.5 * (-2 * g * sine + Math.Sqrt((2 * g * sine) * (2 * g * sine) + 4 * (T * T - g * g)));
double decelTime = vesselState.speedSurface / effectiveDecel;
Vector3d estimatedLandingSite = vesselState.CoM + 0.5 * decelTime * vessel.srf_velocity;
double terrainRadius = vesselState.mainBody.Radius + vesselState.mainBody.TerrainAltitude(estimatedLandingSite);
double impactTime = 0;
try
{
impactTime = orbit.NextTimeOfRadius(vesselState.time, terrainRadius);
}
catch (ArgumentException)
{
return(0);
}
return(impactTime - decelTime / 2 - vesselState.time);
}
public void WarpToUT(double UT, double maxRate = -1)
{
if (UT <= vesselState.time)
{
warpToUT = 0.0;
return;
}
if (maxRate < 0)
{
maxRate = TimeWarp.fetch.warpRates[TimeWarp.fetch.warpRates.Length - 1];
}
double desiredRate = 1.0 * (UT - (vesselState.time + Time.fixedDeltaTime * (float)TimeWarp.CurrentRateIndex));
desiredRate = MuUtils.Clamp(desiredRate, 1, maxRate);
if (!vessel.LandedOrSplashed &&
vesselState.altitudeASL < TimeWarp.fetch.GetAltitudeLimit(1, mainBody))
{
//too low to use any regular warp rates. Use physics warp at a max of x2:
WarpPhysicsAtRate((float)Math.Min(desiredRate, 2));
}
else
{
WarpRegularAtRate((float)desiredRate);
}
warpToUT = UT;
}
/*
* private void ConvertToVTRT(double sma, double ecc, double gamma, out double rt, out double vt)
* {
* // All this math comes from symbolic algebraic equation solving in Matlab:
* // f1 = h == cos(gamma) * r * v
* // f2 = v^2 == -mu*(1/a - 2/r)
* // f3 = h == sqrt(mu * a * ( 1 - e^ 2 ))
* // S = solve(f1, f2, f3)
* // and we pick the higher velocity root closer to the periapsis
* double h = Math.Sqrt( mainBody.gravParameter * sma * ( 1 - ecc * ecc ) );
* double cg = Math.Cos( gamma * UtilMath.Deg2Rad );
* double d1 = sma * ( ecc - 1 ) * ( ecc + 1 );
* double d2 = d1 * h * cg;
* double n1 = h * Math.Sqrt( ecc*ecc + cg*cg - 1 ) + h * cg; // FIXME: should check sqrt for imaginary numbers
* vt = - n1 / d1;
* rt = 2 * sma + n1 * sma * sma * ( 1 - ecc * ecc) / d2;
* }
*/
private void ConvertToVTRT(double sma, double ecc, double attachAlt, out double gammaT, out double rT, out double vT)
{
rT = mainBody.Radius + attachAlt;
double h = Math.Sqrt(mainBody.gravParameter * sma * (1 - ecc * ecc));
vT = Math.Sqrt(mainBody.gravParameter * (2 / rT - 1 / sma));
gammaT = Math.Acos(MuUtils.Clamp(h / (rT * vT), -1, 1)) * UtilMath.Rad2Deg;
}
private void converge()
{
if (p == null)
{
status = PVGStatus.INITIALIZING;
return;
}
if (p.last_success_time > 0)
{
last_success_time = p.last_success_time;
}
if (p.solution == null)
{
/* we have a solver but no solution */
status = PVGStatus.INITIALIZING;
}
else
{
/* we have a solver and have a valid solution */
if (isTerminalGuidance())
{
return;
}
/* hardcoded 10 seconds of terminal guidance */
if (tgo < 10)
{
// drop out of warp for terminal guidance (smaller time ticks => more accuracy)
core.warp.MinimumWarp();
status = PVGStatus.TERMINAL;
return;
}
}
if ((vesselState.time - last_optimizer_time) < MuUtils.Clamp(pvgInterval, 1.00, 30.00))
{
return;
}
// for last 10 seconds of coast phase don't recompute (FIXME: can this go lower? it was a workaround for a bug)
if (p.solution != null && p.solution.arc(vesselState.time).thrust == 0 && p.solution.current_tgo(vesselState.time) < 10)
{
return;
}
p.threadStart(vesselState.time);
//if ( p.threadStart(vesselState.time) )
//Debug.Log("MechJeb: started optimizer thread");
if (status == PVGStatus.INITIALIZING && p.solution != null)
{
status = PVGStatus.CONVERGED;
}
last_optimizer_time = vesselState.time;
}
//Computes the delta-V of the burn required to attain a given periapsis, starting from
//a given orbit and burning at a given UT. Throws an ArgumentException if given an impossible periapsis.
//The computed burn is always horizontal, though this may not be strictly optimal.
public static Vector3d DeltaVToChangePeriapsis(Orbit o, double UT, double newPeR)
{
double radius = o.Radius(UT);
//sanitize input
newPeR = MuUtils.Clamp(newPeR, 0 + 1, radius - 1);
//are we raising or lowering the periapsis?
bool raising = (newPeR > o.PeR);
Vector3d burnDirection = (raising ? 1 : -1) * o.Horizontal(UT);
double minDeltaV = 0;
double maxDeltaV;
if (raising)
{
//put an upper bound on the required deltaV:
maxDeltaV = 0.25;
while (o.PerturbedOrbit(UT, maxDeltaV * burnDirection).PeR < newPeR)
{
maxDeltaV *= 2;
if (maxDeltaV > 100000)
{
break; //a safety precaution
}
}
}
else
{
//when lowering periapsis, we burn horizontally, and max possible deltaV is the deltaV required to kill all horizontal velocity
maxDeltaV = Math.Abs(Vector3d.Dot(o.SwappedOrbitalVelocityAtUT(UT), burnDirection));
}
//now do a binary search to find the needed delta-v
while (maxDeltaV - minDeltaV > 0.01)
{
double testDeltaV = (maxDeltaV + minDeltaV) / 2.0;
double testPeriapsis = o.PerturbedOrbit(UT, testDeltaV * burnDirection).PeR;
if ((testPeriapsis > newPeR && raising) || (testPeriapsis < newPeR && !raising))
{
maxDeltaV = testDeltaV;
}
else
{
minDeltaV = testDeltaV;
}
}
return(((maxDeltaV + minDeltaV) / 2) * burnDirection);
}
void DriveGravityTurn(FlightCtrlState s)
{
//stop the gravity turn when our apoapsis reaches the desired altitude
if (autopilot.autoThrottle && orbit.ApA > autopilot.desiredOrbitAltitude)
{
mode = AscentMode.COAST_TO_APOAPSIS;
return;
}
//if we've fallen below the turn start altitude, go back to vertical ascent
if (IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface))
{
mode = AscentMode.VERTICAL_ASCENT;
return;
}
if (autopilot.autoThrottle)
{
core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, autopilot.desiredOrbitAltitude + mainBody.Radius);
if (core.thrust.targetThrottle < 1.0F)
{
// follow surface velocity to reduce flipping
attitudeTo(srfvelPitch());
status = "Fine tuning apoapsis";
return;
}
}
double desiredFlightPathAngle = FlightPathAngle(vesselState.altitudeASL, vesselState.speedSurface);
if (autopilot.correctiveSteering)
{
double actualFlightPathAngle = Math.Atan2(vesselState.speedVertical, vesselState.speedSurfaceHorizontal) * UtilMath.Rad2Deg;
/* form an isosceles triangle with unit vectors pointing in the desired and actual flight path angle directions and find the length of the base */
double velocityError = 2 * Math.Sin(UtilMath.Deg2Rad * (desiredFlightPathAngle - actualFlightPathAngle) / 2);
double difficulty = vesselState.surfaceVelocity.magnitude * 0.02 / vesselState.ThrustAccel(core.thrust.targetThrottle);
difficulty = MuUtils.Clamp(difficulty, 0.1, 1.0);
double steerOffset = autopilot.correctiveSteeringGain * difficulty * velocityError;
double steerAngle = MuUtils.Clamp(Math.Asin(steerOffset) * UtilMath.Rad2Deg, -30, 30);
desiredFlightPathAngle = MuUtils.Clamp(desiredFlightPathAngle + steerAngle, -90, 90);
}
attitudeTo(desiredFlightPathAngle);
status = "Gravity turn";
}
//Computes the time and dV of a Hohmann transfer injection burn such that at apoapsis the transfer
//orbit passes as close as possible to the target.
//The output burnUT will be the first transfer window found after the given UT.
//Assumes o and target are in approximately the same plane, and orbiting in the same direction.
//Also assumes that o is a perfectly circular orbit (though result should be OK for small eccentricity).
public static Vector3d DeltaVAndTimeForHohmannTransfer(Orbit o, Orbit target, double UT, out double burnUT)
{
double synodicPeriod = o.SynodicPeriod(target);
Vector3d burnDV = Vector3d.zero;
burnUT = UT;
double bestApproachDistance = Double.MaxValue;
double minTime = UT;
double maxTime = UT + 1.1 * synodicPeriod;
int numDivisions = 20;
for (int iter = 0; iter < 8; iter++)
{
double dt = (maxTime - minTime) / numDivisions;
for (int i = 0; i < numDivisions; i++)
{
double t = minTime + i * dt;
Vector3d apsisDirection = -o.SwappedRelativePositionAtUT(t);
double desiredApsis = target.RadiusAtTrueAnomaly(target.TrueAnomalyFromVector(apsisDirection));
double approachDistance;
Vector3d burn;
if (desiredApsis > o.ApR)
{
burn = DeltaVToChangeApoapsis(o, t, desiredApsis);
Orbit transferOrbit = o.PerturbedOrbit(t, burn);
approachDistance = transferOrbit.Separation(target, transferOrbit.NextApoapsisTime(t));
}
else
{
burn = DeltaVToChangePeriapsis(o, t, desiredApsis);
Orbit transferOrbit = o.PerturbedOrbit(t, burn);
approachDistance = transferOrbit.Separation(target, transferOrbit.NextPeriapsisTime(t));
}
if (approachDistance < bestApproachDistance)
{
bestApproachDistance = approachDistance;
burnUT = t;
burnDV = burn;
}
}
minTime = MuUtils.Clamp(burnUT - dt, UT, UT + synodicPeriod);
maxTime = MuUtils.Clamp(burnUT + dt, UT, UT + synodicPeriod);
}
return(burnDV);
}
public void WarpToUT(double UT, double maxRate = 100000)
{
double desiredRate = 1.0 * (UT - vesselState.time);
desiredRate = MuUtils.Clamp(desiredRate, 1, maxRate);
if (!vessel.LandedOrSplashed &&
vesselState.altitudeASL < TimeWarp.fetch.GetAltitudeLimit(1, mainBody))
{
//too low to use any regular warp rates. Use physics warp at a max of x2:
WarpPhysicsAtRate((float)Math.Min(desiredRate, 2));
}
else
{
WarpRegularAtRate((float)desiredRate);
}
}
private void DriveInitiateTurn(FlightCtrlState s)
{
double dt = autopilot.MET - pitchStartTime;
double theta = dt * pitchRate;
double pitch_program = 90 - theta;
double pitch;
if (!mainBody.atmosphere)
{
mode = AscentMode.GUIDANCE;
return;
}
if (pitch_program > srfvelPitch())
{
pitch = srfvelPitch();
status = Localizer.Format("#MechJeb_Ascent_status14", String.Format("{0:F}", pitch - core.guidance.pitch));//Gravity Turn <<1>>° to guidance
}
else
{
pitch = pitch_program;
status = Localizer.Format("#MechJeb_Ascent_status15", String.Format("{0:F}", pitch - core.guidance.pitch));//Pitch program <<1>>° to guidance
}
if (pitch <= core.guidance.pitch && core.guidance.isStable())
{
mode = AscentMode.GUIDANCE;
return;
}
if ((vesselState.maxDynamicPressure > 0) && (vesselState.maxDynamicPressure * 0.90 > vesselState.dynamicPressure))
{
mode = AscentMode.GUIDANCE;
return;
}
if (mainBody.atmosphere && vesselState.maxDynamicPressure > 0)
{
// from 95% to 90% of dynamic pressure apply a "fade" from the pitch selected above to the guidance pitch
double fade = MuUtils.Clamp((0.95 - vesselState.dynamicPressure / vesselState.maxDynamicPressure) * 20.0, 0.0, 1.0);
pitch = fade * core.guidance.pitch + (1.0 - fade) * pitch;
}
attitudeTo(pitch, core.guidance.heading);
}
void DriveGravityTurn(FlightCtrlState s)
{
//stop the intermediate "burn" when our apoapsis reaches the desired altitude
if (orbit.ApA > autopilot.desiredOrbitAltitude)
{
mode = AscentMode.COAST_TO_APOAPSIS;
return;
}
if (fixedTimeToAp() < holdAPTime && maxholdAPTime > holdAPTime)
{
mode = AscentMode.HOLD_AP;
return;
}
maxholdAPTime = Math.Max(maxholdAPTime, fixedTimeToAp());
// fade pitch from AoA at 90% of intermediateAltitude to 0 at 95% of intermediateAltitude
double pitchfade = MuUtils.Clamp(- 20 * vesselState.altitudeASL / intermediateAltitude + 19, 0.0, 1.0);
// srfvelPitch == zero AoA
attitudeTo(srfvelPitch() * pitchfade);
if (autopilot.autoThrottle)
{
core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, intermediateAltitude + mainBody.Radius);
if (core.thrust.targetThrottle < 1.0F)
{
status = "Fine tuning intermediate altitude";
return;
}
}
status = "Gravity turn";
}
//Computes the deltaV of the burn needed to set a given PeR and ApR at at a given UT.
public static Vector3d DeltaVToEllipticize(Orbit o, double UT, double newPeR, double newApR)
{
double radius = o.Radius(UT);
//sanitize inputs
newPeR = MuUtils.Clamp(newPeR, 0 + 1, radius - 1);
newApR = Math.Max(newApR, radius + 1);
double GM = o.referenceBody.gravParameter;
double E = -GM / (newPeR + newApR); //total energy per unit mass of new orbit
double L = Math.Sqrt(Math.Abs((Math.Pow(E * (newApR - newPeR), 2) - GM * GM) / (2 * E))); //angular momentum per unit mass of new orbit
double kineticE = E + GM / radius; //kinetic energy (per unit mass) of new orbit at UT
double horizontalV = L / radius; //horizontal velocity of new orbit at UT
double verticalV = Math.Sqrt(Math.Abs(2 * kineticE - horizontalV * horizontalV)); //vertical velocity of new orbit at UT
Vector3d actualVelocity = o.SwappedOrbitalVelocityAtUT(UT);
//untested:
verticalV *= Math.Sign(Vector3d.Dot(o.Up(UT), actualVelocity));
Vector3d desiredVelocity = horizontalV * o.Horizontal(UT) + verticalV * o.Up(UT);
return(desiredVelocity - actualVelocity);
}
protected override void WindowGUI(int windowID)
{
if (btNormal == null)
{
btNormal = new GUIStyle(GUI.skin.button);
btNormal.normal.textColor = btNormal.focused.textColor = Color.white;
btNormal.hover.textColor = btNormal.active.textColor = Color.yellow;
btNormal.onNormal.textColor = btNormal.onFocused.textColor = btNormal.onHover.textColor = btNormal.onActive.textColor = Color.green;
//btNormal.padding = new RectOffset(8, 8, 8, 8);
btActive = new GUIStyle(btNormal);
btActive.active = btActive.onActive;
btActive.normal = btActive.onNormal;
btActive.onFocused = btActive.focused;
btActive.hover = btActive.onHover;
btGreen = new GUIStyle(btNormal);
btGreen.normal.textColor = Color.green;
btGreen.fixedWidth = 35;
btWhite = new GUIStyle(btNormal);
btWhite.normal.textColor = Color.white;
btWhite.fixedWidth = 35;
btAuto = new GUIStyle(btNormal);
btAuto.padding = new RectOffset(8, 8, 8, 8);
btAuto.normal.textColor = Color.red;
btAuto.onActive = btAuto.onFocused = btAuto.onHover = btAuto.onNormal = btAuto.active = btAuto.focused = btAuto.hover = btAuto.normal;
}
if (autopilot.enabled)
{
if (GUILayout.Button("Disengage autopilot", btActive))
{
autopilot.users.Remove(this);
}
}
else if (core.attitude.enabled && core.attitude.users.Count(u => !this.Equals(u)) > 0)
{
if (core.attitude.users.Contains(this))
{
core.attitude.users.Remove(this); // so we don't suddenly turn on when the other autopilot finishes
}
GUILayout.Button("Auto", btAuto, GUILayout.ExpandWidth(true));
}
else
{
if (GUILayout.Button("Engage autopilot"))
{
autopilot.users.Add(this);
}
}
GUILayout.BeginHorizontal();
bool AltitudeHold = autopilot.AltitudeHoldEnabled;
autopilot.AltitudeHoldEnabled = GUILayout.Toggle(autopilot.AltitudeHoldEnabled, "Altitude Hold", GUILayout.Width(140));
if (AltitudeHold != autopilot.AltitudeHoldEnabled)
{
if (autopilot.AltitudeHoldEnabled)
{
autopilot.EnableAltitudeHold();
}
else
{
autopilot.DisableAltitudeHold();
}
}
bool change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
AltitudeTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
AltitudeTargettmp.text = GUILayout.TextField(AltitudeTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
AltitudeTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (AltitudeTargettmp < 0)
{
AltitudeTargettmp = 0;
}
GUILayout.Label("m", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.AltitudeTarget == AltitudeTargettmp ? btWhite : btGreen))
{
autopilot.AltitudeTarget = AltitudeTargettmp;
}
GUILayout.EndHorizontal();
if (!autopilot.AltitudeHoldEnabled)
{
bool _VertSpeedHoldEnabled = autopilot.VertSpeedHoldEnabled;
GUILayout.BeginHorizontal();
autopilot.VertSpeedHoldEnabled = GUILayout.Toggle(autopilot.VertSpeedHoldEnabled, "Vertical Speed Hold", GUILayout.Width(140));
if (_VertSpeedHoldEnabled != autopilot.VertSpeedHoldEnabled)
{
if (autopilot.VertSpeedHoldEnabled)
{
autopilot.EnableVertSpeedHold();
}
else
{
autopilot.DisableVertSpeedHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
VertSpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp.text = GUILayout.TextField(VertSpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
VertSpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.VertSpeedTarget == VertSpeedTargettmp ? btWhite : btGreen))
{
autopilot.VertSpeedTarget = VertSpeedTargettmp;
}
GUILayout.EndHorizontal();
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(" Vertical Speed Limit", GUILayout.Width(140));
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
VertSpeedMaxtmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedMaxtmp.text = GUILayout.TextField(VertSpeedMaxtmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
VertSpeedMaxtmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (VertSpeedMaxtmp < 0)
{
VertSpeedMaxtmp = 0;
}
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.VertSpeedMax == VertSpeedMaxtmp ? btWhite : btGreen))
{
autopilot.VertSpeedMax = VertSpeedMaxtmp;
}
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
bool _HeadingHoldEnabled = autopilot.HeadingHoldEnabled;
autopilot.HeadingHoldEnabled = GUILayout.Toggle(autopilot.HeadingHoldEnabled, "Heading Hold", GUILayout.Width(140));
if (_HeadingHoldEnabled != autopilot.HeadingHoldEnabled)
{
if (autopilot.HeadingHoldEnabled)
{
autopilot.EnableHeadingHold();
}
else
{
autopilot.DisableHeadingHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
HeadingTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
HeadingTargettmp.text = GUILayout.TextField(HeadingTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
HeadingTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
HeadingTargettmp = MuUtils.ClampDegrees360(HeadingTargettmp);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.HeadingTarget == HeadingTargettmp ? btWhite : btGreen))
{
autopilot.HeadingTarget = HeadingTargettmp;
}
GUILayout.EndHorizontal();
if (!autopilot.HeadingHoldEnabled)
{
GUILayout.BeginHorizontal();
autopilot.RollHoldEnabled = GUILayout.Toggle(autopilot.RollHoldEnabled, "Roll Hold", GUILayout.Width(140));
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
RollTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollTargettmp.text = GUILayout.TextField(RollTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
RollTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollTargettmp = MuUtils.Clamp(RollTargettmp, -90, 90);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.RollTarget == RollTargettmp ? btWhite : btGreen))
{
autopilot.RollTarget = RollTargettmp;
}
GUILayout.EndHorizontal();
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(" Roll Limit ±", GUILayout.Width(140));
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
RollMaxtmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollMaxtmp.text = GUILayout.TextField(RollMaxtmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
RollMaxtmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollMaxtmp = MuUtils.Clamp(RollMaxtmp, -60, 60);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.RollMax == RollMaxtmp ? btWhite : btGreen))
{
autopilot.RollMax = RollMaxtmp;
}
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
bool _AutoThrustCtrl = autopilot.SpeedHoldEnabled;
autopilot.SpeedHoldEnabled = GUILayout.Toggle(autopilot.SpeedHoldEnabled, "Speed Hold", GUILayout.Width(140));
if (autopilot.SpeedHoldEnabled != _AutoThrustCtrl)
{
if (autopilot.SpeedHoldEnabled)
{
autopilot.EnableSpeedHold();
}
else
{
autopilot.DisableSpeedHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
SpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
SpeedTargettmp.text = GUILayout.TextField(SpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
SpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (SpeedTargettmp < 0)
{
SpeedTargettmp = 0;
}
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button("Set", autopilot.SpeedTarget == SpeedTargettmp ? btWhite : btGreen))
{
autopilot.SpeedTarget = SpeedTargettmp;
}
GUILayout.EndHorizontal();
if (!showpid)
{
if (GUILayout.Button("PID", GUILayout.Width(40)))
{
showpid = true;
}
}
else
{
if (GUILayout.Button("Hide PID", GUILayout.Width(140)))
{
showpid = false;
}
GUILayout.BeginHorizontal();
GUILayout.Label("Accceleration", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.AccKp.text = GUILayout.TextField(autopilot.AccKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.AccKi.text = GUILayout.TextField(autopilot.AccKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.AccKd.text = GUILayout.TextField(autopilot.AccKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.SpeedHoldEnabled)
{
GUILayout.Label("error:" + autopilot.a_err.ToString("F2") + " Target:" + autopilot.RealAccelerationTarget.ToString("F2") + " Cur:" + autopilot.cur_acc.ToString("F2"), GUILayout.ExpandWidth(false));
}
GUILayout.BeginHorizontal();
GUILayout.Label("VertSpeed", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.VerKp.text = GUILayout.TextField(autopilot.VerKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.VerKi.text = GUILayout.TextField(autopilot.VerKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.VerKd.text = GUILayout.TextField(autopilot.VerKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.VertSpeedHoldEnabled)
{
GUILayout.Label("error:" + autopilot.v_err.ToString("F2") + " Target:" + autopilot.RealVertSpeedTarget.ToString("F2") + " Cur:" + vesselState.speedVertical.ToString("F2"), GUILayout.ExpandWidth(false));
}
GUILayout.BeginHorizontal();
GUILayout.Label("Roll", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.RolKp.text = GUILayout.TextField(autopilot.RolKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.RolKi.text = GUILayout.TextField(autopilot.RolKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.RolKd.text = GUILayout.TextField(autopilot.RolKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Yaw", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.YawKp.text = GUILayout.TextField(autopilot.YawKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.YawKi.text = GUILayout.TextField(autopilot.YawKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.YawKd.text = GUILayout.TextField(autopilot.YawKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Yaw Control Limit", GUILayout.ExpandWidth(false));
autopilot.YawLimit.text = GUILayout.TextField(autopilot.YawLimit.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
}
base.WindowGUI(windowID);
}
void DriveGravityTurn(FlightCtrlState s)
{
//stop the gravity turn when our apoapsis reaches the desired altitude
if (autoThrottle && orbit.ApA > desiredOrbitAltitude)
{
mode = AscentMode.COAST_TO_APOAPSIS;
return;
}
//if we've fallen below the turn start altitude, go back to vertical ascent
if (ascentPath.IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface))
{
mode = AscentMode.VERTICAL_ASCENT;
return;
}
if (autoThrottle)
{
core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, desiredOrbitAltitude + mainBody.Radius);
if (core.thrust.targetThrottle < 1.0F)
{
//when we are bringing down the throttle to make the apoapsis accurate, we're liable to point in weird
//directions because thrust goes down and so "difficulty" goes up. so just burn prograde
core.attitude.attitudeTo(Vector3d.forward, AttitudeReference.ORBIT, this);
status = "Fine tuning apoapsis";
return;
}
}
//transition gradually from the rotating to the non-rotating reference frame. this calculation ensures that
//when our maximum possible apoapsis, given our orbital energy, is desiredOrbitalRadius, then we are
//fully in the non-rotating reference frame and thus doing the correct calculations to get the right inclination
double GM = mainBody.gravParameter;
double potentialDifferenceWithApoapsis = GM / vesselState.radius - GM / (mainBody.Radius + desiredOrbitAltitude);
double verticalSpeedForDesiredApoapsis = Math.Sqrt(2 * potentialDifferenceWithApoapsis);
double referenceFrameBlend = Mathf.Clamp((float)(vesselState.speedOrbital / verticalSpeedForDesiredApoapsis), 0.0F, 1.0F);
Vector3d actualVelocityUnit = ((1 - referenceFrameBlend) * vesselState.surfaceVelocity.normalized
+ referenceFrameBlend * vesselState.orbitalVelocity.normalized).normalized;
double desiredHeading = UtilMath.Deg2Rad * OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, desiredInclination);
Vector3d desiredHeadingVector = Math.Sin(desiredHeading) * vesselState.east + Math.Cos(desiredHeading) * vesselState.north;
double desiredFlightPathAngle = ascentPath.FlightPathAngle(vesselState.altitudeASL, vesselState.speedSurface);
Vector3d desiredVelocityUnit = Math.Cos(desiredFlightPathAngle * UtilMath.Deg2Rad) * desiredHeadingVector
+ Math.Sin(desiredFlightPathAngle * UtilMath.Deg2Rad) * vesselState.up;
Vector3d desiredThrustVector = desiredVelocityUnit;
if (correctiveSteering)
{
Vector3d velocityError = (desiredVelocityUnit - actualVelocityUnit);
double difficulty = vesselState.surfaceVelocity.magnitude * 0.02 / vesselState.ThrustAccel(core.thrust.targetThrottle);
difficulty = MuUtils.Clamp(difficulty, 0.1, 1.0);
Vector3d steerOffset = correctiveSteeringGain * difficulty * velocityError;
//limit the amount of steering to 10 degrees. Furthermore, never steer to a FPA of > 90 (that is, never lean backward)
double maxOffset = 10 * UtilMath.Deg2Rad;
if (desiredFlightPathAngle > 80)
{
maxOffset = (90 - desiredFlightPathAngle) * UtilMath.Deg2Rad;
}
if (steerOffset.magnitude > maxOffset)
{
steerOffset = maxOffset * steerOffset.normalized;
}
desiredThrustVector += steerOffset;
}
desiredThrustVector = desiredThrustVector.normalized;
if (limitAoA)
{
float fade = vesselState.dynamicPressure < aoALimitFadeoutPressure ? (float)(aoALimitFadeoutPressure / vesselState.dynamicPressure) : 1;
currentMaxAoA = Math.Min(fade * maxAoA, 180d);
limitingAoA = vessel.altitude <mainBody.atmosphereDepth && Vector3.Angle(vesselState.surfaceVelocity, desiredThrustVector)> currentMaxAoA;
if (limitingAoA)
{
desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(currentMaxAoA * Mathf.Deg2Rad), 1).normalized;
}
}
if (forceRoll && Vector3.Angle(vesselState.up, vesselState.forward) > 7 && core.attitude.attitudeError < 5)
{
var pitch = 90 - Vector3.Angle(vesselState.up, desiredThrustVector);
var hdg = core.rover.HeadingToPos(vessel.CoM, vessel.CoM + desiredThrustVector);
core.attitude.attitudeTo(hdg, pitch, turnRoll, this);
}
else
{
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
}
status = "Gravity turn";
}
/*
* R1 = position at t0
* V1 = velocity at t0
* R2 = position at t1
* V2 = velocity at t1
* tof = time of flight (t1 - t0) (+ posigrade "shortway", - retrograde "longway")
* nrev = number of full revolutions (+ left-branch, - right-branch for nrev != 0)
* Vi = initial velocity vector of transfer orbit (Vi - V1 = deltaV)
* Vf = final velocity vector of transfer orbit (V2 - Vf = deltaV)
*/
public static void Solve(double GM, Vector3d R1, Vector3d V1, Vector3d R2, Vector3d V2, double tof, int nrev, out Vector3d Vi, out Vector3d Vf)
{
/* most of this function lifted from https://www.mathworks.com/matlabcentral/fileexchange/39530-lambert-s-problem/content/glambert.m */
// if we don't catch this edge condition, the solver will spin forever (internal state will NaN and produce great sadness)
if (tof == 0)
{
throw new Exception("MechJeb's Gooding Lambert Solver does not support zero time of flight (teleportation)");
}
double VR11, VT11, VR12, VT12;
double VR21, VT21, VR22, VT22;
int n;
// initialize in case we throw
Vi = Vector3d.zero;
Vf = Vector3d.zero;
Vector3d ur1xv1 = Vector3d.Cross(R1, V1).normalized;
Vector3d ux1 = R1.normalized;
Vector3d ux2 = R2.normalized;
Vector3d uz1 = Vector3d.Cross(ux1, ux2).normalized;
/* calculate the minimum transfer angle (radians) */
double theta = Math.Acos(MuUtils.Clamp(Vector3d.Dot(ux1, ux2), -1.0, 1.0));
/* calculate the angle between the orbit normal of the initial orbit and the fundamental reference plane */
double angle_to_on = Math.Acos(MuUtils.Clamp(Vector3d.Dot(ur1xv1, uz1), -1.0, 1.0));
/* if angle to orbit normal is greater than 90 degrees and posigrade orbit, then flip the orbit normal and the transfer angle */
if ((angle_to_on > 0.5 * Math.PI) && (tof > 0.0))
{
theta = 2.0 * Math.PI - theta;
uz1 = -uz1;
}
if ((angle_to_on < 0.5 * Math.PI) && (tof < 0.0))
{
theta = 2.0 * Math.PI - theta;
uz1 = -uz1;
}
Vector3d uz2 = uz1;
Vector3d uy1 = Vector3d.Cross(uz1, ux1).normalized;
Vector3d uy2 = Vector3d.Cross(uz2, ux2).normalized;
theta = theta + 2.0 * Math.PI * Math.Abs(nrev);
VLAMB(GM, R1.magnitude, R2.magnitude, theta, tof, out n, out VR11, out VT11, out VR12, out VT12, out VR21, out VT21, out VR22, out VT22);
//Debug.Log("VLAMBOUT: n= " + n + " VR11= " + VR11 + " VT11= " + VT11 + " VR12= " + VR12 + " VT12= " + VT12 + " VR21= " + VR21 + " VT21= " + VT21 + " VR22= " + VR22 + " VT22= " + VT22);
if (nrev > 0)
{
if (n == -1)
{
throw new Exception("Gooding Solver found no tminimum");
}
else if (n == 0)
{
throw new Exception("Gooding Solver found no solution time");
}
}
/* compute transfer orbit initial and final velocity vectors */
if ((nrev > 0) && (n > 1))
{
Vi = VR21 * ux1 + VT21 * uy1;
Vf = VR22 * ux2 + VT22 * uy2;
}
else
{
Vi = VR11 * ux1 + VT11 * uy1;
Vf = VR12 * ux2 + VT12 * uy2;
}
}
protected override void WindowGUI(int windowID)
{
if (btNormal == null)
{
btNormal = new GUIStyle(GUI.skin.button);
btNormal.normal.textColor = btNormal.focused.textColor = Color.white;
btNormal.hover.textColor = btNormal.active.textColor = Color.yellow;
btNormal.onNormal.textColor = btNormal.onFocused.textColor = btNormal.onHover.textColor = btNormal.onActive.textColor = Color.green;
//btNormal.padding = new RectOffset(8, 8, 8, 8);
btActive = new GUIStyle(btNormal);
btActive.active = btActive.onActive;
btActive.normal = btActive.onNormal;
btActive.onFocused = btActive.focused;
btActive.hover = btActive.onHover;
btGreen = new GUIStyle(btNormal);
btGreen.normal.textColor = Color.green;
btGreen.fixedWidth = 35;
btWhite = new GUIStyle(btNormal);
btWhite.normal.textColor = Color.white;
btWhite.fixedWidth = 35;
btAuto = new GUIStyle(btNormal);
btAuto.padding = new RectOffset(8, 8, 8, 8);
btAuto.normal.textColor = Color.red;
btAuto.onActive = btAuto.onFocused = btAuto.onHover = btAuto.onNormal = btAuto.active = btAuto.focused = btAuto.hover = btAuto.normal;
}
if (autopilot.enabled)
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_button1"), btActive)) //Disengage autopilot
{
autopilot.users.Remove(this);
}
}
else if (core.attitude.enabled && core.attitude.users.Count(u => !this.Equals(u)) > 0)
{
if (core.attitude.users.Contains(this))
{
core.attitude.users.Remove(this); // so we don't suddenly turn on when the other autopilot finishes
}
GUILayout.Button("Auto", btAuto, GUILayout.ExpandWidth(true));
}
else
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_button2"))) //Engage autopilot
{
autopilot.users.Add(this);
}
}
GUILayout.BeginHorizontal();
bool AltitudeHold = autopilot.AltitudeHoldEnabled;
autopilot.AltitudeHoldEnabled = GUILayout.Toggle(autopilot.AltitudeHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label1"), GUILayout.Width(140)); //Altitude Hold
if (AltitudeHold != autopilot.AltitudeHoldEnabled)
{
if (autopilot.AltitudeHoldEnabled)
{
autopilot.EnableAltitudeHold();
}
else
{
autopilot.DisableAltitudeHold();
}
}
bool change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
AltitudeTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
AltitudeTargettmp.text = GUILayout.TextField(AltitudeTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
AltitudeTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (AltitudeTargettmp < 0)
{
AltitudeTargettmp = 0;
}
GUILayout.Label("m", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset1"), autopilot.AltitudeTarget == AltitudeTargettmp ? btWhite : btGreen)) //Set
{
autopilot.AltitudeTarget = AltitudeTargettmp;
}
GUILayout.EndHorizontal();
if (!autopilot.AltitudeHoldEnabled)
{
bool _VertSpeedHoldEnabled = autopilot.VertSpeedHoldEnabled;
GUILayout.BeginHorizontal();
autopilot.VertSpeedHoldEnabled = GUILayout.Toggle(autopilot.VertSpeedHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label2"), GUILayout.Width(140)); //Vertical Speed Hold
if (_VertSpeedHoldEnabled != autopilot.VertSpeedHoldEnabled)
{
if (autopilot.VertSpeedHoldEnabled)
{
autopilot.EnableVertSpeedHold();
}
else
{
autopilot.DisableVertSpeedHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
VertSpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp.text = GUILayout.TextField(VertSpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
VertSpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp = Math.Max(0, VertSpeedTargettmp);
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset2"), autopilot.VertSpeedTarget == VertSpeedTargettmp ? btWhite : btGreen))
{
autopilot.VertSpeedTarget = VertSpeedTargettmp;
}
GUILayout.EndHorizontal();
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_VS"), GUILayout.Width(140));//" V/S ±"
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
VertSpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp.text = GUILayout.TextField(VertSpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
VertSpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
VertSpeedTargettmp = Math.Max(0, VertSpeedTargettmp);
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset6"), autopilot.VertSpeedTarget == VertSpeedTargettmp ? btWhite : btGreen))
{
autopilot.VertSpeedTarget = VertSpeedTargettmp;
}
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
bool _HeadingHoldEnabled = autopilot.HeadingHoldEnabled;
autopilot.HeadingHoldEnabled = GUILayout.Toggle(autopilot.HeadingHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label4"), GUILayout.Width(140)); //"Heading Hold"
if (_HeadingHoldEnabled != autopilot.HeadingHoldEnabled)
{
if (autopilot.HeadingHoldEnabled)
{
autopilot.EnableHeadingHold();
}
else
{
autopilot.DisableHeadingHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
HeadingTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
HeadingTargettmp.text = GUILayout.TextField(HeadingTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
HeadingTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
HeadingTargettmp = MuUtils.ClampDegrees360(HeadingTargettmp);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset4"), autopilot.HeadingTarget == HeadingTargettmp ? btWhite : btGreen))
{
autopilot.HeadingTarget = HeadingTargettmp;
}
GUILayout.EndHorizontal();
if (!autopilot.HeadingHoldEnabled)
{
GUILayout.BeginHorizontal();
autopilot.RollHoldEnabled = GUILayout.Toggle(autopilot.RollHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label5"), GUILayout.Width(140)); //"Roll Hold"
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
RollTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollTargettmp.text = GUILayout.TextField(RollTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
RollTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollTargettmp = MuUtils.Clamp(RollTargettmp, -90, 90);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset5"), autopilot.RollTarget == RollTargettmp ? btWhite : btGreen))
{
autopilot.RollTarget = RollTargettmp;
}
GUILayout.EndHorizontal();
}
else
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Label6"), GUILayout.Width(140)); //" Roll Limit ±"
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
RollMaxtmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollMaxtmp.text = GUILayout.TextField(RollMaxtmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
RollMaxtmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
RollMaxtmp = MuUtils.Clamp(RollMaxtmp, -60, 60);
GUILayout.Label("°", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset6"), autopilot.BankAngle == RollMaxtmp ? btWhite : btGreen))
{
autopilot.BankAngle = RollMaxtmp;
}
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
bool _AutoThrustCtrl = autopilot.SpeedHoldEnabled;
autopilot.SpeedHoldEnabled = GUILayout.Toggle(autopilot.SpeedHoldEnabled, Localizer.Format("#MechJeb_Aircraftauto_Label7"), GUILayout.Width(140)); //Speed Hold
if (autopilot.SpeedHoldEnabled != _AutoThrustCtrl)
{
if (autopilot.SpeedHoldEnabled)
{
autopilot.EnableSpeedHold();
}
else
{
autopilot.DisableSpeedHold();
}
}
change = false;
if (GUILayout.Button("-", GUILayout.Width(18)))
{
SpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
SpeedTargettmp.text = GUILayout.TextField(SpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18)))
{
SpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true;
}
if (SpeedTargettmp < 0)
{
SpeedTargettmp = 0;
}
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset7"), autopilot.SpeedTarget == SpeedTargettmp ? btWhite : btGreen)) //Set
{
autopilot.SpeedTarget = SpeedTargettmp;
}
GUILayout.EndHorizontal();
if (!showpid)
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_button3"), GUILayout.Width(40))) //"PID"
{
showpid = true;
}
}
else
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_button4"), GUILayout.Width(140))) //Hide PID
{
showpid = false;
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Label8"), GUILayout.ExpandWidth(true)); //Accceleration
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.AccKp.text = GUILayout.TextField(autopilot.AccKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.AccKi.text = GUILayout.TextField(autopilot.AccKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.AccKd.text = GUILayout.TextField(autopilot.AccKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.SpeedHoldEnabled)
{
GUILayout.Label(Localizer.Format("#MecgJeb_Aircraftauto_error1", autopilot.a_err.ToString("F2"), autopilot.RealAccelerationTarget.ToString("F2"), autopilot.cur_acc.ToString("F2")), GUILayout.ExpandWidth(false)); //"error:"<<1>>" Target:"<<2>> " Cur:"<<3>>
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Pitch"), GUILayout.ExpandWidth(true));//"VertSpeed"
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.PitKp.text = GUILayout.TextField(autopilot.PitKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.PitKi.text = GUILayout.TextField(autopilot.PitKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.PitKd.text = GUILayout.TextField(autopilot.PitKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.VertSpeedHoldEnabled)
{
GUILayout.Label(Localizer.Format("#MecgJeb_Aircraftauto_error2", autopilot.pitch_err.ToString("F2"), autopilot.RealPitchTarget.ToString("F2"), vesselState.currentPitch.ToString("F2"), autopilot.pitch_act.ToString("F5"), GUILayout.ExpandWidth(false)));//error:" Target:"" Cur:"
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Label10"), GUILayout.ExpandWidth(true)); //Roll
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.RolKp.text = GUILayout.TextField(autopilot.RolKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.RolKi.text = GUILayout.TextField(autopilot.RolKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.RolKd.text = GUILayout.TextField(autopilot.RolKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.RollHoldEnabled)
{
GUILayout.Label(Localizer.Format("#MecgJeb_Aircraftauto_error2", autopilot.roll_err.ToString("F2"), autopilot.RealRollTarget.ToString("F2"), (-vesselState.currentRoll).ToString("F2"), autopilot.roll_act.ToString("F5"), GUILayout.ExpandWidth(false)));//error:" Target:"" Cur:"
}
GUILayout.BeginHorizontal();
GUILayout.Label("Yaw", GUILayout.ExpandWidth(true));
GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
autopilot.YawKp.text = GUILayout.TextField(autopilot.YawKp.text, GUILayout.Width(40));
GUILayout.Label("i", GUILayout.ExpandWidth(false));
autopilot.YawKi.text = GUILayout.TextField(autopilot.YawKi.text, GUILayout.Width(40));
GUILayout.Label("d", GUILayout.ExpandWidth(false));
autopilot.YawKd.text = GUILayout.TextField(autopilot.YawKd.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
if (autopilot.HeadingHoldEnabled)
{
GUILayout.Label(Localizer.Format("#MecgJeb_Aircraftauto_error2", autopilot.yaw_err.ToString("F2"), autopilot.RealYawTarget.ToString("F2"), autopilot.curr_yaw.ToString("F2"), autopilot.yaw_act.ToString("F5"), GUILayout.ExpandWidth(false)));//error:" Target:"" Cur:"
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_Limits"), GUILayout.ExpandWidth(false)); //Yaw Control Limit
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_PitchDownLimit"), GUILayout.ExpandWidth(false));
autopilot.PitchDownLimit.text = GUILayout.TextField(autopilot.PitchDownLimit.text, GUILayout.Width(40));
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_PitchUpLimit"), GUILayout.ExpandWidth(false));
autopilot.PitchUpLimit.text = GUILayout.TextField(autopilot.PitchUpLimit.text, GUILayout.Width(40));
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_YawLimit"), GUILayout.ExpandWidth(false));
autopilot.YawLimit.text = GUILayout.TextField(autopilot.YawLimit.text, GUILayout.Width(40));
GUILayout.Label(Localizer.Format("#MechJeb_Aircraftauto_RollLimit"), GUILayout.ExpandWidth(false));
autopilot.RollLimit.text = GUILayout.TextField(autopilot.RollLimit.text, GUILayout.Width(40));
GUILayout.EndHorizontal();
}
base.WindowGUI(windowID);
}
protected override void WindowGUI(int windowID)
{
if (btNormal == null)
{
btNormal = new GUIStyle(GUI.skin.button);
btNormal.normal.textColor = btNormal.focused.textColor = Color.white;
btNormal.hover.textColor = btNormal.active.textColor = Color.yellow;
btNormal.onNormal.textColor = btNormal.onFocused.textColor = btNormal.onHover.textColor = btNormal.onActive.textColor = Color.green;
btNormal.padding = new RectOffset(8, 8, 8, 8);
btActive = new GUIStyle(btNormal);
btActive.active = btActive.onActive;
btActive.normal = btActive.onNormal;
btActive.onFocused = btActive.focused;
btActive.hover = btActive.onHover;
}
GUILayout.BeginVertical();
if (autopilot != null)
{
if (autopilot.enabled)
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button1")))//Disengage autopilot
{
autopilot.users.Remove(this);
}
}
else
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button2")))//Engage autopilot
{
autopilot.users.Add(this);
}
}
if (ascentPathIdx == ascentType.PVG)
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button3")))//Reset Guidance (DO NOT PRESS)
{
core.guidance.Reset();
}
GUILayout.BeginHorizontal(); // EditorStyles.toolbar);
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button4"), autopilot.showTargeting ? btActive : btNormal, GUILayout.ExpandWidth(true))) //"TARG"
{
autopilot.showTargeting = !autopilot.showTargeting;
}
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button5"), autopilot.showGuidanceSettings ? btActive : btNormal, GUILayout.ExpandWidth(true))) //GUID
{
autopilot.showGuidanceSettings = !autopilot.showGuidanceSettings;
}
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button6"), autopilot.showSettings ? btActive : btNormal, GUILayout.ExpandWidth(true))) //OPTS
{
autopilot.showSettings = !autopilot.showSettings;
}
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button7"), autopilot.showStatus ? btActive : btNormal, GUILayout.ExpandWidth(true))) //STATUS
{
autopilot.showStatus = !autopilot.showStatus;
}
GUILayout.EndHorizontal();
}
else if (ascentPathIdx == ascentType.GRAVITYTURN)
{
GUILayout.BeginHorizontal(); // EditorStyles.toolbar);
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button8"), autopilot.showTargeting ? btActive : btNormal, GUILayout.ExpandWidth(true))) //TARG
{
autopilot.showTargeting = !autopilot.showTargeting;
}
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button9"), autopilot.showGuidanceSettings ? btActive : btNormal, GUILayout.ExpandWidth(true))) //GUID
{
autopilot.showGuidanceSettings = !autopilot.showGuidanceSettings;
}
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button10"), autopilot.showSettings ? btActive : btNormal, GUILayout.ExpandWidth(true))) //OPTS
{
autopilot.showSettings = !autopilot.showSettings;
}
GUILayout.EndHorizontal();
}
else if (ascentPathIdx == ascentType.BREATHING_GT)
{
GUILayout.BeginHorizontal(); // EditorStyles.toolbar);
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button8"), autopilot.showTargeting ? btActive : btNormal, GUILayout.ExpandWidth(true))) //TARG
{
autopilot.showTargeting = !autopilot.showTargeting;
}
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button9"), autopilot.showGuidanceSettings ? btActive : btNormal, GUILayout.ExpandWidth(true)))//GUID
{
autopilot.showGuidanceSettings = !autopilot.showGuidanceSettings;
}
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button10"), autopilot.showSettings ? btActive : btNormal, GUILayout.ExpandWidth(true)))//OPTS
{
autopilot.showSettings = !autopilot.showSettings;
}
GUILayout.EndHorizontal();
}
else if (ascentPathIdx == ascentType.CLASSIC)
{
GUILayout.BeginHorizontal(); // EditorStyles.toolbar);
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button11"), autopilot.showTargeting ? btActive : btNormal, GUILayout.ExpandWidth(true))) //TARG
{
autopilot.showTargeting = !autopilot.showTargeting;
}
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button12"), autopilot.showSettings ? btActive : btNormal, GUILayout.ExpandWidth(true))) //OPTS
{
autopilot.showSettings = !autopilot.showSettings;
}
GUILayout.EndHorizontal();
}
if (autopilot.showTargeting)
{
if (ascentPathIdx == ascentType.PVG)
{
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label1"), autopilot.desiredOrbitAltitude, "km"); //Target Periapsis
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label2"), pvgascent.desiredApoapsis, "km"); //Target Apoapsis:
if (pvgascent.desiredApoapsis >= 0 && pvgascent.desiredApoapsis < autopilot.desiredOrbitAltitude)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label3"), s);//Ap < Pe: circularizing orbit
}
if (pvgascent.desiredApoapsis < 0)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = XKCDColors.Orange;
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label4"), s);//Hyperbolic target orbit (neg Ap)
}
}
else
{
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label5"), autopilot.desiredOrbitAltitude, "km");//Orbit altitude
}
GUIStyle si = new GUIStyle(GUI.skin.label);
if (Math.Abs(desiredInclination) < Math.Abs(vesselState.latitude) - 2.001)
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Orange;
}
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label6"), si, GUILayout.ExpandWidth(true));//Orbit inc.
desiredInclination.text = GUILayout.TextField(desiredInclination.text, GUILayout.ExpandWidth(true), GUILayout.Width(100));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button13")))//Current
{
desiredInclination.val = vesselState.latitude;
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (Math.Abs(desiredInclination) < Math.Abs(vesselState.latitude) - 2.001)
{
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label7", Math.Abs(vesselState.latitude) - Math.Abs(desiredInclination)), si);//inc {0:F1}º below current latitude
}
GUILayout.EndHorizontal();
autopilot.desiredInclination = desiredInclination;
}
if (autopilot.showGuidanceSettings)
{
if (ascentPathIdx == ascentType.GRAVITYTURN)
{
GUILayout.BeginVertical();
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label8"), gtascent.turnStartAltitude, "km"); //Turn start altitude:
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label9"), gtascent.turnStartVelocity, "m/s"); //Turn start velocity:
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label10"), gtascent.turnStartPitch, "deg"); //Turn start pitch:
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label11"), gtascent.intermediateAltitude, "km"); //Intermediate altitude:
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label12"), gtascent.holdAPTime, "s"); //Hold AP Time:
GUILayout.EndVertical();
}
else if (ascentPathIdx == ascentType.BREATHING_GT)
{
GUILayout.BeginVertical();
//GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label8"), bgtascent.turnStartAltitude, "km");//Turn start altitude:
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label9"), bgtascent.turnStartVelocity, "m/s"); //Turn start velocity:
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label10"), bgtascent.turnStartPitch, "deg"); //Turn start pitch:
GuiUtils.SimpleTextBox(Localizer.Format("speed for breathing mode:"), bgtascent.startBreathingSpeed, "m/s"); //speed for breathing mode:
GuiUtils.SimpleTextBox(Localizer.Format("TWR to throttle other:"), bgtascent.minTWRthrottle, "s"); //TWR to throttle other:
GUILayout.EndVertical();
}
else if (ascentPathIdx == ascentType.PVG)
{
GUILayout.BeginVertical();
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label13"), pvgascent.pitchStartVelocity, "m/s"); //Booster Pitch start:
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label14"), pvgascent.pitchRate, "°/s"); //Booster Pitch rate:
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label15"), core.guidance.pvgInterval, "s"); //Guidance Interval:
if (core.guidance.pvgInterval < 1 || core.guidance.pvgInterval > 30)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label16"), s); //Guidance intervals are limited to between 1s and 30s
}
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label17"), autopilot.limitQa, "Pa-rad"); //Qα limit
if (autopilot.limitQa < 100 || autopilot.limitQa > 4000)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
if (autopilot.limitQa < 100)
{
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label18"), s);//Qα limit cannot be set to lower than 100 Pa-rad
}
else if (autopilot.limitQa > 10000)
{
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label19"), s);//Qα limit cannot be set to higher than 10000 Pa-rad
}
else
{
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label20"), s);//Qα limit is recommended to be 1000 to 4000 Pa-rad
}
}
pvgascent.omitCoast = GUILayout.Toggle(pvgascent.omitCoast, Localizer.Format("#MechJeb_Ascent_checkbox1"));//Omit Coast
GUILayout.EndVertical();
}
}
autopilot.limitQaEnabled = (ascentPathIdx == ascentType.PVG); // this is mandatory for PVG
if (autopilot.showSettings)
{
ToggleAscentNavballGuidanceInfoItem();
if (ascentPathIdx != ascentType.PVG)
{
core.thrust.LimitToPreventOverheatsInfoItem();
//core.thrust.LimitToTerminalVelocityInfoItem();
core.thrust.LimitToMaxDynamicPressureInfoItem();
core.thrust.LimitAccelerationInfoItem();
core.thrust.LimitThrottleInfoItem();
core.thrust.LimiterMinThrottleInfoItem();
core.thrust.LimitElectricInfoItem();
}
else
{
core.thrust.LimitToPreventOverheatsInfoItem();
//core.thrust.LimitToTerminalVelocityInfoItem();
core.thrust.LimitToMaxDynamicPressureInfoItem();
//core.thrust.LimitAccelerationInfoItem();
//core.thrust.LimitThrottleInfoItem();
core.thrust.LimiterMinThrottleInfoItem();
//core.thrust.LimitElectricInfoItem();
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label21")); //FIXME: g-limiter is down for maintenance
core.thrust.limitAcceleration = false;
core.thrust.limitThrottle = false;
core.thrust.limitToTerminalVelocity = false;
core.thrust.electricThrottle = false;
}
GUILayout.BeginHorizontal();
autopilot.forceRoll = GUILayout.Toggle(autopilot.forceRoll, Localizer.Format("#MechJeb_Ascent_checkbox2"));//Force Roll
if (autopilot.forceRoll)
{
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label22"), autopilot.verticalRoll, "º", 30f); //climb
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label23"), autopilot.turnRoll, "º", 30f); //turn
}
GUILayout.EndHorizontal();
if (ascentPathIdx != ascentType.PVG)
{
GUILayout.BeginHorizontal();
GUIStyle s = new GUIStyle(GUI.skin.toggle);
if (autopilot.limitingAoA)
{
s.onHover.textColor = s.onNormal.textColor = Color.green;
}
autopilot.limitAoA = GUILayout.Toggle(autopilot.limitAoA, Localizer.Format("#MechJeb_Ascent_checkbox3"), s, GUILayout.ExpandWidth(true));//Limit AoA to
autopilot.maxAoA.text = GUILayout.TextField(autopilot.maxAoA.text, GUILayout.Width(30));
GUILayout.Label("º (" + autopilot.currentMaxAoA.ToString("F1") + "°)", GUILayout.ExpandWidth(true));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Space(25);
if (autopilot.limitAoA)
{
GUIStyle sl = new GUIStyle(GUI.skin.label);
if (autopilot.limitingAoA && vesselState.dynamicPressure < autopilot.aoALimitFadeoutPressure)
{
sl.normal.textColor = sl.hover.textColor = Color.green;
}
GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_Ascent_label24"), autopilot.aoALimitFadeoutPressure, "Pa", 50, sl);//Dynamic Pressure Fadeout
}
GUILayout.EndHorizontal();
autopilot.limitQaEnabled = false; // this is only for PVG
}
if (ascentPathIdx == ascentType.CLASSIC)
{
// corrective steering only applies to Classic
GUILayout.BeginHorizontal();
autopilot.correctiveSteering = GUILayout.Toggle(autopilot.correctiveSteering, Localizer.Format("#MechJeb_Ascent_checkbox4"), GUILayout.ExpandWidth(false));//Corrective steering
if (autopilot.correctiveSteering)
{
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label25"), GUILayout.ExpandWidth(false));//Gain
autopilot.correctiveSteeringGain.text = GUILayout.TextField(autopilot.correctiveSteeringGain.text, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
}
autopilot.autostage = GUILayout.Toggle(autopilot.autostage, Localizer.Format("#MechJeb_Ascent_checkbox5"));//Autostage
if (autopilot.autostage)
{
core.staging.AutostageSettingsInfoItem();
}
autopilot.autodeploySolarPanels = GUILayout.Toggle(autopilot.autodeploySolarPanels,
Localizer.Format("#MechJeb_Ascent_checkbox6"));//Auto-deploy solar panels
autopilot.autoDeployAntennas = GUILayout.Toggle(autopilot.autoDeployAntennas,
Localizer.Format("#MechJeb_Ascent_checkbox7"));//Auto-deploy antennas
GUILayout.BeginHorizontal();
core.node.autowarp = GUILayout.Toggle(core.node.autowarp, Localizer.Format("#MechJeb_Ascent_checkbox8"));//Auto-warp
if (ascentPathIdx != ascentType.PVG)
{
autopilot.skipCircularization = GUILayout.Toggle(autopilot.skipCircularization, Localizer.Format("#MechJeb_Ascent_checkbox9"));//Skip Circularization
}
else
{
// skipCircularization is always true for Optimizer
autopilot.skipCircularization = true;
}
GUILayout.EndHorizontal();
}
if (autopilot.showStatus)
{
if (ascentPathIdx == ascentType.PVG)
{
if (core.guidance.solution != null)
{
for (int i = core.guidance.solution.num_segments; i > 0; i--)
{
GUILayout.Label(String.Format("{0}: {1}", i, core.guidance.solution.ArcString(vesselState.time, i - 1)));
}
}
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("vgo: {0:F1}", core.guidance.vgo), GUILayout.Width(100));
GUILayout.Label(String.Format("heading: {0:F1}", core.guidance.heading), GUILayout.Width(100));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("tgo: {0:F3}", core.guidance.tgo), GUILayout.Width(100));
GUILayout.Label(String.Format("pitch: {0:F1}", core.guidance.pitch), GUILayout.Width(100));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUIStyle si = new GUIStyle(GUI.skin.label);
if (core.guidance.isStable())
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Green;
}
else if (core.guidance.isInitializing() || core.guidance.status == PVGStatus.FINISHED)
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Orange;
}
else
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Red;
}
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label26") + core.guidance.status, si);//Guidance Status:
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label27") + core.guidance.successful_converges, GUILayout.Width(100)); //converges:
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label28") + core.guidance.last_lm_status, GUILayout.Width(100)); //status:
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("n: " + core.guidance.last_lm_iteration_count + "(" + core.guidance.max_lm_iteration_count + ")", GUILayout.Width(100));
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label29") + GuiUtils.TimeToDHMS(core.guidance.staleness));//staleness:
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("znorm: {0:G5}", core.guidance.last_znorm));
GUILayout.EndHorizontal();
if (core.guidance.last_failure_cause != null)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.red;
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label30") + core.guidance.last_failure_cause, s);//LAST FAILURE:
GUILayout.EndHorizontal();
}
if (vessel.situation != Vessel.Situations.LANDED && vessel.situation != Vessel.Situations.PRELAUNCH && vessel.situation != Vessel.Situations.SPLASHED)
{
double m0 = atmoStats[vessel.currentStage].startMass;
double thrust = atmoStats[vessel.currentStage].startThrust;
if (Math.Abs(vesselState.mass - m0) / m0 > 0.01)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format(Localizer.Format("#MechJeb_Ascent_label31") + "{0:F1}%", (vesselState.mass - m0) / m0 * 100.0), s);//MASS IS OFF BY
GUILayout.EndHorizontal();
}
if (Math.Abs(vesselState.thrustCurrent - thrust) / thrust > 0.01)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format(Localizer.Format("#MechJeb_Ascent_label32") + "{0:F1}%", (vesselState.thrustCurrent - thrust) / thrust * 100.0), s);//THRUST IS OFF BY
GUILayout.EndHorizontal();
}
}
}
}
if (vessel.LandedOrSplashed)
{
if (core.target.NormalTargetExists)
{
if (core.node.autowarp)
{
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label33"), GUILayout.ExpandWidth(true)); //Launch countdown:
autopilot.warpCountDown.text = GUILayout.TextField(autopilot.warpCountDown.text,
GUILayout.Width(60));
GUILayout.Label("s", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
if (!launchingToPlane && !launchingToRendezvous && !launchingToInterplanetary)
{
// disable plane/rendezvous/interplanetary for now
if (ascentPathIdx != ascentType.PVG)
{
GUILayout.BeginHorizontal();
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button14"), GUILayout.ExpandWidth(false))) //Launch to rendezvous:
{
launchingToRendezvous = true;
autopilot.StartCountdown(vesselState.time +
LaunchTiming.TimeToPhaseAngle(autopilot.launchPhaseAngle,
mainBody, vesselState.longitude, core.target.TargetOrbit));
}
autopilot.launchPhaseAngle.text = GUILayout.TextField(autopilot.launchPhaseAngle.text,
GUILayout.Width(60));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button15"), GUILayout.ExpandWidth(false))) //Launch into plane of target
{
launchingToPlane = true;
autopilot.StartCountdown(vesselState.time +
LaunchTiming.TimeToPlane(autopilot.launchLANDifference,
mainBody, vesselState.latitude, vesselState.longitude,
core.target.TargetOrbit));
}
autopilot.launchLANDifference.text = GUILayout.TextField(
autopilot.launchLANDifference.text, GUILayout.Width(60));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
if (core.target.TargetOrbit.referenceBody == orbit.referenceBody.referenceBody)
{
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button16"))) //Launch at interplanetary window
{
launchingToInterplanetary = true;
//compute the desired launch date
OrbitalManeuverCalculator.DeltaVAndTimeForHohmannTransfer(mainBody.orbit,
core.target.TargetOrbit, vesselState.time, out interplanetaryWindowUT);
double desiredOrbitPeriod = 2 * Math.PI *
Math.Sqrt(
Math.Pow(mainBody.Radius + autopilot.desiredOrbitAltitude, 3)
/ mainBody.gravParameter);
//launch just before the window, but don't try to launch in the past
interplanetaryWindowUT -= 3 * desiredOrbitPeriod;
interplanetaryWindowUT = Math.Max(vesselState.time + autopilot.warpCountDown,
interplanetaryWindowUT);
autopilot.StartCountdown(interplanetaryWindowUT);
}
}
}
}
else
{
launchingToInterplanetary = launchingToPlane = launchingToRendezvous = false;
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label34")); //Select a target for a timed launch.
}
if (launchingToInterplanetary || launchingToPlane || launchingToRendezvous)
{
string message = "";
if (launchingToInterplanetary)
{
message = Localizer.Format("#MechJeb_Ascent_msg1"); //Launching at interplanetary window
}
else if (launchingToPlane)
{
desiredInclination = MuUtils.Clamp(core.target.TargetOrbit.inclination, Math.Abs(vesselState.latitude), 180 - Math.Abs(vesselState.latitude));
desiredInclination *=
Math.Sign(Vector3d.Dot(core.target.TargetOrbit.SwappedOrbitNormal(),
Vector3d.Cross(vesselState.CoM - mainBody.position, mainBody.transform.up)));
message = Localizer.Format("#MechJeb_Ascent_msg2"); //Launching to target plane
}
else if (launchingToRendezvous)
{
message = "#MechJeb_Ascent_msg3"; //Launching to rendezvous
}
if (autopilot.tMinus > 3 * vesselState.deltaT)
{
message += ": T-" + GuiUtils.TimeToDHMS(autopilot.tMinus, 1);
}
GUILayout.Label(message);
if (GUILayout.Button(Localizer.Format("#MechJeb_Ascent_button17")))//Abort
{
launchingToInterplanetary =
launchingToPlane = launchingToRendezvous = autopilot.timedLaunch = false;
}
}
}
if (autopilot.enabled)
{
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label35") + autopilot.status);//Autopilot status:
}
if (core.DeactivateControl)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.red;
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label36"), s);//CONTROL DISABLED (AVIONICS)
}
}
if (!vessel.patchedConicsUnlocked() && ascentPathIdx != ascentType.PVG)
{
GUILayout.Label(Localizer.Format("#MechJeb_Ascent_label37"));//"Warning: MechJeb is unable to circularize without an upgraded Tracking Station."
}
GUILayout.BeginHorizontal();
autopilot.ascentPathIdxPublic = (ascentType)GuiUtils.ComboBox.Box((int)autopilot.ascentPathIdxPublic, autopilot.ascentPathList, this);
GUILayout.EndHorizontal();
if (autopilot.ascentMenu != null)
{
autopilot.ascentMenu.enabled = GUILayout.Toggle(autopilot.ascentMenu.enabled, Localizer.Format("#MechJeb_Ascent_checkbox10")); //Edit ascent path
}
GUILayout.EndVertical();
base.WindowGUI(windowID);
}
private void converge()
{
if (p == null)
{
status = PVGStatus.INITIALIZING;
return;
}
if (p.last_success_time > 0)
{
last_success_time = p.last_success_time;
}
// FIXME: should make this use wall clock time rather than simulation time and drop it down to
// 10 seconds or so (phys warp means that this timeout could be 1/4 of the wall time and the
// optimizer may commonly take 3-4 seconds to reconverge in a suboptimal setting -- TF failure case, etc)
if (p.running_time(vesselState.time) > 30)
{
p.KillThread();
p.last_failure_cause = "Optimizer watchdog timeout"; // bit dirty poking other people's data
}
if (p.solution == null)
{
/* we have a solver but no solution */
status = PVGStatus.INITIALIZING;
}
else
{
/* we have a solver and have a valid solution */
if (isTerminalGuidance())
{
return;
}
/* hardcoded 10 seconds of terminal guidance */
if (tgo < 10)
{
// drop out of warp for terminal guidance (smaller time ticks => more accuracy)
core.warp.MinimumWarp();
status = PVGStatus.TERMINAL;
return;
}
}
if ((vesselState.time - last_optimizer_time) < MuUtils.Clamp(pvgInterval, 1.00, 30.00))
{
return;
}
// if we have unstable ullage then continuously update the "staging" timer until we are not
if ((vesselState.lowestUllage < VesselState.UllageState.Stable) && !isCoasting())
{
last_stage_time = vesselState.time;
}
if (p.solution != null)
{
// The current_tgo is the "booster" stage of the solution, it is allowed to go negative, for staging freeze
// running the optimizer for 4 seconds on either side of staging.
if (Math.Abs(p.solution.current_tgo(vesselState.time)) < 4)
{
return;
}
// Also if we just triggered a KSP stage separation or just started coasting, then wait for 4 seconds for
// stats to settle before running the optimizer again.
if (vesselState.time < last_stage_time + 4)
{
return;
}
}
p.threadStart(vesselState.time);
//if ( p.threadStart(vesselState.time) )
//Debug.Log("MechJeb: started optimizer thread");
if (status == PVGStatus.INITIALIZING && p.solution != null)
{
status = PVGStatus.CONVERGED;
}
last_optimizer_time = vesselState.time;
}
/// <summary>
/// This is the implementation function of the single impulse transfer from an elliptical, non-coplanar parking orbit.
///
/// It could be called directly with e.g. rotation of zero to bypass the line search for the rotation which wil be nearly
/// optimal in many cases, but fails in the kinds of nearly coplanar conditions which are common in KSP.
/// </summary>
///
/// <param name="mu">Gravitational parameter of central body.</param>
/// <param name="r0">Reference position on the parking orbit.</param>
/// <param name="v0">Reference velocity on the parking orbit.</param>
/// <param name="v_inf">Target hyperbolic v-infinity vector.</param>
/// <param name="vneg">Velocity on the parking orbit before the burn.</param>
/// <param name="vpos">Velocity on the hyperboliic ejection orbit after the burn.</param>
/// <param name="r">Position of the burn.</param>
/// <param name="dt">Coasting time on the parking orbit from the reference to the burn.</param>
/// <param name="rot">Rotation of hf_hat around v_inf_hat (or r1_hat around h0_hat) [degrees].</param>
///
public static void singleImpulseHyperbolicBurn(double mu, Vector3d r0, Vector3d v0, Vector3d v_inf,
ref Vector3d vneg, ref Vector3d vpos, ref Vector3d r, ref double dt, float rot, bool debug = false)
{
if (debug)
{
Debug.Log("[MechJeb] singleImpulseHyperbolicBurn mu = " + mu + " r0 = " + r0 + " v0 = " + v0 + " v_inf = " + v_inf);
}
// angular momentum of the parking orbit
Vector3d h0 = Vector3d.Cross(r0, v0);
// semi major axis of parking orbit
double a0 = 1.0 / (2.0 / r0.magnitude - v0.sqrMagnitude / mu);
// sma of hyperbolic ejection orbit
double af = -mu / v_inf.sqrMagnitude;
// parking orbit angular momentum unit
Vector3d h0_hat = h0 / h0.magnitude;
// eccentricity vector of the parking orbit
Vector3d ecc = Vector3d.Cross(v0, h0) / mu - r0 / r0.magnitude;
// eccentricity of the parking orbit.
double e0 = ecc.magnitude;
// semilatus rectum of parking orbit
double p0 = a0 * (1 - e0 * e0);
// parking orbit periapsis position unit vector
Vector3d rp0_hat;
if (Math.Abs(e0) > 1e-14)
{
rp0_hat = ecc / e0;
}
else
{
rp0_hat = r0 / r0.magnitude;
}
// parking orbit periapsis velocity unit vector
Vector3d vp0_hat = Vector3d.Cross(h0, rp0_hat).normalized;
// direction of hyperbolic v-infinity vector
Vector3d v_inf_hat = v_inf.normalized;
// 2 cases for finding hf_hat
Vector3d hf_hat;
if (Math.Abs(Vector3d.Dot(h0_hat, v_inf_hat)) == 1)
{
// 90 degree plane change case
hf_hat = Vector3d.Cross(rp0_hat, v_inf_hat);
hf_hat = hf_hat.normalized;
}
else
{
// general case
hf_hat = Vector3d.Cross(v_inf_hat, Vector3d.Cross(h0_hat, v_inf_hat));
hf_hat = hf_hat.normalized;
}
Vector3d r1_hat;
if (Math.Abs(Vector3d.Dot(h0_hat, v_inf_hat)) > 2.22044604925031e-16)
{
// if the planes are not coincident, rotate hf_hat by applying rodriguez formula around v_inf_hat
hf_hat = Quaternion.AngleAxis(rot, v_inf_hat) * hf_hat;
// unit vector pointing at the position of the burn on the parking orbit
r1_hat = Math.Sign(Vector3d.Dot(h0_hat, v_inf_hat)) * Vector3d.Cross(h0_hat, hf_hat).normalized;
}
else
{
// unit vector pointing at the position of the burn on the parking orbit
r1_hat = Vector3d.Cross(v_inf_hat, hf_hat).normalized;
// if the planes are coincident, rotate r1_hat by applying rodriguez formula around h0_hat
r1_hat = Quaternion.AngleAxis(rot, h0_hat) * r1_hat;
}
// true anomaly of r1 on the parking orbit
double nu_10 = Math.Sign(Vector3d.Dot(h0_hat, Vector3d.Cross(rp0_hat, r1_hat))) * Math.Acos(Vector3d.Dot(rp0_hat, r1_hat));
// length of the position vector of the burn on the parking orbit
double r1 = p0 / (1 + e0 * Math.Cos(nu_10));
// position of the burn
r = r1 * r1_hat;
// constant
double k = -af / r1;
// angle between v_inf and the r1 burn
double delta_nu = Math.Acos(MuUtils.Clamp(Vector3d.Dot(r1_hat, v_inf_hat), -1, 1));
// eccentricity of the hyperbolic ejection orbit
double sindnu = Math.Sin(delta_nu);
double sin2dnu = sindnu * sindnu;
double cosdnu = Math.Cos(delta_nu);
double ef = Math.Max(Math.Sqrt(sin2dnu + 2 * k * k + 2 * k * (1 - cosdnu) + sindnu * Math.Sqrt(sin2dnu + 4 * k * (1 - cosdnu))) / (Math.Sqrt(2) * k), 1 + MuUtils.DBL_EPSILON);
// semilatus rectum of hyperbolic ejection orbit
double pf = af * (1 - ef * ef);
// true anomaly of the v_inf on the hyperbolic ejection orbit
double nu_inf = Math.Acos(-1 / ef);
// true anomaly of the burn on the hyperbolic ejection orbit
double nu_1f = Math.Acos(MuUtils.Clamp(-1 / ef * cosdnu + Math.Sqrt(ef * ef - 1) / ef * sindnu, -1, 1));
// turning angle of the hyperbolic orbit
double delta = 2 * Math.Asin(1 / ef);
// incoming hyperbolic velocity unit vector
Vector3d v_inf_minus_hat = Math.Cos(delta) * v_inf_hat + Math.Sin(delta) * Vector3d.Cross(v_inf_hat, hf_hat);
// periapsis position and velocity vectors of the hyperbolic ejection orbit
Vector3d rpf_hat = v_inf_minus_hat - v_inf_hat;
rpf_hat = rpf_hat / rpf_hat.magnitude;
Vector3d vpf_hat = v_inf_minus_hat + v_inf_hat;
vpf_hat = vpf_hat / vpf_hat.magnitude;
// compute the velocity on the hyperbola and the parking orbit
vpos = Math.Sqrt(mu / pf) * (-Math.Sin(nu_1f) * rpf_hat + (ef + Math.Cos(nu_1f)) * vpf_hat);
vneg = Math.Sqrt(mu / p0) * (-Math.Sin(nu_10) * rp0_hat + (e0 + Math.Cos(nu_10)) * vp0_hat);
// compute nu of the reference position on the parking orbit
Vector3d r0_hat = r0 / r0.magnitude;
double nu0 = Math.Sign(Vector3d.Dot(h0_hat, Vector3d.Cross(rp0_hat, r0_hat))) * Math.Acos(Vector3d.Dot(rp0_hat, r0_hat));
// mean angular motion of the parking orbit
double n = 1 / Math.Sqrt(a0 * a0 * a0 / mu);
// eccentric anomalies of reference position and r1 on the parking orbit
double E0 = Math.Atan2(Math.Sqrt(1 - e0 * e0) * Math.Sin(nu0), e0 + Math.Cos(nu0));
double E1 = Math.Atan2(Math.Sqrt(1 - e0 * e0) * Math.Sin(nu_10), e0 + Math.Cos(nu_10));
// mean anomalies of reference position and r1 on the parking orbit
double M0 = E0 - e0 * Math.Sin(E0);
double M1 = E1 - e0 * Math.Sin(E1);
// coast time on the parking orbit
dt = (M1 - M0) / n;
if (dt < 0)
{
dt += 2 * Math.PI / n;
}
if (debug)
{
Debug.Log("[MechJeb] singleImpulseHyperbolicBurn vneg = " + vneg + " vpos = " + vpos + " r = " + r + " dt = " + dt);
}
}
public override void Drive(FlightCtrlState s)
{
UpdatePID();
//SpeedHold (set AccelerationTarget automatically to hold speed)
if (SpeedHoldEnabled)
{
double spd = vesselState.speedSurface;
cur_acc = (spd - _spd) / Time.fixedDeltaTime;
_spd = spd;
RealAccelerationTarget = (SpeedTarget - spd) / 4;
a_err = (RealAccelerationTarget - cur_acc);
AccelerationPIDController.intAccum = MuUtils.Clamp(AccelerationPIDController.intAccum, -1 / AccKi, 1 / AccKi);
double t_act = AccelerationPIDController.Compute(a_err);
if (!double.IsNaN(t_act))
{
core.thrust.targetThrottle = (float)MuUtils.Clamp(t_act, 0, 1);
}
else
{
core.thrust.targetThrottle = 0.0f;
AccelerationPIDController.Reset();
}
}
//AltitudeHold (set VertSpeed automatically to hold altitude)
if (AltitudeHoldEnabled)
{
RealVertSpeedTarget = convertAltitudeToVerticalSpeed(AltitudeTarget - vesselState.altitudeASL);
RealVertSpeedTarget = UtilMath.Clamp(RealVertSpeedTarget, -VertSpeedTarget, VertSpeedTarget);
}
else
{
RealVertSpeedTarget = VertSpeedTarget;
}
pitch_err = roll_err = yaw_err = 0;
pitch_act = roll_act = yaw_act = 0;
//VertSpeedHold
if (VertSpeedHoldEnabled)
{
// NOTE: 60-to-1 rule:
// deltaAltitude = 2 * PI * r * deltaPitch / 360
// Vvertical = 2 * PI * TAS * deltaPitch / 360
// deltaPitch = Vvertical / Vhorizontal * 180 / PI
double deltaVertSpeed = RealVertSpeedTarget - vesselState.speedVertical;
double adjustment = 180 / vesselState.speedSurface * deltaVertSpeed / Math.PI;
RealPitchTarget = vesselState.vesselPitch + adjustment;
RealPitchTarget = UtilMath.Clamp(RealPitchTarget, -PitchDownLimit, PitchUpLimit);
pitch_err = MuUtils.ClampDegrees180(RealPitchTarget - vesselState.vesselPitch);
PitchPIDController.intAccum = UtilMath.Clamp(PitchPIDController.intAccum, -100 / PitKi, 100 / PitKi);
pitch_act = PitchPIDController.Compute(pitch_err) / 100;
//Debug.Log (p_act);
if (double.IsNaN(pitch_act))
{
PitchPIDController.Reset();
}
else
{
s.pitch = Mathf.Clamp((float)pitch_act, -1, 1);
}
}
//HeadingHold
double curFlightPath = vesselState.HeadingFromDirection(vesselState.forward);
// NOTE: we can not use vesselState.vesselHeading here because it interpolates headings internally
// i.e. turning from 1° to 359° will end up as (1+359)/2 = 180°
// see class MovingAverage for more details
curr_yaw = vesselState.currentHeading - curFlightPath;
if (HeadingHoldEnabled)
{
double toturn = MuUtils.ClampDegrees180(HeadingTarget - curFlightPath);
if (Math.Abs(toturn) < 0.2)
{
// yaw for small adjustments
RealYawTarget = MuUtils.Clamp(toturn * 2, -YawLimit, YawLimit);
RealRollTarget = 0;
}
else
{
// roll for large adjustments
RealYawTarget = 0;
RealRollTarget = MuUtils.Clamp(toturn * 2, -RollLimit, RollLimit);
}
}
else
{
RealRollTarget = RollTarget;
RealYawTarget = 0;
}
if (RollHoldEnabled)
{
RealRollTarget = UtilMath.Clamp(RealRollTarget, -BankAngle, BankAngle);
RealRollTarget = UtilMath.Clamp(RealRollTarget, -RollLimit, RollLimit);
roll_err = MuUtils.ClampDegrees180(RealRollTarget - -vesselState.currentRoll);
RollPIDController.intAccum = MuUtils.Clamp(RollPIDController.intAccum, -100 / RolKi, 100 / RolKi);
roll_act = RollPIDController.Compute(roll_err) / 100;
if (double.IsNaN(roll_act))
{
RollPIDController.Reset();
}
else
{
s.roll = Mathf.Clamp((float)roll_act, -1, 1);
}
}
if (HeadingHoldEnabled)
{
RealYawTarget = UtilMath.Clamp(RealYawTarget, -YawLimit, YawLimit);
yaw_err = MuUtils.ClampDegrees180(RealYawTarget - curr_yaw);
YawPIDController.intAccum = MuUtils.Clamp(YawPIDController.intAccum, -100 / YawKi, 100 / YawKi);
yaw_act = YawPIDController.Compute(yaw_err) / 100;
if (double.IsNaN(yaw_act))
{
YawPIDController.Reset();
}
else
{
s.yaw = Mathf.Clamp((float)yaw_act, -1, 1);
}
}
}
public override void Drive(FlightCtrlState s)
{
UpdatePID();
//SpeedHold (set AccelerationTarget automatically to hold speed)
if (SpeedHoldEnabled)
{
double spd = vesselState.speedSurface;
cur_acc = (spd - _spd) / Time.fixedDeltaTime;
_spd = spd;
RealAccelerationTarget = (SpeedTarget - spd) / 4;
a_err = (RealAccelerationTarget - cur_acc);
AccelerationPIDController.intAccum = MuUtils.Clamp(AccelerationPIDController.intAccum, -1 / AccKi, 1 / AccKi);
double t_act = AccelerationPIDController.Compute(a_err);
if (!double.IsNaN(t_act))
{
core.thrust.targetThrottle = (float)MuUtils.Clamp(t_act, 0, 1);
}
else
{
core.thrust.targetThrottle = 0.0f;
AccelerationPIDController.Reset();
}
}
//AltitudeHold (set VertSpeed automatically to hold altitude)
if (AltitudeHoldEnabled)
{
RealVertSpeedTarget = MuUtils.Clamp(fixVertSpeed(AltitudeTarget - vesselState.altitudeASL), -VertSpeedMax, VertSpeedMax);
}
else
{
RealVertSpeedTarget = VertSpeedTarget;
}
//VertSpeedHold
if (VertSpeedHoldEnabled)
{
double vertspd = vesselState.speedVertical;
v_err = RealVertSpeedTarget - vertspd;
VertSpeedPIDController.intAccum = MuUtils.Clamp(VertSpeedPIDController.intAccum, -1 / (VerKi * VerPIDScale), 1 / (VerKi * VerPIDScale));
double p_act = VertSpeedPIDController.Compute(v_err);
//Log.dbg(p_act);
if (double.IsNaN(p_act))
{
VertSpeedPIDController.Reset();
}
else
{
s.pitch = Mathf.Clamp((float)p_act, -1, 1);
}
}
//HeadingHold
double curHeading = vesselState.HeadingFromDirection(vesselState.forward);
if (HeadingHoldEnabled)
{
double toturn = MuUtils.ClampDegrees180(HeadingTarget - curHeading);
RealRollTarget = MuUtils.Clamp(toturn * 2, -RollMax, RollMax);
}
else
{
RealRollTarget = RollTarget;
}
if (RollHoldEnabled)
{
double roll_err = MuUtils.ClampDegrees180(vesselState.vesselRoll + RealRollTarget);
RollPIDController.intAccum = MuUtils.Clamp(RollPIDController.intAccum, -100 / AccKi, 100 / AccKi);
double roll_act = RollPIDController.Compute(roll_err);
s.roll = Mathf.Clamp((float)roll_act / 100, -1, 1);
}
if (HeadingHoldEnabled)
{
double yaw_err = MuUtils.ClampDegrees180(HeadingTarget - curHeading);
YawPIDController.intAccum = MuUtils.Clamp(YawPIDController.intAccum, -YawLimit * 100 / AccKi, YawLimit * 100 / AccKi);
double yaw_act = YawPIDController.Compute(yaw_err);
s.yaw = (float)MuUtils.Clamp(yaw_act / 100, -YawLimit, +YawLimit);
}
}
// provides AoA limiting and roll control
// provides no ground tracking and should only be called by autopilots like PVG that deeply know what they're doing with yaw control
// (possibly should be moved into the attitude controller, but right now it collaborates too heavily with the ascent autopilot)
//
protected void attitudeTo(double desiredPitch, double desiredHeading)
{
/*
* Vector6 rcs = vesselState.rcsThrustAvailable;
*
* // FIXME? should this be up/down and not forward/back? seems wrong? why was i using down before for the ullage direction?
* bool has_rcs = vessel.hasEnabledRCSModules() && vessel.ActionGroups[KSPActionGroup.RCS] && ( rcs.left > 0.01 ) && ( rcs.right > 0.01 ) && ( rcs.forward > 0.01 ) && ( rcs.back > 0.01 );
*
* if ( (vesselState.thrustCurrent / vesselState.thrustAvailable < 0.50) && !has_rcs )
* {
* // if engines are spooled up at less than 50% and we have no RCS in the stage, do not issue any guidance commands yet
* return;
* }
*/
Vector3d desiredHeadingVector = Math.Sin(desiredHeading * UtilMath.Deg2Rad) * vesselState.east + Math.Cos(desiredHeading * UtilMath.Deg2Rad) * vesselState.north;
Vector3d desiredThrustVector = Math.Cos(desiredPitch * UtilMath.Deg2Rad) * desiredHeadingVector
+ Math.Sin(desiredPitch * UtilMath.Deg2Rad) * vesselState.up;
desiredThrustVector = desiredThrustVector.normalized;
thrustVectorForNavball = desiredThrustVector;
/* old style AoA limiter */
if (autopilot.limitAoA && !autopilot.limitQaEnabled)
{
float fade = vesselState.dynamicPressure < autopilot.aoALimitFadeoutPressure ? (float)(autopilot.aoALimitFadeoutPressure / vesselState.dynamicPressure) : 1;
autopilot.currentMaxAoA = Math.Min(fade * autopilot.maxAoA, 180d);
autopilot.limitingAoA = vessel.altitude <mainBody.atmosphereDepth && Vector3.Angle(vesselState.surfaceVelocity, desiredThrustVector)> autopilot.currentMaxAoA;
if (autopilot.limitingAoA)
{
desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(autopilot.currentMaxAoA * Mathf.Deg2Rad), 1).normalized;
}
}
/* AoA limiter for PVG */
if (autopilot.limitQaEnabled)
{
double lim = MuUtils.Clamp(autopilot.limitQa, 100, 10000);
autopilot.limitingAoA = vesselState.dynamicPressure * Vector3.Angle(vesselState.surfaceVelocity, desiredThrustVector) * UtilMath.Deg2Rad > lim;
if (autopilot.limitingAoA)
{
autopilot.currentMaxAoA = lim / vesselState.dynamicPressure * UtilMath.Rad2Deg;
desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(autopilot.currentMaxAoA * UtilMath.Deg2Rad), 1).normalized;
}
}
double pitch = 90 - Vector3d.Angle(desiredThrustVector, vesselState.up);
double hdg;
if (pitch > 89.9)
{
hdg = desiredHeading;
}
else
{
hdg = MuUtils.ClampDegrees360(UtilMath.Rad2Deg * Math.Atan2(Vector3d.Dot(desiredThrustVector, vesselState.east), Vector3d.Dot(desiredThrustVector, vesselState.north)));
}
if (autopilot.forceRoll)
{
core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && (vesselState.altitudeBottom > 50));
if (desiredPitch == 90.0)
{
core.attitude.attitudeTo(hdg, pitch, autopilot.verticalRoll, this, !vessel.Landed, !vessel.Landed, !vessel.Landed && (vesselState.altitudeBottom > 50));
}
else
{
core.attitude.attitudeTo(hdg, pitch, autopilot.turnRoll, this, !vessel.Landed, !vessel.Landed, !vessel.Landed && (vesselState.altitudeBottom > 50));
}
}
else
{
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
}
}
public void WarpToUT(double UT, double maxRate = -1)
{
if (UT <= vesselState.time)
{
warpToUT = 0.0;
return;
}
if (maxRate < 0)
{
maxRate = TimeWarp.fetch.warpRates[TimeWarp.fetch.warpRates.Length - 1];
}
double desiredRate;
if (useQuickWarp)
{
desiredRate = 1;
if (orbit.patchEndTransition != Orbit.PatchTransitionType.FINAL && orbit.EndUT < UT)
{
for (int i = 0; i < TimeWarp.fetch.warpRates.Length; i++)
{
if (i * Time.fixedDeltaTime * TimeWarp.fetch.warpRates[i] <= orbit.EndUT - vesselState.time)
{
desiredRate = TimeWarp.fetch.warpRates[i] + 0.1;
}
else
{
break;
}
}
}
else
{
for (int i = 0; i < TimeWarp.fetch.warpRates.Length; i++)
{
if (i * Time.fixedDeltaTime * TimeWarp.fetch.warpRates[i] <= UT - vesselState.time)
{
desiredRate = TimeWarp.fetch.warpRates[i] + 0.1;
}
else
{
break;
}
}
}
}
else
{
desiredRate = 1.0 * (UT - (vesselState.time + Time.fixedDeltaTime * (float)TimeWarp.CurrentRateIndex));
}
desiredRate = MuUtils.Clamp(desiredRate, 1, maxRate);
if (!vessel.LandedOrSplashed &&
vesselState.altitudeASL < TimeWarp.fetch.GetAltitudeLimit(1, mainBody))
{
//too low to use any regular warp rates. Use physics warp at a max of x2:
WarpPhysicsAtRate((float)Math.Min(desiredRate, 2));
}
else
{
WarpRegularAtRate((float)desiredRate, useQuickWarp, useQuickWarp);
}
warpToUT = UT;
}
protected override void WindowGUI(int windowID)
{
GUILayout.BeginVertical();
GUILayout.Label("When guidance is enabled, the purple circle on the navball points along the ascent path.");
ToggleAscentNavballGuidanceInfoItem();
if (autopilot != null)
{
if (autopilot.enabled)
{
if (GUILayout.Button("Disengage autopilot"))
{
autopilot.users.Remove(this);
}
}
else
{
if (GUILayout.Button("Engage autopilot"))
{
autopilot.users.Add(this);
}
}
GuiUtils.SimpleTextBox("Orbit altitude", autopilot.desiredOrbitAltitude, "km");
autopilot.desiredInclination = desiredInclination;
GUIStyle si = new GUIStyle(GUI.skin.label);
if (!autopilot.enabled && Math.Abs(desiredInclination) < Math.Abs(vesselState.latitude))
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Orange;
}
GUILayout.BeginHorizontal();
GUILayout.Label("Orbit inc.", si, GUILayout.ExpandWidth(true));
desiredInclination.text = GUILayout.TextField(desiredInclination.text, GUILayout.ExpandWidth(true), GUILayout.Width(100));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
if (GUILayout.Button("Current"))
{
desiredInclination.val = vesselState.latitude;
}
GUILayout.EndHorizontal();
core.thrust.LimitToPreventOverheatsInfoItem();
//core.thrust.LimitToTerminalVelocityInfoItem();
core.thrust.LimitToMaxDynamicPressureInfoItem();
core.thrust.LimitAccelerationInfoItem();
core.thrust.LimitThrottleInfoItem();
core.thrust.LimiterMinThrottleInfoItem();
core.thrust.LimitElectricInfoItem();
GUILayout.BeginHorizontal();
autopilot.forceRoll = GUILayout.Toggle(autopilot.forceRoll, "Force Roll");
if (autopilot.forceRoll)
{
GuiUtils.SimpleTextBox("climb", autopilot.verticalRoll, "º", 30f);
GuiUtils.SimpleTextBox("turn", autopilot.turnRoll, "º", 30f);
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUIStyle s = new GUIStyle(GUI.skin.toggle);
if (autopilot.limitingAoA)
{
s.onHover.textColor = s.onNormal.textColor = Color.green;
}
autopilot.limitAoA = GUILayout.Toggle(autopilot.limitAoA, "Limit AoA to", s, GUILayout.ExpandWidth(true));
autopilot.maxAoA.text = GUILayout.TextField(autopilot.maxAoA.text, GUILayout.Width(30));
GUILayout.Label("º (" + autopilot.currentMaxAoA.ToString("F1") + "°)", GUILayout.ExpandWidth(true));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Space(25);
if (autopilot.limitAoA)
{
GUIStyle sl = new GUIStyle(GUI.skin.label);
if (autopilot.limitingAoA && vesselState.dynamicPressure < autopilot.aoALimitFadeoutPressure)
{
sl.normal.textColor = sl.hover.textColor = Color.green;
}
GuiUtils.SimpleTextBox("Dynamic Pressure Fadeout", autopilot.aoALimitFadeoutPressure, "pa", 50, sl);
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
autopilot.correctiveSteering = GUILayout.Toggle(autopilot.correctiveSteering, "Corrective steering", GUILayout.ExpandWidth(false));
if (autopilot.correctiveSteering)
{
GUILayout.Label("Gain", GUILayout.ExpandWidth(false));
autopilot.correctiveSteeringGain.text = GUILayout.TextField(autopilot.correctiveSteeringGain.text, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
autopilot.autostage = GUILayout.Toggle(autopilot.autostage, "Autostage");
if (autopilot.autostage)
{
core.staging.AutostageSettingsInfoItem();
}
autopilot.autodeploySolarPanels = GUILayout.Toggle(autopilot.autodeploySolarPanels,
"Auto-deploy solar panels");
autopilot.autoDeployAntennas = GUILayout.Toggle(autopilot.autoDeployAntennas,
"Auto-deploy antennas");
GUILayout.BeginHorizontal();
core.node.autowarp = GUILayout.Toggle(core.node.autowarp, "Auto-warp");
autopilot.skipCircularization = GUILayout.Toggle(autopilot.skipCircularization, "Skip Circularization");
GUILayout.EndHorizontal();
if (vessel.LandedOrSplashed)
{
if (core.target.NormalTargetExists)
{
if (core.node.autowarp)
{
GUILayout.BeginHorizontal();
GUILayout.Label("Launch countdown:", GUILayout.ExpandWidth(true));
autopilot.warpCountDown.text = GUILayout.TextField(autopilot.warpCountDown.text,
GUILayout.Width(60));
GUILayout.Label("s", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
if (!launchingToPlane && !launchingToRendezvous && !launchingToInterplanetary)
{
GUILayout.BeginHorizontal();
if (GUILayout.Button("Launch to rendezvous:", GUILayout.ExpandWidth(false)))
{
launchingToRendezvous = true;
autopilot.StartCountdown(vesselState.time +
LaunchTiming.TimeToPhaseAngle(autopilot.launchPhaseAngle,
mainBody, vesselState.longitude, core.target.TargetOrbit));
}
autopilot.launchPhaseAngle.text = GUILayout.TextField(autopilot.launchPhaseAngle.text,
GUILayout.Width(60));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button("Launch into plane of target", GUILayout.ExpandWidth(false)))
{
launchingToPlane = true;
autopilot.StartCountdown(vesselState.time +
LaunchTiming.TimeToPlane(autopilot.launchLANDifference,
mainBody, vesselState.latitude, vesselState.longitude,
core.target.TargetOrbit));
}
autopilot.launchLANDifference.text = GUILayout.TextField(
autopilot.launchLANDifference.text, GUILayout.Width(60));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
if (core.target.TargetOrbit.referenceBody == orbit.referenceBody.referenceBody)
{
if (GUILayout.Button("Launch at interplanetary window"))
{
launchingToInterplanetary = true;
//compute the desired launch date
OrbitalManeuverCalculator.DeltaVAndTimeForHohmannTransfer(mainBody.orbit,
core.target.TargetOrbit, vesselState.time, out interplanetaryWindowUT);
double desiredOrbitPeriod = 2 * Math.PI *
Math.Sqrt(
Math.Pow(mainBody.Radius + autopilot.desiredOrbitAltitude, 3)
/ mainBody.gravParameter);
//launch just before the window, but don't try to launch in the past
interplanetaryWindowUT -= 3 * desiredOrbitPeriod;
interplanetaryWindowUT = Math.Max(vesselState.time + autopilot.warpCountDown,
interplanetaryWindowUT);
autopilot.StartCountdown(interplanetaryWindowUT);
}
}
}
}
else
{
launchingToInterplanetary = launchingToPlane = launchingToRendezvous = false;
GUILayout.Label("Select a target for a timed launch.");
}
if (launchingToInterplanetary || launchingToPlane || launchingToRendezvous)
{
string message = "";
if (launchingToInterplanetary)
{
message = "Launching at interplanetary window";
}
else if (launchingToPlane)
{
// FIXME: When plane matching azimuth autopilot is available, this clamping can be removed
desiredInclination = MuUtils.Clamp(core.target.TargetOrbit.inclination, Math.Abs(vesselState.latitude), 180 - Math.Abs(vesselState.latitude));
desiredInclination *=
Math.Sign(Vector3d.Dot(core.target.TargetOrbit.SwappedOrbitNormal(),
Vector3d.Cross(vesselState.CoM - mainBody.position, mainBody.transform.up)));
message = "Launching to target plane";
}
else if (launchingToRendezvous)
{
message = "Launching to rendezvous";
}
if (autopilot.tMinus > 3 * vesselState.deltaT)
{
message += ": T-" + GuiUtils.TimeToDHMS(autopilot.tMinus, 1);
}
GUILayout.Label(message);
if (GUILayout.Button("Abort"))
{
launchingToInterplanetary =
launchingToPlane = launchingToRendezvous = autopilot.timedLaunch = false;
}
}
}
if (autopilot.enabled)
{
GUILayout.Label("Autopilot status: " + autopilot.status);
}
}
if (!vessel.patchedConicsUnlocked())
{
GUILayout.Label("Warning: MechJeb is unable to circularize without an upgraded Tracking Station.");
}
int last_idx = ascentPathIdx;
GUILayout.BeginHorizontal();
ascentPathIdx = GuiUtils.ComboBox.Box(ascentPathIdx, ascentPathList, this);
GUILayout.EndHorizontal();
if (last_idx != ascentPathIdx)
{
bool last_enabled = editor.enabled;
enable_path_module(ascentPathIdx);
editor.enabled = last_enabled;
}
if (editor != null)
{
editor.enabled = GUILayout.Toggle(editor.enabled, "Edit ascent path");
}
GUILayout.EndVertical();
base.WindowGUI(windowID);
}
