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