From 2d16c69a460110b8a3139bd4e7532bf7c56a1358 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sun, 25 Jun 2017 11:16:10 -0700 Subject: [PATCH 01/51] Ascent refactoring and IGM and GT modes Work-in-Progress checkpoint. Signed-off-by: Lamont Granquist --- MechJeb2/MechJebCore.cs | 13 +- MechJeb2/MechJebModuleAscentAutopilot.cs | 452 ++++++------------ MechJeb2/MechJebModuleAscentClassic.cs | 263 ++++++++++ ...r.cs => MechJebModuleAscentClassicMenu.cs} | 429 ++++++++--------- MechJeb2/MechJebModuleAscentGT.cs | 238 +++++++++ MechJeb2/MechJebModuleAscentGTMenu.cs | 54 +++ MechJeb2/MechJebModuleAscentGuidance.cs | 136 ++++-- MechJeb2/MechJebModuleAscentIGM.cs | 352 ++++++++++++++ MechJeb2/MechJebModuleAscentIGMMenu.cs | 52 ++ MechJeb2/MechJebModuleAscentNavBall.cs | 7 +- MechJeb2/MechJebModuleAttitudeController.cs | 10 +- MechJeb2/MechJebModuleFlightRecorderGraph.cs | 32 +- MechJeb2/OrbitalManeuverCalculator.cs | 4 +- .../MechJebModuleScriptActionAscent.cs | 3 +- 14 files changed, 1447 insertions(+), 598 deletions(-) create mode 100644 MechJeb2/MechJebModuleAscentClassic.cs rename MechJeb2/{MechJebModuleAscentPathEditor.cs => MechJebModuleAscentClassicMenu.cs} (92%) create mode 100644 MechJeb2/MechJebModuleAscentGT.cs create mode 100644 MechJeb2/MechJebModuleAscentGTMenu.cs create mode 100644 MechJeb2/MechJebModuleAscentIGM.cs create mode 100644 MechJeb2/MechJebModuleAscentIGMMenu.cs diff --git a/MechJeb2/MechJebCore.cs b/MechJeb2/MechJebCore.cs index a183fc77f..0b42a8e33 100644 --- a/MechJeb2/MechJebCore.cs +++ b/MechJeb2/MechJebCore.cs @@ -82,7 +82,7 @@ public bool DeactivateControl [KSPField(isPersistant = false)] public bool eduMode = false; - + public bool rssMode { get { return settings.rssMode; } } [KSPAction("Orbit Prograde")] @@ -306,7 +306,7 @@ private void SetTranslatronSpeed(float speed, bool relative = false) public bool someModuleAreLocked = false; // True if any module was locked by the R&D system - //Returns whether the vessel we've registered OnFlyByWire with is the correct one. + //Returns whether the vessel we've registered OnFlyByWire with is the correct one. //If it isn't the correct one, fixes it before returning false bool CheckControlledVessel() { @@ -461,7 +461,7 @@ public override void OnStart(PartModule.StartState state) ready = true; if (state == PartModule.StartState.None) return; //don't do anything when we start up in the loading screen - //OnLoad doesn't get called for parts created in editor, so do that manually so + //OnLoad doesn't get called for parts created in editor, so do that manually so //that we can load global settings. //However, if you press ctrl-Z, a new PartModule object gets created, on which the //game DOES call OnLoad, and then OnStart. So before calling OnLoad from OnStart, @@ -676,7 +676,7 @@ public void Update() Profiler.BeginSample("OnMenuUpdate"); GetComputerModule().OnMenuUpdate(); // Allow the menu movement, even while in Editor Profiler.EndSample(); - + if (vessel == null) { Profiler.EndSample(); @@ -708,7 +708,7 @@ void LoadComputerModules() { try { - foreach (var module in (from t in ass.GetTypes() where t.IsSubclassOf(typeof(ComputerModule)) select t).ToList()) + foreach (var module in (from t in ass.GetTypes() where t.IsSubclassOf(typeof(ComputerModule)) && !t.IsAbstract select t).ToList()) { moduleRegistry.Add(module); } @@ -911,7 +911,7 @@ public override void OnSave(ConfigNode sfsNode) // Only Masters can save if (this != vessel.GetMasterMechJeb()) return; - //KSP calls OnSave *before* OnLoad when the first command pod is created in the editor. + //KSP calls OnSave *before* OnLoad when the first command pod is created in the editor. //Defend against saving empty settings. if (unorderedComputerModules.Count == 0) return; @@ -1190,4 +1190,3 @@ public new static void print(object message) } } } - diff --git a/MechJeb2/MechJebModuleAscentAutopilot.cs b/MechJeb2/MechJebModuleAscentAutopilot.cs index 0910540b7..e8f3861b8 100644 --- a/MechJeb2/MechJebModuleAscentAutopilot.cs +++ b/MechJeb2/MechJebModuleAscentAutopilot.cs @@ -16,7 +16,7 @@ public class MechJebModuleAscentAutopilot : ComputerModule //input parameters: [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public IAscentPath ascentPath = new DefaultAscentPath(); + public MechJebModuleAscentBase ascentPath; [Persistent(pass = (int)(Pass.Type | Pass.Global))] public EditableDoubleMult desiredOrbitAltitude = new EditableDoubleMult(100000, 1000); [Persistent(pass = (int)(Pass.Type | Pass.Global))] @@ -30,9 +30,9 @@ public class MechJebModuleAscentAutopilot : ComputerModule [Persistent(pass = (int)(Pass.Type | Pass.Global))] public bool forceRoll = true; [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDouble verticalRoll = new EditableDouble(0); + public EditableDouble verticalRoll = new EditableDouble(90); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDouble turnRoll = new EditableDouble(0); + public EditableDouble turnRoll = new EditableDouble(90); [Persistent(pass = (int)(Pass.Type | Pass.Global))] public bool autodeploySolarPanels = true; [Persistent(pass = (int)(Pass.Type | Pass.Global))] @@ -87,17 +87,14 @@ public double tMinus } //internal state: - enum AscentMode { RETRACT_SOLAR_PANELS, VERTICAL_ASCENT, GRAVITY_TURN, COAST_TO_APOAPSIS, CIRCULARIZE }; + enum AscentMode { PRELAUNCH, ASCEND, CIRCULARIZE }; AscentMode mode; bool placedCircularizeNode = false; private double lastTMinus = 999; public override void OnModuleEnabled() { - if (autodeploySolarPanels && mainBody.atmosphere) - mode = AscentMode.RETRACT_SOLAR_PANELS; - else - mode = AscentMode.VERTICAL_ASCENT; + mode = AscentMode.PRELAUNCH; placedCircularizeNode = false; @@ -150,279 +147,85 @@ public override void OnFixedUpdate() public override void Drive(FlightCtrlState s) { limitingAoA = false; + switch (mode) { - case AscentMode.RETRACT_SOLAR_PANELS: - DriveRetractSolarPanels(s); - break; - - case AscentMode.VERTICAL_ASCENT: - DriveVerticalAscent(s); - break; - - case AscentMode.GRAVITY_TURN: - DriveGravityTurn(s); + case AscentMode.PRELAUNCH: + DrivePrelaunch(s); break; - case AscentMode.COAST_TO_APOAPSIS: - DriveCoastToApoapsis(s); + case AscentMode.ASCEND: + DriveAscent(s); break; case AscentMode.CIRCULARIZE: DriveCircularizationBurn(s); break; } - } - - void DriveRetractSolarPanels(FlightCtrlState s) - { - if (autoThrottle) core.thrust.targetThrottle = 0.0f; - - core.attitude.AxisControl(false, false, false); - - if (timedLaunch && tMinus > 10.0) - { - status = "Pre Launch"; - return; - } - status = "Retracting solar panels"; - core.solarpanel.RetractAll(); - if (core.solarpanel.AllRetracted()) - mode = AscentMode.VERTICAL_ASCENT; } - void DriveVerticalAscent(FlightCtrlState s) + void DriveSolarPanels(FlightCtrlState s) { - if (timedLaunch) - { - status = "Awaiting liftoff"; - core.attitude.AxisControl(false, false, false); - return; - } - - if (!ascentPath.IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface)) mode = AscentMode.GRAVITY_TURN; - if (autoThrottle && orbit.ApA > desiredOrbitAltitude) mode = AscentMode.COAST_TO_APOAPSIS; - - //during the vertical ascent we just thrust straight up at max throttle - if (forceRoll) - { // pre-align roll unless correctiveSteering is active as it would just interfere with that - double desiredHeading = OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, desiredInclination, launchLatitude); - core.attitude.attitudeTo(desiredHeading, 90, verticalRoll, this); - } - else + if (autodeploySolarPanels) { - core.attitude.attitudeTo(Vector3d.up, AttitudeReference.SURFACE_NORTH, this); + if (vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude()) + core.solarpanel.ExtendAll(); + else + core.solarpanel.RetractAll(); } - - core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); - - if (autoThrottle) core.thrust.targetThrottle = 1.0F; - - if (!vessel.LiftedOff() || vessel.Landed) status = "Awaiting liftoff"; - else status = "Vertical ascent"; } - //data used by ThrottleToRaiseApoapsis - float raiseApoapsisLastThrottle = 0; - double raiseApoapsisLastApR = 0; - double raiseApoapsisLastUT = 0; - MovingAverage raiseApoapsisRatePerThrottle = new MovingAverage(3, 0); - - //gives a throttle setting that reduces as we approach the desired apoapsis - //so that we can precisely match the desired apoapsis instead of overshooting it - float ThrottleToRaiseApoapsis(double currentApR, double finalApR) + void DrivePrelaunch(FlightCtrlState s) { - float desiredThrottle; - - if (currentApR > finalApR + 5.0) - { - desiredThrottle = 0.0F; //done, throttle down - } - else if (orbit.ApA < mainBody.RealMaxAtmosphereAltitude()) - { - desiredThrottle = 1.0F; //throttle hard to escape atmosphere - } - else if (raiseApoapsisLastUT > vesselState.time - 1) - { - //reduce throttle as apoapsis nears target - double instantRatePerThrottle = (orbit.ApR - raiseApoapsisLastApR) / ((vesselState.time - raiseApoapsisLastUT) * raiseApoapsisLastThrottle); - instantRatePerThrottle = Math.Max(1.0, instantRatePerThrottle); //avoid problems from negative rates - raiseApoapsisRatePerThrottle.value = instantRatePerThrottle; - double desiredApRate = (finalApR - currentApR) / 1.0; - desiredThrottle = Mathf.Clamp((float)(desiredApRate / raiseApoapsisRatePerThrottle), 0.05F, 1.0F); + if (autoThrottle) { + Debug.Log("prelaunch killing throttle"); + core.thrust.targetThrottle = 0.0f; } - else - { - desiredThrottle = 1.0F; //no recent data point; just use max thrust. - } - - //record data for next frame - raiseApoapsisLastThrottle = desiredThrottle; - raiseApoapsisLastApR = orbit.ApR; - raiseApoapsisLastUT = vesselState.time; - - return desiredThrottle; - } - 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; - } + core.attitude.AxisControl(false, false, false); - //if we've fallen below the turn start altitude, go back to vertical ascent - if (ascentPath.IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface)) + if (timedLaunch && tMinus > 10.0) { - mode = AscentMode.VERTICAL_ASCENT; + status = "Pre Launch"; return; } - if (autoThrottle) + if (autodeploySolarPanels && mainBody.atmosphere) { - 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; + core.solarpanel.RetractAll(); + if (core.solarpanel.AllRetracted()) { + Debug.Log("Prelaunch -> Ascend"); + mode = AscentMode.ASCEND; } - } - - //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, launchLatitude); - 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) + else { - desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(currentMaxAoA * Mathf.Deg2Rad), 1).normalized; + status = "Retracting solar panels"; } + } else { + mode = AscentMode.ASCEND; } - - 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"; } - void DriveCoastToApoapsis(FlightCtrlState s) + void DriveAscent(FlightCtrlState s) { - core.thrust.targetThrottle = 0; - - double circularSpeed = OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, orbit.ApR); - double apoapsisSpeed = orbit.SwappedOrbitalVelocityAtUT(orbit.NextApoapsisTime(vesselState.time)).magnitude; - double circularizeBurnTime = (circularSpeed - apoapsisSpeed) / vesselState.limitedMaxThrustAccel; - - //Once we get above the atmosphere, plan and execute the circularization maneuver. - //For orbits near the edge of the atmosphere, we can't wait until we break the atmosphere - //to start the burn, so we also compare the timeToAp with the expected circularization burn time. - //if ((vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude()) - // || (vesselState.limitedMaxThrustAccel > 0 && orbit.timeToAp < circularizeBurnTime / 1.8)) - - // Sarbian : removed the special case for now. Some ship where turning while still in atmosphere - - if (vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude()) - { - if (autodeploySolarPanels) - core.solarpanel.ExtendAll(); - - mode = AscentMode.CIRCULARIZE; - core.warp.MinimumWarp(); - return; - } - - //if our apoapsis has fallen too far, resume the gravity turn - if (orbit.ApA < desiredOrbitAltitude - 1000.0) + if (timedLaunch) { - mode = AscentMode.GRAVITY_TURN; - core.warp.MinimumWarp(); + Debug.Log("Awaiting Liftoff"); + status = "Awaiting liftoff"; + core.attitude.AxisControl(false, false, false); return; } - //point prograde and thrust gently if our apoapsis falls below the target - //core.attitude.attitudeTo(Vector3d.forward, AttitudeReference.ORBIT, this); - - // Actually I have a better idea: Don't initiate orientation changes when there's a chance that our main engine - // might reignite. There won't be enough control authority to counteract that much momentum change. - // - Starwaster - core.thrust.targetThrottle = 0; - - double desiredHeading = UtilMath.Deg2Rad * OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, desiredInclination, launchLatitude); - Vector3d desiredHeadingVector = Math.Sin(desiredHeading) * vesselState.east + Math.Cos(desiredHeading) * vesselState.north; - double desiredFlightPathAngle = ascentPath.FlightPathAngle(vesselState.altitudeASL, vesselState.speedSurface); - - Vector3d desiredThrustVector = Math.Cos(desiredFlightPathAngle * UtilMath.Deg2Rad) * desiredHeadingVector - + Math.Sin(desiredFlightPathAngle * UtilMath.Deg2Rad) * vesselState.up; - + DriveSolarPanels(s); - core.attitude.attitudeTo(desiredThrustVector.normalized, AttitudeReference.INERTIAL, this); - if (autoThrottle && orbit.ApA < desiredOrbitAltitude) - { - core.attitude.attitudeTo(Vector3d.forward, AttitudeReference.INERTIAL, this); - core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, desiredOrbitAltitude + mainBody.Radius); - } - - if (core.node.autowarp) - { - //warp at x2 physical warp: - core.warp.WarpPhysicsAtRate(2); + if ( ascentPath.DriveAscent(s) ) { + Debug.Log("Remaining in Ascent"); + status = ascentPath.status; + } else { + Debug.Log("Ascend -> Circularize"); + mode = AscentMode.CIRCULARIZE; } - - status = "Coasting to edge of atmosphere"; } void DriveCircularizationBurn(FlightCtrlState s) @@ -433,6 +236,8 @@ void DriveCircularizationBurn(FlightCtrlState s) return; } + DriveSolarPanels(s); + if (placedCircularizeNode) { if (!vessel.patchedConicSolver.maneuverNodes.Any()) @@ -456,6 +261,8 @@ void DriveCircularizationBurn(FlightCtrlState s) // into it, you actually only spend 1513 m/s to execute combined manuver. Mechjeb should also do correction burns before // this if possible, and this can't correct all errors... but it's better then nothing. // (A better version of this should try to match inclination & LAN if target is specified) + // FIXME? this inclination correction is unlikely to be at tha AN/DN and will throw the LAN off with anything other than high + // TWR launches from equatorial launch sites -- should probably be made optional (or clip it if the correction is too large). Vector3d inclinationCorrection = OrbitalManeuverCalculator.DeltaVToChangeInclination(orbit, UT, Math.Abs(desiredInclination)); Vector3d smaCorrection = OrbitalManeuverCalculator.DeltaVForSemiMajorAxis(orbit.PerturbedOrbit(UT, inclinationCorrection), UT, desiredOrbitAltitude + mainBody.Radius); @@ -471,106 +278,121 @@ void DriveCircularizationBurn(FlightCtrlState s) } } - //An IAscentPath describes the desired gravity turn path - public interface IAscentPath + public abstract class MechJebModuleAscentMenuBase : DisplayModule { - //altitude at which to stop going straight up - bool IsVerticalAscent(double altitude, double velocity); - - //controls the ascent path - double FlightPathAngle(double altitude, double velocity); + public MechJebModuleAscentMenuBase(MechJebCore core) : base(core) { } } - public class DefaultAscentPath : IAscentPath + public abstract class MechJebModuleAscentBase : ComputerModule { - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult turnStartAltitude = new EditableDoubleMult(500, 1000); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult turnStartVelocity = new EditableDoubleMult(100); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult turnEndAltitude = new EditableDoubleMult(60000, 1000); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDouble turnEndAngle = 0; - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult turnShapeExponent = new EditableDoubleMult(0.4, 0.01); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public bool autoPath = true; - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public float autoTurnPerc = 0.05f; - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public float autoTurnSpdFactor = 18.5f; + public MechJebModuleAscentBase(MechJebCore core) : base(core) { } + + public string status { get; set; } + + public MechJebModuleAscentAutopilot autopilot { get { return core.GetComputerModule(); } } + + public abstract bool DriveAscent(FlightCtrlState s); - private double actualTurnStart = 0; + public Vector3d thrustVectorForNavball; - public double autoTurnStartAltitude + //data used by ThrottleToRaiseApoapsis + float raiseApoapsisLastThrottle = 0; + double raiseApoapsisLastApR = 0; + double raiseApoapsisLastUT = 0; + MovingAverage raiseApoapsisRatePerThrottle = new MovingAverage(3, 0); + + //gives a throttle setting that reduces as we approach the desired apoapsis + //so that we can precisely match the desired apoapsis instead of overshooting it + protected float ThrottleToRaiseApoapsis(double currentApR, double finalApR) { - get + float desiredThrottle; + + if (currentApR > finalApR + 5.0) { - // TODO remove the ActiveVessel reference - var vessel = FlightGlobals.ActiveVessel; - return (vessel.mainBody.atmosphere ? vessel.mainBody.RealMaxAtmosphereAltitude() * autoTurnPerc : vessel.terrainAltitude + 25); + desiredThrottle = 0.0F; //done, throttle down } - } - - public double autoTurnStartVelocity - { - get + else if (raiseApoapsisLastUT > vesselState.time - 1) { - // TODO remove the ActiveVessel reference - var vessel = FlightGlobals.ActiveVessel; - return vessel.mainBody.atmosphere ? autoTurnSpdFactor * autoTurnSpdFactor * autoTurnSpdFactor * 0.015625f : double.PositiveInfinity; + //reduce throttle as apoapsis nears target + double instantRatePerThrottle = (orbit.ApR - raiseApoapsisLastApR) / ((vesselState.time - raiseApoapsisLastUT) * raiseApoapsisLastThrottle); + instantRatePerThrottle = Math.Max(1.0, instantRatePerThrottle); //avoid problems from negative rates + raiseApoapsisRatePerThrottle.value = instantRatePerThrottle; + double desiredApRate = (finalApR - currentApR) / 1.0; + desiredThrottle = Mathf.Clamp((float)(desiredApRate / raiseApoapsisRatePerThrottle), 0.05F, 1.0F); } - } - - public double autoTurnEndAltitude - { - get + else { - // TODO remove the ActiveVessel reference - var vessel = FlightGlobals.ActiveVessel; - var targetAlt = vessel.GetMasterMechJeb().GetComputerModule().desiredOrbitAltitude; - if (vessel.mainBody.atmosphere) - { - return Math.Min(vessel.mainBody.RealMaxAtmosphereAltitude() * 0.85, targetAlt); - } - else - { - return Math.Min(30000, targetAlt * 0.85); - } + desiredThrottle = 1.0F; //no recent data point; just use max thrust. } - } - public double VerticalAscentEnd() - { - return autoPath ? autoTurnStartAltitude : turnStartAltitude; + //record data for next frame + raiseApoapsisLastThrottle = desiredThrottle; + raiseApoapsisLastApR = orbit.ApR; + raiseApoapsisLastUT = vesselState.time; + + return desiredThrottle; } - public double SpeedAscentEnd() - { - return autoPath ? autoTurnStartVelocity : turnStartVelocity; + protected double srfvelPitch() { + return 90.0 - Vector3d.Angle(vesselState.surfaceVelocity, vesselState.up); } - public bool IsVerticalAscent(double altitude, double velocity) + // provides AoA limiting and ground track steering to pitch controllers (possibly should be moved into the attitude controller, but + // right now it collaborates too heavily with the ascent autopilot) + // + protected void attitudeTo(double desiredPitch) { - actualTurnStart = Math.Min(actualTurnStart, autoTurnStartAltitude); - if (altitude < VerticalAscentEnd() && velocity < SpeedAscentEnd()) + double desiredHeading = OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, autopilot.desiredInclination, autopilot.launchLatitude); + + 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; + + thrustVectorForNavball = desiredThrustVector; + + desiredThrustVector = desiredThrustVector.normalized; + + if (autopilot.limitAoA) { - actualTurnStart = Math.Max(actualTurnStart, altitude); - return true; + 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; + } } - return false; - } - public double FlightPathAngle(double altitude, double velocity) - { - var turnEnd = (autoPath ? autoTurnEndAltitude : turnEndAltitude); + double pitch = 90 - Vector3d.Angle(desiredThrustVector, vesselState.up); + double hdg; - if (IsVerticalAscent(altitude, velocity)) return 90.0; + 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 (altitude > turnEnd) return turnEndAngle; + if (autopilot.forceRoll) + { + core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); - return Mathf.Clamp((float)(90.0 - Math.Pow((altitude - actualTurnStart) / (turnEnd - actualTurnStart), turnShapeExponent) * (90.0 - turnEndAngle)), 0.01F, 89.99F); + if ( desiredPitch == 90.0) + { + core.attitude.attitudeTo(hdg, pitch, autopilot.turnRoll, this); + } + else + { + core.attitude.attitudeTo(hdg, pitch, autopilot.verticalRoll, this); + } + } + else + { + core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this); + } } + } public static class LaunchTiming diff --git a/MechJeb2/MechJebModuleAscentClassic.cs b/MechJeb2/MechJebModuleAscentClassic.cs new file mode 100644 index 000000000..a5ae84197 --- /dev/null +++ b/MechJeb2/MechJebModuleAscentClassic.cs @@ -0,0 +1,263 @@ +using System; +using KSP.UI.Screens; +using UnityEngine; + +namespace MuMech +{ + //Todo: -reimplement measurement of LPA + public class MechJebModuleAscentClassic : MechJebModuleAscentBase + { + public MechJebModuleAscentClassic(MechJebCore core) : base(core) { } + + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnStartAltitude = new EditableDoubleMult(500, 1000); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnStartVelocity = new EditableDoubleMult(100); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnEndAltitude = new EditableDoubleMult(60000, 1000); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDouble turnEndAngle = 0; + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnShapeExponent = new EditableDoubleMult(0.4, 0.01); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public bool autoPath = true; + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public float autoTurnPerc = 0.05f; + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public float autoTurnSpdFactor = 18.5f; + + private double actualTurnStart = 0; + + public override void OnModuleEnabled() + { + mode = AscentMode.VERTICAL_ASCENT; + } + + public override void OnModuleDisabled() + { + } + + public double autoTurnStartAltitude + { + get + { + return (vessel.mainBody.atmosphere ? vessel.mainBody.RealMaxAtmosphereAltitude() * autoTurnPerc : vessel.terrainAltitude + 25); + } + } + + public double autoTurnStartVelocity + { + get + { + return vessel.mainBody.atmosphere ? autoTurnSpdFactor * autoTurnSpdFactor * autoTurnSpdFactor * 0.015625f : double.PositiveInfinity; + } + } + + public double autoTurnEndAltitude + { + get + { + var targetAlt = vessel.GetMasterMechJeb().GetComputerModule().desiredOrbitAltitude; + if (vessel.mainBody.atmosphere) + { + return Math.Min(vessel.mainBody.RealMaxAtmosphereAltitude() * 0.85, targetAlt); + } + else + { + return Math.Min(30000, targetAlt * 0.85); + } + } + } + + public double VerticalAscentEnd() + { + return autoPath ? autoTurnStartAltitude : turnStartAltitude; + } + + public double SpeedAscentEnd() + { + return autoPath ? autoTurnStartVelocity : turnStartVelocity; + } + + public bool IsVerticalAscent(double altitude, double velocity) + { + actualTurnStart = Math.Min(actualTurnStart, autoTurnStartAltitude); + if (altitude < VerticalAscentEnd() && velocity < SpeedAscentEnd()) + { + actualTurnStart = Math.Max(actualTurnStart, altitude); + return true; + } + return false; + } + + public double FlightPathAngle(double altitude, double velocity) + { + var turnEnd = (autoPath ? autoTurnEndAltitude : turnEndAltitude); + + if (IsVerticalAscent(altitude, velocity)) return 90.0; + + if (altitude > turnEnd) return turnEndAngle; + + return Mathf.Clamp((float)(90.0 - Math.Pow((altitude - actualTurnStart) / (turnEnd - actualTurnStart), turnShapeExponent) * (90.0 - turnEndAngle)), 0.01F, 89.99F); + } + + enum AscentMode { VERTICAL_ASCENT, GRAVITY_TURN, COAST_TO_APOAPSIS, EXIT }; + AscentMode mode; + + public override bool DriveAscent(FlightCtrlState s) + { + switch (mode) + { + case AscentMode.VERTICAL_ASCENT: + DriveVerticalAscent(s); + break; + + case AscentMode.GRAVITY_TURN: + DriveGravityTurn(s); + break; + + case AscentMode.COAST_TO_APOAPSIS: + DriveCoastToApoapsis(s); + break; + + case AscentMode.EXIT: + return false; + } + return true; + } + + void DriveVerticalAscent(FlightCtrlState s) + { + if (!IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface)) mode = AscentMode.GRAVITY_TURN; + if (autopilot.autoThrottle && orbit.ApA > autopilot.desiredOrbitAltitude) mode = AscentMode.COAST_TO_APOAPSIS; + + //during the vertical ascent we just thrust straight up at max throttle + attitudeTo(90); + + core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); + + if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; + + if (!vessel.LiftedOff() || vessel.Landed) status = "Awaiting liftoff"; + else status = "Vertical ascent"; + } + + + 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); + + /* FIXME: this code is now somewhat horrible and overly complicated and should probably just a PID to adjust the + pitch to try to fix the flight angle */ + if (autopilot.correctiveSteering) + { + //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 + autopilot.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, autopilot.desiredInclination, autopilot.launchLatitude); + Vector3d desiredHeadingVector = Math.Sin(desiredHeading) * vesselState.east + Math.Cos(desiredHeading) * vesselState.north; + + Vector3d desiredVelocityUnit = Math.Cos(desiredFlightPathAngle * UtilMath.Deg2Rad) * desiredHeadingVector + + Math.Sin(desiredFlightPathAngle * UtilMath.Deg2Rad) * vesselState.up; + + Vector3d desiredThrustVector = desiredVelocityUnit; + + 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 = autopilot.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; + desiredFlightPathAngle = 90 - Vector3d.Angle(desiredThrustVector, vesselState.up); + } + + attitudeTo(desiredFlightPathAngle); + + status = "Gravity turn"; + } + + void DriveCoastToApoapsis(FlightCtrlState s) + { + core.thrust.targetThrottle = 0; + + double circularSpeed = OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, orbit.ApR); + double apoapsisSpeed = orbit.SwappedOrbitalVelocityAtUT(orbit.NextApoapsisTime(vesselState.time)).magnitude; + double circularizeBurnTime = (circularSpeed - apoapsisSpeed) / vesselState.limitedMaxThrustAccel; + + if (vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude()) + { + mode = AscentMode.EXIT; + core.warp.MinimumWarp(); + return; + } + + //if our apoapsis has fallen too far, resume the gravity turn + if (orbit.ApA < autopilot.desiredOrbitAltitude - 1000.0) + { + mode = AscentMode.GRAVITY_TURN; + core.warp.MinimumWarp(); + return; + } + + core.thrust.targetThrottle = 0; + + // follow surface velocity to reduce flipping + attitudeTo(srfvelPitch()); + + if (autopilot.autoThrottle && orbit.ApA < autopilot.desiredOrbitAltitude) + { + core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, autopilot.desiredOrbitAltitude + mainBody.Radius); + } + + if (core.node.autowarp) + { + //warp at x2 physical warp: + core.warp.WarpPhysicsAtRate(2); + } + + status = "Coasting to edge of atmosphere"; + } + } +} diff --git a/MechJeb2/MechJebModuleAscentPathEditor.cs b/MechJeb2/MechJebModuleAscentClassicMenu.cs similarity index 92% rename from MechJeb2/MechJebModuleAscentPathEditor.cs rename to MechJeb2/MechJebModuleAscentClassicMenu.cs index 697536284..5e82f03fe 100644 --- a/MechJeb2/MechJebModuleAscentPathEditor.cs +++ b/MechJeb2/MechJebModuleAscentClassicMenu.cs @@ -1,214 +1,215 @@ -using System; -using UnityEngine; - -namespace MuMech -{ - public class MechJebModuleAscentPathEditor : DisplayModule - { - public MechJebModuleAscentPathEditor(MechJebCore core) - : base(core) - { - hidden = true; - } - - public DefaultAscentPath path; - static Texture2D pathTexture = new Texture2D(400, 100); - private MechJebModuleFlightRecorder recorder; - private double lastMaxAtmosphereAltitude = -1; - - public override void OnStart(PartModule.StartState state) - { - path = (DefaultAscentPath)core.GetComputerModule().ascentPath; - recorder = core.GetComputerModule(); - } - - public override GUILayoutOption[] WindowOptions() - { - return new GUILayoutOption[] { GUILayout.Width(300), GUILayout.Height(100) }; - } - - protected override void WindowGUI(int windowID) - { - if (path == null) - { - GUILayout.Label("Path is null!!!1!!1!1!1111!11eleven"); - base.WindowGUI(windowID); - return; - } - - if (lastMaxAtmosphereAltitude != mainBody.RealMaxAtmosphereAltitude()) - { - lastMaxAtmosphereAltitude = mainBody.RealMaxAtmosphereAltitude(); - UpdateAtmoTexture(pathTexture, mainBody, path.autoPath ? path.autoTurnEndAltitude : path.turnEndAltitude); - } - - GUILayout.BeginVertical(); - - double oldTurnShapeExponent = path.turnShapeExponent; - - path.autoPath = GUILayout.Toggle(path.autoPath, "Automatic Altitude Turn", GUILayout.ExpandWidth(false)); - if (path.autoPath) - { - GUILayout.BeginHorizontal(); - GUILayout.Label("Altitude: ", GUILayout.Width(60)); - // 1 to 200 / 200 = 0.5% to 105%, without this mess would the slider cause lots of garbage floats like 0.9999864 - path.autoTurnPerc = Mathf.Floor(GUILayout.HorizontalSlider(path.autoTurnPerc * 200f, 1f, 210.5f)) / 200f; - GUILayout.EndHorizontal(); - GUILayout.BeginHorizontal(); - GUILayout.Label("Velocity: ", GUILayout.Width(60)); - path.autoTurnSpdFactor = Mathf.Floor(GUILayout.HorizontalSlider(path.autoTurnSpdFactor * 2f, 8f, 160f)) / 2f; - GUILayout.EndHorizontal(); - } - - if (path.autoPath) - { - GUILayout.BeginHorizontal(); - GUILayout.Label("Turn start when Altitude is ", GUILayout.ExpandWidth(false)); - GUILayout.Label(MuUtils.ToSI(path.autoTurnStartAltitude, -1, 2) + "m", GUILayout.ExpandWidth(false)); - GUILayout.Label("or Velocity reach ", GUILayout.ExpandWidth(false)); - GUILayout.Label(MuUtils.ToSI(path.autoTurnStartVelocity, -1, 3) + "m/s", GUILayout.ExpandWidth(false)); - GUILayout.EndHorizontal(); - GUILayout.BeginHorizontal(); - GUILayout.Label("Turn end altitude: "); - GUILayout.Label(MuUtils.ToSI(path.autoTurnEndAltitude, -1, 2) + "m", new GUIStyle(GUI.skin.label) { alignment = TextAnchor.MiddleRight }, GUILayout.ExpandWidth(true)); - GUILayout.EndHorizontal(); - } - else - { - GuiUtils.SimpleTextBox("Turn start altitude:", path.turnStartAltitude, "km"); - GuiUtils.SimpleTextBox("Turn start velocity:", path.turnStartVelocity, "m/s"); - GuiUtils.SimpleTextBox("Turn end altitude:", path.turnEndAltitude, "km"); - } - - GuiUtils.SimpleTextBox("Final flight path angle:", path.turnEndAngle, "°"); - GuiUtils.SimpleTextBox("Turn shape:", path.turnShapeExponent, "%"); - - // Round the slider's value (0..1) to sliderPrecision decimal places. - const int sliderPrecision = 3; - - double sliderTurnShapeExponent = GUILayout.HorizontalSlider((float)path.turnShapeExponent, 0.0F, 1.0F); - if (Math.Round(Math.Abs(sliderTurnShapeExponent - oldTurnShapeExponent), sliderPrecision) > 0) - { - path.turnShapeExponent = new EditableDoubleMult(Math.Round(sliderTurnShapeExponent, sliderPrecision), 0.01); - } - - GUILayout.Box(pathTexture); - - if (Event.current.type == EventType.Repaint) - { - Rect r = GUILayoutUtility.GetLastRect(); - r.xMin += GUI.skin.box.margin.left; - r.yMin += GUI.skin.box.margin.top; - - r.xMax -= GUI.skin.box.margin.right; - r.yMax -= GUI.skin.box.margin.bottom; - - float scale = (float)((path.autoPath ? path.autoTurnEndAltitude : path.turnEndAltitude) / r.height); - - DrawnPath(r, scale, scale, path, Color.red); - DrawnTrajectory(r, path, recorder); - } - - GUILayout.EndVertical(); - - base.WindowGUI(windowID); - } - - //redraw the picture of the planned flight path - public static void UpdateAtmoTexture(Texture2D texture, CelestialBody mainBody, double maxAltitude, bool realAtmo = false) - { - double scale = maxAltitude / texture.height; //meters per pixel - - double maxAtmosphereAltitude = mainBody.RealMaxAtmosphereAltitude(); - double pressureSeaLevel = mainBody.atmospherePressureSeaLevel; - - for (int y = 0; y < texture.height; y++) - { - double alt = scale * y; - - if (realAtmo) - { - alt = mainBody.GetPressure(alt) / pressureSeaLevel; - } - else - { - alt = 1.0 - alt / maxAtmosphereAltitude; - } - - float v = (float)(mainBody.atmosphere ? alt : 0.0F); - Color c = new Color(0.0F, 0.0F, v); - - for (int x = 0; x < texture.width; x++) - { - texture.SetPixel(x, y, c); - - if (mainBody.atmosphere && (int)(maxAtmosphereAltitude / scale) == y) - texture.SetPixel(x, y, XKCDColors.LightGreyBlue); - } - } - - texture.Apply(); - } - - public static void DrawnPath(Rect r, float scaleX, float scaleY, DefaultAscentPath path, Color color) - { - - float alt = 0; - float downrange = 0; - Vector2 p1 = new Vector2(r.xMin, r.yMax); - Vector2 p2 = new Vector2(); - - while (alt < (path.autoPath ? path.autoTurnEndAltitude : path.turnEndAltitude) && downrange < r.width * scaleX) - { - float desiredAngle = (float)(alt < path.VerticalAscentEnd() ? 90 : path.FlightPathAngle(alt, 0)); - - alt += scaleY * Mathf.Sin(desiredAngle * Mathf.Deg2Rad); - downrange += scaleX * Mathf.Cos(desiredAngle * Mathf.Deg2Rad); - - p2.x = r.xMin + downrange / scaleX; - p2.y = r.yMax - alt / scaleY; - - if ((p1 - p2).sqrMagnitude >= 1.0) - { - Drawing.DrawLine(p1, p2, color, 2, true); - p1.x = p2.x; - p1.y = p2.y; - } - } - } - - private static void DrawnTrajectory(Rect r, DefaultAscentPath path, MechJebModuleFlightRecorder recorder) - { - if (recorder.history.Length <= 2 || recorder.historyIdx == 0) - return; - - float scale = (float)((path.autoPath ? path.autoTurnEndAltitude : path.turnEndAltitude) / r.height); //meters per pixel - - int t = 1; - - Vector2 p1 = new Vector2(r.xMin + (float)(recorder.history[0].downRange / scale), r.yMax - (float)(recorder.history[0].altitudeASL / scale)); - Vector2 p2 = new Vector2(); - - while (t <= recorder.historyIdx && t < recorder.history.Length) - { - var rec = recorder.history[t]; - p2.x = r.xMin + (float)(rec.downRange / scale); - p2.y = r.yMax - (float)(rec.altitudeASL / scale); - - if (r.Contains(p2) && (p1 - p2).sqrMagnitude >= 1.0 || t < 2) - { - Drawing.DrawLine(p1, p2, Color.white, 2, true); - p1.x = p2.x; - p1.y = p2.y; - } - - t++; - } - } - - public override string GetName() - { - return "Ascent Path Editor"; - } - } -} +using System; +using UnityEngine; + +namespace MuMech +{ + public class MechJebModuleAscentClassicMenu : MechJebModuleAscentMenuBase + { + public MechJebModuleAscentClassicMenu(MechJebCore core) + : base(core) + { + hidden = true; + } + + public MechJebModuleAscentClassic path { get { return autopilot.ascentPath as MechJebModuleAscentClassic; } } + public MechJebModuleAscentAutopilot autopilot; + static Texture2D pathTexture = new Texture2D(400, 100); + private MechJebModuleFlightRecorder recorder; + private double lastMaxAtmosphereAltitude = -1; + + public override void OnStart(PartModule.StartState state) + { + autopilot = core.GetComputerModule(); + recorder = core.GetComputerModule(); + } + + public override GUILayoutOption[] WindowOptions() + { + return new GUILayoutOption[] { GUILayout.Width(300), GUILayout.Height(100) }; + } + + protected override void WindowGUI(int windowID) + { + if (path == null) + { + GUILayout.Label("Path is null!!!1!!1!1!1111!11eleven"); + base.WindowGUI(windowID); + return; + } + + if (lastMaxAtmosphereAltitude != mainBody.RealMaxAtmosphereAltitude()) + { + lastMaxAtmosphereAltitude = mainBody.RealMaxAtmosphereAltitude(); + UpdateAtmoTexture(pathTexture, mainBody, path.autoPath ? path.autoTurnEndAltitude : path.turnEndAltitude); + } + + GUILayout.BeginVertical(); + + double oldTurnShapeExponent = path.turnShapeExponent; + + path.autoPath = GUILayout.Toggle(path.autoPath, "Automatic Altitude Turn", GUILayout.ExpandWidth(false)); + if (path.autoPath) + { + GUILayout.BeginHorizontal(); + GUILayout.Label("Altitude: ", GUILayout.Width(60)); + // 1 to 200 / 200 = 0.5% to 105%, without this mess would the slider cause lots of garbage floats like 0.9999864 + path.autoTurnPerc = Mathf.Floor(GUILayout.HorizontalSlider(path.autoTurnPerc * 200f, 1f, 210.5f)) / 200f; + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label("Velocity: ", GUILayout.Width(60)); + path.autoTurnSpdFactor = Mathf.Floor(GUILayout.HorizontalSlider(path.autoTurnSpdFactor * 2f, 8f, 160f)) / 2f; + GUILayout.EndHorizontal(); + } + + if (path.autoPath) + { + GUILayout.BeginHorizontal(); + GUILayout.Label("Turn start when Altitude is ", GUILayout.ExpandWidth(false)); + GUILayout.Label(MuUtils.ToSI(path.autoTurnStartAltitude, -1, 2) + "m", GUILayout.ExpandWidth(false)); + GUILayout.Label("or Velocity reach ", GUILayout.ExpandWidth(false)); + GUILayout.Label(MuUtils.ToSI(path.autoTurnStartVelocity, -1, 3) + "m/s", GUILayout.ExpandWidth(false)); + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label("Turn end altitude: "); + GUILayout.Label(MuUtils.ToSI(path.autoTurnEndAltitude, -1, 2) + "m", new GUIStyle(GUI.skin.label) { alignment = TextAnchor.MiddleRight }, GUILayout.ExpandWidth(true)); + GUILayout.EndHorizontal(); + } + else + { + GuiUtils.SimpleTextBox("Turn start altitude:", path.turnStartAltitude, "km"); + GuiUtils.SimpleTextBox("Turn start velocity:", path.turnStartVelocity, "m/s"); + GuiUtils.SimpleTextBox("Turn end altitude:", path.turnEndAltitude, "km"); + } + + GuiUtils.SimpleTextBox("Final flight path angle:", path.turnEndAngle, "°"); + GuiUtils.SimpleTextBox("Turn shape:", path.turnShapeExponent, "%"); + + // Round the slider's value (0..1) to sliderPrecision decimal places. + const int sliderPrecision = 3; + + double sliderTurnShapeExponent = GUILayout.HorizontalSlider((float)path.turnShapeExponent, 0.0F, 1.0F); + if (Math.Round(Math.Abs(sliderTurnShapeExponent - oldTurnShapeExponent), sliderPrecision) > 0) + { + path.turnShapeExponent = new EditableDoubleMult(Math.Round(sliderTurnShapeExponent, sliderPrecision), 0.01); + } + + GUILayout.Box(pathTexture); + + if (Event.current.type == EventType.Repaint) + { + Rect r = GUILayoutUtility.GetLastRect(); + r.xMin += GUI.skin.box.margin.left; + r.yMin += GUI.skin.box.margin.top; + + r.xMax -= GUI.skin.box.margin.right; + r.yMax -= GUI.skin.box.margin.bottom; + + float scale = (float)((path.autoPath ? path.autoTurnEndAltitude : path.turnEndAltitude) / r.height); + + DrawnPath(r, scale, scale, path, Color.red); + DrawnTrajectory(r, path, recorder); + } + + GUILayout.EndVertical(); + + base.WindowGUI(windowID); + } + + //redraw the picture of the planned flight path + public static void UpdateAtmoTexture(Texture2D texture, CelestialBody mainBody, double maxAltitude, bool realAtmo = false) + { + double scale = maxAltitude / texture.height; //meters per pixel + + double maxAtmosphereAltitude = mainBody.RealMaxAtmosphereAltitude(); + double pressureSeaLevel = mainBody.atmospherePressureSeaLevel; + + for (int y = 0; y < texture.height; y++) + { + double alt = scale * y; + + if (realAtmo) + { + alt = mainBody.GetPressure(alt) / pressureSeaLevel; + } + else + { + alt = 1.0 - alt / maxAtmosphereAltitude; + } + + float v = (float)(mainBody.atmosphere ? alt : 0.0F); + Color c = new Color(0.0F, 0.0F, v); + + for (int x = 0; x < texture.width; x++) + { + texture.SetPixel(x, y, c); + + if (mainBody.atmosphere && (int)(maxAtmosphereAltitude / scale) == y) + texture.SetPixel(x, y, XKCDColors.LightGreyBlue); + } + } + + texture.Apply(); + } + + public static void DrawnPath(Rect r, float scaleX, float scaleY, MechJebModuleAscentClassic path, Color color) + { + + float alt = 0; + float downrange = 0; + Vector2 p1 = new Vector2(r.xMin, r.yMax); + Vector2 p2 = new Vector2(); + + while (alt < (path.autoPath ? path.autoTurnEndAltitude : path.turnEndAltitude) && downrange < r.width * scaleX) + { + float desiredAngle = (float)(alt < path.VerticalAscentEnd() ? 90 : path.FlightPathAngle(alt, 0)); + + alt += scaleY * Mathf.Sin(desiredAngle * Mathf.Deg2Rad); + downrange += scaleX * Mathf.Cos(desiredAngle * Mathf.Deg2Rad); + + p2.x = r.xMin + downrange / scaleX; + p2.y = r.yMax - alt / scaleY; + + if ((p1 - p2).sqrMagnitude >= 1.0) + { + Drawing.DrawLine(p1, p2, color, 2, true); + p1.x = p2.x; + p1.y = p2.y; + } + } + } + + private static void DrawnTrajectory(Rect r, MechJebModuleAscentClassic path, MechJebModuleFlightRecorder recorder) + { + if (recorder.history.Length <= 2 || recorder.historyIdx == 0) + return; + + float scale = (float)((path.autoPath ? path.autoTurnEndAltitude : path.turnEndAltitude) / r.height); //meters per pixel + + int t = 1; + + Vector2 p1 = new Vector2(r.xMin + (float)(recorder.history[0].downRange / scale), r.yMax - (float)(recorder.history[0].altitudeASL / scale)); + Vector2 p2 = new Vector2(); + + while (t <= recorder.historyIdx && t < recorder.history.Length) + { + var rec = recorder.history[t]; + p2.x = r.xMin + (float)(rec.downRange / scale); + p2.y = r.yMax - (float)(rec.altitudeASL / scale); + + if (r.Contains(p2) && (p1 - p2).sqrMagnitude >= 1.0 || t < 2) + { + Drawing.DrawLine(p1, p2, Color.white, 2, true); + p1.x = p2.x; + p1.y = p2.y; + } + + t++; + } + } + + public override string GetName() + { + return "Ascent Path Editor"; + } + } +} diff --git a/MechJeb2/MechJebModuleAscentGT.cs b/MechJeb2/MechJebModuleAscentGT.cs new file mode 100644 index 000000000..220805db9 --- /dev/null +++ b/MechJeb2/MechJebModuleAscentGT.cs @@ -0,0 +1,238 @@ +using System; +using KSP.UI.Screens; +using UnityEngine; + +/* + * NOTE: THAT THIS IS NOT INTENDED TO BE A PERFECTLY FAITHFUL REIMPLEMENTATION OF + * THE GRAVITY TURN MOD FOR KSP. + */ + +namespace MuMech +{ + public class MechJebModuleAscentGT : MechJebModuleAscentBase + { + public MechJebModuleAscentGT(MechJebCore core) : base(core) { } + + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnStartAltitude = new EditableDoubleMult(500, 1000); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnStartVelocity = new EditableDoubleMult(50); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnStartPitch = new EditableDoubleMult(25); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult intermediateAltitude = new EditableDoubleMult(45000, 1000); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult holdAPTime = new EditableDoubleMult(1); + + public override void OnModuleEnabled() + { + maxholdAPTime = 0.0F; + mode = AscentMode.VERTICAL_ASCENT; + } + + public override void OnModuleDisabled() + { + } + + public bool IsVerticalAscent(double altitude, double velocity) + { + if (altitude < turnStartAltitude && velocity < turnStartVelocity) + { + return true; + } + return false; + } + + enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, HOLD_AP, COAST_TO_APOAPSIS, EXIT }; + AscentMode mode; + + public override bool DriveAscent(FlightCtrlState s) + { + switch (mode) + { + case AscentMode.VERTICAL_ASCENT: + DriveVerticalAscent(s); + break; + + case AscentMode.INITIATE_TURN: + DriveInitiateTurn(s); + break; + + case AscentMode.GRAVITY_TURN: + DriveGravityTurn(s); + break; + + case AscentMode.HOLD_AP: + DriveHoldAP(s); + break; + + case AscentMode.COAST_TO_APOAPSIS: + DriveCoastToApoapsis(s); + break; + + case AscentMode.EXIT: + return false; + } + return true; + } + + void DriveVerticalAscent(FlightCtrlState s) + { + if (!IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface)) mode = AscentMode.INITIATE_TURN; + if (autopilot.autoThrottle && orbit.ApA > intermediateAltitude) mode = AscentMode.GRAVITY_TURN; + + //during the vertical ascent we just thrust straight up at max throttle + attitudeTo(90); + + core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); + + if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; + + if (!vessel.LiftedOff() || vessel.Landed) status = "Awaiting liftoff"; + else status = "Vertical ascent"; + } + + void DriveInitiateTurn(FlightCtrlState s) + { + if ((90 - turnStartPitch) >= srfvelPitch()) + { + mode = AscentMode.GRAVITY_TURN; + return; + } + + if (autopilot.autoThrottle && orbit.ApA > intermediateAltitude) + { + mode = AscentMode.GRAVITY_TURN; + 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; + } + + attitudeTo(90 - turnStartPitch); + + 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 = "Initiate gravity turn"; + } + + double fixedTimeToAp() + { + if ( vessel.orbit.timeToPe < vessel.orbit.timeToAp ) + return vessel.orbit.timeToAp - vessel.orbit.period; + else + return vessel.orbit.timeToAp; + } + + double maxholdAPTime = 0; + + void DriveGravityTurn(FlightCtrlState s) + { + + 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"; + } + + void DriveHoldAP(FlightCtrlState s) + { + //stop the intermediate "burn" when our apoapsis reaches the desired altitude + if (orbit.ApA > autopilot.desiredOrbitAltitude) + { + mode = AscentMode.COAST_TO_APOAPSIS; + return; + } + + attitudeTo(0); /* FIXME: corrective steering */ + + if (fixedTimeToAp() < holdAPTime) + { + core.thrust.targetThrottle = 1.0F; + } + else + { + core.thrust.targetThrottle = 0.1F; + } + status = "Holding AP"; + } + + void DriveCoastToApoapsis(FlightCtrlState s) + { + core.thrust.targetThrottle = 0; + + double circularSpeed = OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, orbit.ApR); + double apoapsisSpeed = orbit.SwappedOrbitalVelocityAtUT(orbit.NextApoapsisTime(vesselState.time)).magnitude; + double circularizeBurnTime = (circularSpeed - apoapsisSpeed) / vesselState.limitedMaxThrustAccel; + + if (vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude()) + { + mode = AscentMode.EXIT; + core.warp.MinimumWarp(); + return; + } + + //if our apoapsis has fallen too far, resume the gravity turn + if (orbit.ApA < autopilot.desiredOrbitAltitude - 1000.0) + { + mode = AscentMode.HOLD_AP; + core.warp.MinimumWarp(); + return; + } + + core.thrust.targetThrottle = 0; + + // follow surface velocity to reduce flipping + attitudeTo(srfvelPitch()); + + if (autopilot.autoThrottle && orbit.ApA < autopilot.desiredOrbitAltitude) + { + core.warp.WarpPhysicsAtRate(1); + core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, autopilot.desiredOrbitAltitude + mainBody.Radius); + } + else + { + if (core.node.autowarp) + { + //warp at x2 physical warp: + core.warp.WarpPhysicsAtRate(2); + } + } + + status = "Coasting to edge of atmosphere"; + } + } +} diff --git a/MechJeb2/MechJebModuleAscentGTMenu.cs b/MechJeb2/MechJebModuleAscentGTMenu.cs new file mode 100644 index 000000000..fa5798ebf --- /dev/null +++ b/MechJeb2/MechJebModuleAscentGTMenu.cs @@ -0,0 +1,54 @@ +using System; +using UnityEngine; + +namespace MuMech +{ + public class MechJebModuleAscentGTMenu : MechJebModuleAscentMenuBase + { + public MechJebModuleAscentGTMenu(MechJebCore core) + : base(core) + { + hidden = true; + } + + public MechJebModuleAscentGT path { get { return autopilot.ascentPath as MechJebModuleAscentGT; } } + public MechJebModuleAscentAutopilot autopilot; + + public override void OnStart(PartModule.StartState state) + { + autopilot = core.GetComputerModule(); + } + + public override GUILayoutOption[] WindowOptions() + { + return new GUILayoutOption[] { GUILayout.Width(300), GUILayout.Height(100) }; + } + + protected override void WindowGUI(int windowID) + { + if (path == null) + { + GUILayout.Label("Path is null!!!1!!1!1!1111!11eleven"); + base.WindowGUI(windowID); + return; + } + + GUILayout.BeginVertical(); + + GuiUtils.SimpleTextBox("Turn start altitude:", path.turnStartAltitude, "km"); + GuiUtils.SimpleTextBox("Turn start velocity:", path.turnStartVelocity, "m/s"); + GuiUtils.SimpleTextBox("Turn start pitch:", path.turnStartPitch, "deg"); + GuiUtils.SimpleTextBox("Intermediate altitude:", path.intermediateAltitude, "km"); + GuiUtils.SimpleTextBox("Hold AP Time:", path.holdAPTime, "s"); + + GUILayout.EndVertical(); + + base.WindowGUI(windowID); + } + + public override string GetName() + { + return "Stock-style GravityTurn™ Pitch Program"; + } + } +} diff --git a/MechJeb2/MechJebModuleAscentGuidance.cs b/MechJeb2/MechJebModuleAscentGuidance.cs index 6ff83c81e..4b306a3d9 100644 --- a/MechJeb2/MechJebModuleAscentGuidance.cs +++ b/MechJeb2/MechJebModuleAscentGuidance.cs @@ -19,6 +19,63 @@ public class MechJebModuleAscentGuidance : DisplayModule MechJebModuleAscentAutopilot autopilot; MechJebModuleAscentNavBall navBall; + MechJebModuleAscentBase path; + MechJebModuleAscentMenuBase editor; + + /* FIXME: this probably needs to get persisted? */ + public int ascentPathIdx = 0; + public string[] ascentPathList = { "Classic Ascent Profile", "Stock-style GravityTurn™", "Iterative Guidance Mode (RSS/RO)" }; + + /* XXX: this is all a bit janky, could rub some reflection on it */ + private void get_path_and_editor(int i, out MechJebModuleAscentBase p, out MechJebModuleAscentMenuBase e) + { + if ( i == 0 ) + { + p = core.GetComputerModule(); + e = core.GetComputerModule(); + } + else if ( i == 1 ) + { + p = core.GetComputerModule(); + e = core.GetComputerModule(); + } + else if ( i == 2 ) + { + p = core.GetComputerModule(); + e = core.GetComputerModule(); + } + else + { + p = null; + e = null; + } + } + + private void disable_path_modules(int otherthan = -1) + { + for(int i = 0; i < ascentPathList.Length; i++) + { + if ( i == otherthan ) continue; + + MechJebModuleAscentBase p; + MechJebModuleAscentMenuBase e; + + get_path_and_editor(i, out p, out e); + + if (p != null) p.enabled = false; + if (e != null) e.enabled = false; + } + } + + private void enable_path_module(int index) + { + disable_path_modules(index); + + get_path_and_editor(index, out path, out editor); + + if (path != null) path.enabled = true; + /* the editor is not necessarily enabled by this, or the window will always pop up */ + } public override void OnStart(PartModule.StartState state) { @@ -32,6 +89,7 @@ public override void OnStart(PartModule.StartState state) public override void OnModuleEnabled() { + enable_path_module(ascentPathIdx); } public override void OnModuleDisabled() @@ -39,27 +97,26 @@ public override void OnModuleDisabled() launchingToInterplanetary = false; launchingToPlane = false; launchingToRendezvous = false; - MechJebModuleAscentPathEditor editor = core.GetComputerModule(); - if (editor != null) editor.enabled = false; + disable_path_modules(); } [GeneralInfoItem("Toggle Ascent Navball Guidance", InfoItem.Category.Misc, showInEditor = false)] - public void ToggleAscentNavballGuidanceInfoItem() - { - if (navBall != null) + public void ToggleAscentNavballGuidanceInfoItem() { - if (navBall.NavBallGuidance) + if (navBall != null) { - if (GUILayout.Button("Hide ascent navball guidance")) - navBall.NavBallGuidance = false; - } - else - { - if (GUILayout.Button("Show ascent navball guidance")) - navBall.NavBallGuidance = true; + if (navBall.NavBallGuidance) + { + if (GUILayout.Button("Hide ascent navball guidance")) + navBall.NavBallGuidance = false; + } + else + { + if (GUILayout.Button("Show ascent navball guidance")) + navBall.NavBallGuidance = true; + } } } - } protected override void WindowGUI(int windowID) { @@ -92,7 +149,7 @@ protected override void WindowGUI(int windowID) if (!autopilot.enabled && Math.Abs(desiredInclination) < Math.Abs(vesselState.latitude)) si.onHover.textColor = si.onNormal.textColor = XKCDColors.Orange; GuiUtils.SimpleTextBox("Orbit inclination", desiredInclination, "º"); - + core.thrust.LimitToPreventOverheatsInfoItem(); //core.thrust.LimitToTerminalVelocityInfoItem(); core.thrust.LimitToMaxDynamicPressureInfoItem(); @@ -140,7 +197,7 @@ protected override void WindowGUI(int windowID) if (autopilot.autostage) core.staging.AutostageSettingsInfoItem(); autopilot.autodeploySolarPanels = GUILayout.Toggle(autopilot.autodeploySolarPanels, - "Auto-deploy solar panels"); + "Auto-deploy solar panels"); GUILayout.BeginHorizontal(); core.node.autowarp = GUILayout.Toggle(core.node.autowarp, "Auto-warp"); @@ -156,7 +213,7 @@ protected override void WindowGUI(int windowID) GUILayout.BeginHorizontal(); GUILayout.Label("Launch countdown:", GUILayout.ExpandWidth(true)); autopilot.warpCountDown.text = GUILayout.TextField(autopilot.warpCountDown.text, - GUILayout.Width(60)); + GUILayout.Width(60)); GUILayout.Label("s", GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); } @@ -167,11 +224,11 @@ protected override void WindowGUI(int windowID) { launchingToRendezvous = true; autopilot.StartCountdown(vesselState.time + - LaunchTiming.TimeToPhaseAngle(autopilot.launchPhaseAngle, - mainBody, vesselState.longitude, core.target.TargetOrbit)); + LaunchTiming.TimeToPhaseAngle(autopilot.launchPhaseAngle, + mainBody, vesselState.longitude, core.target.TargetOrbit)); } autopilot.launchPhaseAngle.text = GUILayout.TextField(autopilot.launchPhaseAngle.text, - GUILayout.Width(60)); + GUILayout.Width(60)); GUILayout.Label("º", GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); @@ -181,12 +238,12 @@ protected override void WindowGUI(int windowID) launchingToPlane = true; autopilot.StartCountdown(vesselState.time + - LaunchTiming.TimeToPlane(autopilot.launchLANDifference, - mainBody, vesselState.latitude, vesselState.longitude, - core.target.TargetOrbit)); + LaunchTiming.TimeToPlane(autopilot.launchLANDifference, + mainBody, vesselState.latitude, vesselState.longitude, + core.target.TargetOrbit)); } autopilot.launchLANDifference.text = GUILayout.TextField( - autopilot.launchLANDifference.text, GUILayout.Width(60)); + autopilot.launchLANDifference.text, GUILayout.Width(60)); GUILayout.Label("º", GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); @@ -197,15 +254,15 @@ protected override void WindowGUI(int windowID) launchingToInterplanetary = true; //compute the desired launch date OrbitalManeuverCalculator.DeltaVAndTimeForHohmannTransfer(mainBody.orbit, - core.target.TargetOrbit, vesselState.time, out interplanetaryWindowUT); + 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 + 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); + interplanetaryWindowUT); autopilot.StartCountdown(interplanetaryWindowUT); } } @@ -229,7 +286,7 @@ protected override void WindowGUI(int windowID) desiredInclination = core.target.TargetOrbit.inclination; desiredInclination *= Math.Sign(Vector3d.Dot(core.target.TargetOrbit.SwappedOrbitNormal(), - Vector3d.Cross(vesselState.CoM - mainBody.position, mainBody.transform.up))); + Vector3d.Cross(vesselState.CoM - mainBody.position, mainBody.transform.up))); message = "Launching to target plane"; } else if (launchingToRendezvous) @@ -261,7 +318,22 @@ protected override void WindowGUI(int windowID) GUILayout.Label("Warning: MechJeb is unable to circularize without an upgraded Tracking Station."); } - MechJebModuleAscentPathEditor editor = core.GetComputerModule(); + 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; + } + + autopilot.ascentPath = path; + if (editor != null) editor.enabled = GUILayout.Toggle(editor.enabled, "Edit ascent path"); GUILayout.EndVertical(); diff --git a/MechJeb2/MechJebModuleAscentIGM.cs b/MechJeb2/MechJebModuleAscentIGM.cs new file mode 100644 index 000000000..9568c6667 --- /dev/null +++ b/MechJeb2/MechJebModuleAscentIGM.cs @@ -0,0 +1,352 @@ +using System; +using KSP.UI.Screens; +using UnityEngine; + +/* + * Apollo-style IGM launches for RSS/RO + */ + +namespace MuMech +{ + public class MechJebModuleAscentIGM : MechJebModuleAscentBase + { + public MechJebModuleAscentIGM(MechJebCore core) : base(core) { } + + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnStartAltitude = new EditableDoubleMult(500, 1000); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnStartVelocity = new EditableDoubleMult(50); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult turnStartPitch = new EditableDoubleMult(25); + + + private MechJebModuleStageStats stats { get { return core.GetComputerModule(); } } + private FuelFlowSimulation.Stats[] vacStats { get { return stats.vacStats; } } + + + public override void OnModuleEnabled() + { + mode = AscentMode.VERTICAL_ASCENT; + } + + public override void OnModuleDisabled() + { + } + + public bool IsVerticalAscent(double altitude, double velocity) + { + if (altitude < turnStartAltitude && velocity < turnStartVelocity) + { + return true; + } + return false; + } + + enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; + AscentMode mode; + + /* current MJ stage index */ + private int last_stage; + /* current exhaust velocity */ + private double v_e; + /* time to burn the entire vehicle */ + private double tau; + /* current acceleration */ + private double a0; + /* tangential velocity at burnout FIXME: circular for now */ + private double vT; + /* radius at burnout */ + private double rT; + /* radius now */ + private double r; + /* gravParameter */ + private double GM; + + /* ending radial velocity (FIXME: circular for now) */ + double rdT; + /* current radial velocity */ + double rd; + /* r: this is in the radial direction (altitude to gain) */ + double dr; + /* rdot: also in the radial direction (upwards velocity to lose) */ + double drd; + + /* angular velocity at burnout */ + double wT; + /* current angular velocity */ + double w; + /* mean radius */ + double rbar; + /* angular momentum at burnout FIXME: circular for now */ + double hT; + /* angular momentum */ + double h; + /* angular momentum to gain */ + double dh; + /* acceleration at burnout */ + double aT; + /* current gravity + centrifugal force term */ + double C; + /* gravity + centrifugal force at burnout */ + double CT; + + private void update_rocket_stats() { + /* sometimes the last stage in MJ has 0.0 dV and we have to search back for the actively burning stage */ + for(int i = vacStats.Length - 1; i >= 0; i--) + { + if ( vacStats[i].deltaV > 0.0D ) + { + last_stage = i; + break; + } + } + + GM = mainBody.gravParameter; + v_e = vacStats[last_stage].isp * 9.80665; + a0 = vesselState.currentThrustAccel; + tau = v_e / a0; + rT = autopilot.desiredOrbitAltitude + mainBody.Radius; + vT = Math.Sqrt(GM/rT); /* FIXME: assumes circular */ + r = mainBody.position.magnitude; + + rdT = 0; /* FIXME: assumes circular */ + rd = vesselState.speedVertical; + + wT = vT / rT; + w = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude / (r * r); + rbar = ( rT + r ) / 2.0D; + hT = rT * vT; /* FIXME: assumes circular */ + h = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude; + dh = hT - h; + aT = a0 / ( 1.0D - T / tau ); + + C = (GM / (r * r) - w * w * r ) / a0; + CT = (GM / (rT * rT) - wT * wT * rT ) / aT; + + Debug.Log("GM = " + GM + " v_e = " + v_e + " tau = " + tau + "vT = " + vT); + Debug.Log("aT = " + aT + " rT = " + rT + " rdT = " + rdT + " wT = " + wT + " hT = " + hT); + Debug.Log("a0 = " + a0 + " r = " + r + " rd = " + rd + " w = " + w + " h = " + h + " rbar = " + rbar); + Debug.Log("C = " + C + " CT = " + CT); + } + + private void peg_solve() + { + + double b0 = -v_e * Math.Log(1.0D - T/tau); + double b1 = b0 * tau - v_e * T; + double c0 = b0 * T - b1; + double c1 = c0 * tau - v_e * T * T / 2.0D; + + double d = b0 * c1 - b1 * c0; + + A = ( c1 * ( rdT - rd ) - b1 * ( rT - r - rd * T ) ) / d; + B = ( -c0 * ( rdT - rd ) + b0 * ( rT - r - rd * T ) ) / d; + } + + /* steering constants */ + private double A; + private double B; + /* time to burnout */ + private double T; + /* dV to add */ + private double dV; + + private void peg_estimate(double dt, bool debug = false) + { + double oldA = A; + double oldB = B; + double oldT = T; + + /* update old guidance */ + A = A + B * dt; + B = B; + T = T - dt; + + aT = a0 / ( 1.0D - T / tau ); + CT = (GM / (rT * rT) - wT * wT * rT ) / aT; + + /* current sin pitch */ + double f_r = A + C; + /* sin pitch at burnout */ + double f_rT = A + B * T + CT; + /* appx rate of sin pitch */ + double fd_r = ( f_rT - f_r ) / T; + + /* placeholder for future yaw term */ + double f_h = 0.0D; + /* placeholder for future yaw rate term */ + double fd_h = 0.0D; + + /* cos pitch */ + double f_th = 1.0D - f_r * f_r / 2.0D - f_h * f_h / 2.0D; + /* cos pitch rate */ + double fd_th = - ( f_r * fd_r + f_h * fd_h ); + /* cos pitch accel */ + double fdd_th = - ( fd_r * fd_r + fd_h * fd_h ) / 2.0D; + + if (debug) { + Debug.Log("f_th = " + f_th + " fd_th = " + fd_th + " fdd_th = " + fdd_th); + Debug.Log("f_r = " + f_r + " f_rT = " + f_rT + " fd_r = " + fd_r); + } + + /* updated estimate of dV to burn */ + dV = ( dh / rbar + v_e * T * ( fd_th + fdd_th * tau ) + fdd_th * v_e * T * T / 2.0D ) / ( f_th + fd_th * tau + fdd_th * tau * tau ); + + /* updated estimate of T */ + T = tau * ( 1 - Math.Exp( - dV / v_e ) ); + } + + private bool bad_dV() + { + return dV <= 0.0D; + } + + private bool bad_pitch() + { + return Double.IsNaN(Math.Asin(A+C)); + } + + private bool bad_guidance() + { + return Double.IsNaN(T) || Double.IsInfinity(T) || T <= 0.0D || Double.IsNaN(A) || Double.IsInfinity(A) || Double.IsNaN(B) || Double.IsInfinity(B); + } + + private bool sane_guidance = false; + + private void converge(double dt, bool initialize = false) + { + if (initialize || bad_guidance() || bad_pitch() || bad_dV() || !sane_guidance) + { + T = 120.0D; + A = 0.0D; + B = 0.0D; + dt = 0.0; + } + + double startingT = T; + double startingA = A; + double startingB = B; + + bool converged = false; + int i; + for(i = 0; i < 50; i++) { + double oldT = T; + if (i == 0) + peg_estimate(dt); + else + peg_estimate(0); + peg_solve(); + if ( Math.Abs(T - oldT) < 0.1 ) { + converged = true; + break; + } + } + + Debug.Log("pitch = " + Math.Asin(A+C) + " dV = " + dV + " cycles = " + i); + + if (!converged || bad_guidance() || bad_dV() || bad_pitch()) + { + /* FIXME: probably shouldn't scribble over globals then restore them if they're bad */ + A = startingA; + B = startingB; + T = startingT; + sane_guidance = false; + } + else + { + sane_guidance = true; + } + } + + double last_time = 0.0D; + + public override bool DriveAscent(FlightCtrlState s) + { + stats.RequestUpdate(this); + update_rocket_stats(); + + if (last_time != 0.0D) + converge(vesselState.time - last_time); + else + converge(0); + + last_time = vesselState.time; + + switch (mode) + { + case AscentMode.VERTICAL_ASCENT: + DriveVerticalAscent(s); + break; + + case AscentMode.INITIATE_TURN: + DriveInitiateTurn(s); + break; + + case AscentMode.GRAVITY_TURN: + DriveGravityTurn(s); + break; + } + + if (mode == AscentMode.EXIT) + return false; + else + return true; + } + + void DriveVerticalAscent(FlightCtrlState s) + { + if (!IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface)) mode = AscentMode.INITIATE_TURN; + + //during the vertical ascent we just thrust straight up at max throttle + attitudeTo(90); + + core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); + + if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; + + if (!vessel.LiftedOff() || vessel.Landed) status = "Awaiting liftoff"; + else status = "Vertical ascent"; + } + + void DriveInitiateTurn(FlightCtrlState s) + { + if ((90 - turnStartPitch) >= srfvelPitch()) + { + mode = AscentMode.GRAVITY_TURN; + 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; + } + + attitudeTo(90 - turnStartPitch); + + status = "Initiate gravity turn"; + } + + void DriveGravityTurn(FlightCtrlState s) + { + if (h >= hT) + { + core.thrust.targetThrottle = 0.0F; + mode = AscentMode.EXIT; + return; + } + + if (sane_guidance) { + attitudeTo(Math.Asin(A + C) * UtilMath.Rad2Deg); + } + else + { + // srfvelPitch == zero AoA + attitudeTo(srfvelPitch()); + } + + status = "Gravity turn"; + } + } +} diff --git a/MechJeb2/MechJebModuleAscentIGMMenu.cs b/MechJeb2/MechJebModuleAscentIGMMenu.cs new file mode 100644 index 000000000..a3e490eb2 --- /dev/null +++ b/MechJeb2/MechJebModuleAscentIGMMenu.cs @@ -0,0 +1,52 @@ +using System; +using UnityEngine; + +namespace MuMech +{ + public class MechJebModuleAscentIGMMenu : MechJebModuleAscentMenuBase + { + public MechJebModuleAscentIGMMenu(MechJebCore core) + : base(core) + { + hidden = true; + } + + public MechJebModuleAscentIGM path { get { return autopilot.ascentPath as MechJebModuleAscentIGM; } } + public MechJebModuleAscentAutopilot autopilot; + + public override void OnStart(PartModule.StartState state) + { + autopilot = core.GetComputerModule(); + } + + public override GUILayoutOption[] WindowOptions() + { + return new GUILayoutOption[] { GUILayout.Width(300), GUILayout.Height(100) }; + } + + protected override void WindowGUI(int windowID) + { + if (path == null) + { + GUILayout.Label("Path is null!!!1!!1!1!1111!11eleven"); + base.WindowGUI(windowID); + return; + } + + GUILayout.BeginVertical(); + + GuiUtils.SimpleTextBox("Turn start altitude:", path.turnStartAltitude, "km"); + GuiUtils.SimpleTextBox("Turn start velocity:", path.turnStartVelocity, "m/s"); + GuiUtils.SimpleTextBox("Turn start pitch:", path.turnStartPitch, "deg"); + + GUILayout.EndVertical(); + + base.WindowGUI(windowID); + } + + public override string GetName() + { + return "Stock-style GravityTurn™ Pitch Program"; + } + } +} diff --git a/MechJeb2/MechJebModuleAscentNavBall.cs b/MechJeb2/MechJebModuleAscentNavBall.cs index f6c01bea7..ce0ddad77 100644 --- a/MechJeb2/MechJebModuleAscentNavBall.cs +++ b/MechJeb2/MechJebModuleAscentNavBall.cs @@ -44,13 +44,8 @@ public override void OnFixedUpdate() { if (NavBallGuidance && autopilot != null && autopilot.ascentPath != null) { - double angle = UtilMath.Deg2Rad * autopilot.ascentPath.FlightPathAngle(vesselState.altitudeASL, vesselState.speedSurface); - double heading = UtilMath.Deg2Rad * OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, autopilot.desiredInclination, autopilot.launchLatitude); - Vector3d horizontalDir = Math.Cos(heading) * vesselState.north + Math.Sin(heading) * vesselState.east; - Vector3d dir = Math.Cos(angle) * horizontalDir + Math.Sin(angle) * vesselState.up; - core.target.UpdateDirectionTarget(dir); + core.target.UpdateDirectionTarget(autopilot.ascentPath.thrustVectorForNavball); } } - } } diff --git a/MechJeb2/MechJebModuleAttitudeController.cs b/MechJeb2/MechJebModuleAttitudeController.cs index 48913267c..3d6459f0c 100644 --- a/MechJeb2/MechJebModuleAttitudeController.cs +++ b/MechJeb2/MechJebModuleAttitudeController.cs @@ -60,7 +60,7 @@ public class MechJebModuleAttitudeController : ComputerModule [Persistent(pass = (int)Pass.Global)] [ValueInfoItem("Steering error", InfoItem.Category.Vessel, format = "F1", units = "º")] public MovingAverage steeringError = new MovingAverage(); - + public bool attitudeKILLROT = false; protected bool attitudeChanged = false; @@ -139,7 +139,7 @@ public Vector3d AxisState protected Quaternion lastSAS = new Quaternion(); public double attitudeError; - + public Vector3d torque; public Vector3d inertia; @@ -191,7 +191,7 @@ public void tuneTf(Vector3d torque) Vector3d ratio = new Vector3d( torque.x != 0 ? vesselState.MoI.x / torque.x : 0, torque.y != 0 ? vesselState.MoI.y / torque.y : 0, - torque.z != 0 ? vesselState.MoI.z / torque.z : 0 + torque.z != 0 ? vesselState.MoI.z / torque.z : 0 ); TfV = 0.05 * ratio; @@ -442,7 +442,7 @@ public override void Drive(FlightCtrlState s) _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget; Transform vesselTransform = vessel.ReferenceTransform; //Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselTransform.rotation) * _requestedAttitude); - + // Find out the real shorter way to turn where we wan to. // Thanks to HoneyFox Vector3d tgtLocalUp = vesselTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward; @@ -598,4 +598,4 @@ private void SetFlightCtrlState(Vector3d act, Vector3d deltaEuler, FlightCtrlSta } } // end of SetFlightCtrlState } -} \ No newline at end of file +} diff --git a/MechJeb2/MechJebModuleFlightRecorderGraph.cs b/MechJeb2/MechJebModuleFlightRecorderGraph.cs index 7b3a7e21c..e34527760 100644 --- a/MechJeb2/MechJebModuleFlightRecorderGraph.cs +++ b/MechJeb2/MechJebModuleFlightRecorderGraph.cs @@ -25,7 +25,7 @@ public void Reset() maximum = 0; labels = new string[ScaleTicks]; labelsPos = new double[ScaleTicks]; - + } } @@ -110,7 +110,7 @@ protected override void WindowGUI(int windowID) backgroundTexture = new Texture2D(1, height); } - MechJebModuleAscentPathEditor.UpdateAtmoTexture(backgroundTexture, vessel.mainBody, lastMaximumAltitude, realAtmo); + MechJebModuleAscentClassicMenu.UpdateAtmoTexture(backgroundTexture, vessel.mainBody, lastMaximumAltitude, realAtmo); oldMainBody = mainBody; } @@ -151,9 +151,9 @@ protected override void WindowGUI(int windowID) GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); - + autoScale = GUILayout.Toggle(autoScale, "Auto Scale", GUILayout.ExpandWidth(false)); - + if (!autoScale && GUILayout.Button("-", GUILayout.ExpandWidth(false))) { if (downrange) @@ -170,9 +170,9 @@ protected override void WindowGUI(int windowID) double activeScaleX = autoScale ? autoScaleX : manualScaleX; double scaleX = downrange ? Math.Pow(2, activeScaleX) : precision * Math.Pow(2, activeScaleX); - + GUILayout.Label((downrange ? MuUtils.ToSI(scaleX, -1, 2) + "m/px" : GuiUtils.TimeToDHMS(scaleX, 1) + "/px"), GUILayout.ExpandWidth(false)); - + if (!autoScale && GUILayout.Button("+", GUILayout.ExpandWidth(false))) { if (downrange) @@ -217,7 +217,7 @@ protected override void WindowGUI(int windowID) realAtmo = GUILayout.Toggle(realAtmo, "Real Atmo", GUILayout.ExpandWidth(false)); if (oldRealAtmo != realAtmo) - MechJebModuleAscentPathEditor.UpdateAtmoTexture(backgroundTexture, vessel.mainBody, lastMaximumAltitude, realAtmo); + MechJebModuleAscentClassicMenu.UpdateAtmoTexture(backgroundTexture, vessel.mainBody, lastMaximumAltitude, realAtmo); //GUILayout.Label("", GUILayout.ExpandWidth(true)); GUILayout.FlexibleSpace(); @@ -297,7 +297,7 @@ protected override void WindowGUI(int windowID) newIdx = 0; scaleIdx = newIdx; } - + if (graphStates[(int)recordType.AltitudeASL].display) { GUI.color = Color.white; @@ -389,7 +389,7 @@ protected override void WindowGUI(int windowID) if (graphStates[(int)recordType.AltitudeASL].display || graphStates[(int)recordType.AltitudeTrue].display) GUI.DrawTexture(r, backgroundTexture, ScaleMode.StretchToFill); - + if (stages) DrawnStages(r, scaleX, downrange); @@ -463,7 +463,7 @@ private void DrawnPath(Rect r, recordType type, float minimum, double scaleX, bo float yBase = r.yMax + (float)(graphState.minimum * invScaleY); int t = 0; - while (t < recorder.historyIdx && t < recorder.history.Length && + while (t < recorder.historyIdx && t < recorder.history.Length && (xBase + (float)((downRange ? recorder.history[t].downRange : recorder.history[t].timeSinceMark) * invScaleX)) <= r.xMin) { t++; @@ -471,7 +471,7 @@ private void DrawnPath(Rect r, recordType type, float minimum, double scaleX, bo Vector2 p1 = new Vector2(xBase + (float)((downRange ? recorder.history[t].downRange : recorder.history[t].timeSinceMark) * invScaleX), yBase - (float)(recorder.history[t][type] * invScaleY)); Vector2 p2 = new Vector2(); - + while (t <= recorder.historyIdx && t < recorder.history.Length) { var rec = recorder.history[t]; @@ -517,12 +517,12 @@ private void DrawnStages(Rect r, double scaleX, bool downRange) t++; } } - + private void UpdateScale() { if (recorder.historyIdx == 0) ResetScale(); - + for (int t = 0; t < typeCount; t++) { bool change = false; @@ -532,7 +532,7 @@ private void UpdateScale() change = true; graphStates[t].maximum = recorder.maximums[t] + Math.Abs(recorder.maximums[t] * 0.2); } - + if (graphStates[t].minimum > recorder.minimums[t]) { change = true; @@ -556,7 +556,7 @@ private void UpdateScale() minimum = Math.Floor(minimum / step) * step; maximum = Math.Ceiling(maximum / step) * step; int digit = (int)Math.Max(-Math.Floor(Math.Log10(step)), 0); - + double currX = minimum; int i = 0; while (currX <= maximum + 0.5 * step) @@ -607,7 +607,7 @@ private void ResetScale() graphStates[(int)recordType.SteeringLosses].maximum = 100; graphStates[(int)recordType.DeltaVExpended].maximum = 100; } - + private double heckbertNiceNum(double x, bool round) { int exp = (int)Math.Log10(x); diff --git a/MechJeb2/OrbitalManeuverCalculator.cs b/MechJeb2/OrbitalManeuverCalculator.cs index 67c1d1532..5992ce349 100644 --- a/MechJeb2/OrbitalManeuverCalculator.cs +++ b/MechJeb2/OrbitalManeuverCalculator.cs @@ -229,7 +229,9 @@ public static double HeadingForLaunchInclination(Vessel vessel, VesselState vess double latitudeDegrees = vesselState.latitude; // we have to clamp our inclination to above our launch site or else tracks go very wonky double clampedInclination = clampInclination(inclinationDegrees, launchLatitude, vessel.orbit.inclination); - Debug.Log("clampinedInc: " + clampedInclination + " incDegrees: " + inclinationDegrees + " lat: " + launchLatitude + " inc: " + vessel.orbit.inclination); + // hack just to make launches to 0 inclination from KSC in stock be precise + if (Math.Abs(launchLatitude) < 0.1) clampedInclination = inclinationDegrees; + double orbVel = OrbitalManeuverCalculator.CircularOrbitSpeed(body, vesselState.altitudeASL + body.Radius); double headingOne = HeadingForInclination(clampedInclination, latitudeDegrees) * UtilMath.Deg2Rad; double headingTwo = HeadingForInclination(-clampedInclination, latitudeDegrees) * UtilMath.Deg2Rad; diff --git a/MechJeb2/ScriptsModule/MechJebModuleScriptActionAscent.cs b/MechJeb2/ScriptsModule/MechJebModuleScriptActionAscent.cs index 004777762..5d6f380f4 100644 --- a/MechJeb2/ScriptsModule/MechJebModuleScriptActionAscent.cs +++ b/MechJeb2/ScriptsModule/MechJebModuleScriptActionAscent.cs @@ -14,7 +14,7 @@ public class MechJebModuleScriptActionAscent : MechJebModuleScriptAction private List actionTypes = new List(); //Module Parameters [Persistent(pass = (int)Pass.Type)] - public IAscentPath ascentPath; + public MechJebModuleAscentBase ascentPath; [Persistent(pass = (int)Pass.Type)] public double desiredOrbitAltitude; [Persistent(pass = (int)Pass.Type)] @@ -154,4 +154,3 @@ override public void onAbord() } } } - From d072623836b3928ac15dc3fe6b7e268db2dd79e3 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sun, 25 Jun 2017 18:08:44 -0700 Subject: [PATCH 02/51] rename PEG->IGM and add Menu stuff Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentGuidance.cs | 6 +- MechJeb2/MechJebModuleAscentIGMMenu.cs | 52 ---------- ...AscentIGM.cs => MechJebModuleAscentPEG.cs} | 98 ++++++++++--------- MechJeb2/MechJebModuleAscentPEGMenu.cs | 65 ++++++++++++ 4 files changed, 119 insertions(+), 102 deletions(-) delete mode 100644 MechJeb2/MechJebModuleAscentIGMMenu.cs rename MechJeb2/{MechJebModuleAscentIGM.cs => MechJebModuleAscentPEG.cs} (78%) create mode 100644 MechJeb2/MechJebModuleAscentPEGMenu.cs diff --git a/MechJeb2/MechJebModuleAscentGuidance.cs b/MechJeb2/MechJebModuleAscentGuidance.cs index 4b306a3d9..758f8f90e 100644 --- a/MechJeb2/MechJebModuleAscentGuidance.cs +++ b/MechJeb2/MechJebModuleAscentGuidance.cs @@ -24,7 +24,7 @@ public class MechJebModuleAscentGuidance : DisplayModule /* FIXME: this probably needs to get persisted? */ public int ascentPathIdx = 0; - public string[] ascentPathList = { "Classic Ascent Profile", "Stock-style GravityTurn™", "Iterative Guidance Mode (RSS/RO)" }; + public string[] ascentPathList = { "Classic Ascent Profile", "Stock-style GravityTurn™", "Powered Explicit Guidance (RSS/RO)" }; /* XXX: this is all a bit janky, could rub some reflection on it */ private void get_path_and_editor(int i, out MechJebModuleAscentBase p, out MechJebModuleAscentMenuBase e) @@ -41,8 +41,8 @@ private void get_path_and_editor(int i, out MechJebModuleAscentBase p, out MechJ } else if ( i == 2 ) { - p = core.GetComputerModule(); - e = core.GetComputerModule(); + p = core.GetComputerModule(); + e = core.GetComputerModule(); } else { diff --git a/MechJeb2/MechJebModuleAscentIGMMenu.cs b/MechJeb2/MechJebModuleAscentIGMMenu.cs deleted file mode 100644 index a3e490eb2..000000000 --- a/MechJeb2/MechJebModuleAscentIGMMenu.cs +++ /dev/null @@ -1,52 +0,0 @@ -using System; -using UnityEngine; - -namespace MuMech -{ - public class MechJebModuleAscentIGMMenu : MechJebModuleAscentMenuBase - { - public MechJebModuleAscentIGMMenu(MechJebCore core) - : base(core) - { - hidden = true; - } - - public MechJebModuleAscentIGM path { get { return autopilot.ascentPath as MechJebModuleAscentIGM; } } - public MechJebModuleAscentAutopilot autopilot; - - public override void OnStart(PartModule.StartState state) - { - autopilot = core.GetComputerModule(); - } - - public override GUILayoutOption[] WindowOptions() - { - return new GUILayoutOption[] { GUILayout.Width(300), GUILayout.Height(100) }; - } - - protected override void WindowGUI(int windowID) - { - if (path == null) - { - GUILayout.Label("Path is null!!!1!!1!1!1111!11eleven"); - base.WindowGUI(windowID); - return; - } - - GUILayout.BeginVertical(); - - GuiUtils.SimpleTextBox("Turn start altitude:", path.turnStartAltitude, "km"); - GuiUtils.SimpleTextBox("Turn start velocity:", path.turnStartVelocity, "m/s"); - GuiUtils.SimpleTextBox("Turn start pitch:", path.turnStartPitch, "deg"); - - GUILayout.EndVertical(); - - base.WindowGUI(windowID); - } - - public override string GetName() - { - return "Stock-style GravityTurn™ Pitch Program"; - } - } -} diff --git a/MechJeb2/MechJebModuleAscentIGM.cs b/MechJeb2/MechJebModuleAscentPEG.cs similarity index 78% rename from MechJeb2/MechJebModuleAscentIGM.cs rename to MechJeb2/MechJebModuleAscentPEG.cs index 9568c6667..a50d45f4c 100644 --- a/MechJeb2/MechJebModuleAscentIGM.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -3,27 +3,28 @@ using UnityEngine; /* - * Apollo-style IGM launches for RSS/RO + * Atlas/Centaur-style PEG launches for RSS/RO */ namespace MuMech { - public class MechJebModuleAscentIGM : MechJebModuleAscentBase + public class MechJebModuleAscentPEG : MechJebModuleAscentBase { - public MechJebModuleAscentIGM(MechJebCore core) : base(core) { } + public MechJebModuleAscentPEG(MechJebCore core) : base(core) { } + /* default pitch program here works seemingly decent at SLT of about 1.4 */ [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult turnStartAltitude = new EditableDoubleMult(500, 1000); + public EditableDoubleMult pitchStartTime = new EditableDoubleMult(10); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult turnStartVelocity = new EditableDoubleMult(50); + public EditableDoubleMult pitchRate = new EditableDoubleMult(0.75); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult turnStartPitch = new EditableDoubleMult(25); - + public EditableDoubleMult pitchEndTime = new EditableDoubleMult(55); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult pitchBias = new EditableDoubleMult(0); private MechJebModuleStageStats stats { get { return core.GetComputerModule(); } } private FuelFlowSimulation.Stats[] vacStats { get { return stats.vacStats; } } - public override void OnModuleEnabled() { mode = AscentMode.VERTICAL_ASCENT; @@ -33,18 +34,12 @@ public override void OnModuleDisabled() { } - public bool IsVerticalAscent(double altitude, double velocity) - { - if (altitude < turnStartAltitude && velocity < turnStartVelocity) - { - return true; - } - return false; - } - enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; AscentMode mode; + /* guidancePitchAngle -- output from the guidance algorithm, not 'manual' pitch */ + public double guidancePitch { get { return Math.Asin(A + C) * UtilMath.Rad2Deg; } } + /* current MJ stage index */ private int last_stage; /* current exhaust velocity */ @@ -147,9 +142,9 @@ private void peg_solve() private double A; private double B; /* time to burnout */ - private double T; + public double T; /* dV to add */ - private double dV; + public double dV; private void peg_estimate(double dt, bool debug = false) { @@ -203,7 +198,7 @@ private bool bad_dV() private bool bad_pitch() { - return Double.IsNaN(Math.Asin(A+C)); + return Double.IsNaN(guidancePitch); } private bool bad_guidance() @@ -211,11 +206,13 @@ private bool bad_guidance() return Double.IsNaN(T) || Double.IsInfinity(T) || T <= 0.0D || Double.IsNaN(A) || Double.IsInfinity(A) || Double.IsNaN(B) || Double.IsInfinity(B); } - private bool sane_guidance = false; + public bool saneGuidance = false; + + public int convergenceSteps; private void converge(double dt, bool initialize = false) { - if (initialize || bad_guidance() || bad_pitch() || bad_dV() || !sane_guidance) + if (initialize || bad_guidance() || bad_pitch() || bad_dV() || !saneGuidance) { T = 120.0D; A = 0.0D; @@ -228,10 +225,9 @@ private void converge(double dt, bool initialize = false) double startingB = B; bool converged = false; - int i; - for(i = 0; i < 50; i++) { + for(convergenceSteps = 0; convergenceSteps < 50; convergenceSteps++) { double oldT = T; - if (i == 0) + if (convergenceSteps == 0) peg_estimate(dt); else peg_estimate(0); @@ -242,19 +238,20 @@ private void converge(double dt, bool initialize = false) } } - Debug.Log("pitch = " + Math.Asin(A+C) + " dV = " + dV + " cycles = " + i); + Debug.Log("pitch = " + Math.Asin(A+C) + " dV = " + dV + " cycles = " + convergenceSteps); if (!converged || bad_guidance() || bad_dV() || bad_pitch()) { - /* FIXME: probably shouldn't scribble over globals then restore them if they're bad */ + /* FIXME: probably shouldn't scribble over globals then restore them if they're bad -- + should scribble in local vars and then set them if they're good. */ A = startingA; B = startingB; T = startingT; - sane_guidance = false; + saneGuidance = false; } else { - sane_guidance = true; + saneGuidance = true; } } @@ -293,9 +290,12 @@ public override bool DriveAscent(FlightCtrlState s) return true; } + double ascentStartTime = 0.0D; + void DriveVerticalAscent(FlightCtrlState s) { - if (!IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface)) mode = AscentMode.INITIATE_TURN; + if (ascentStartTime > 0.0D && (vesselState.time - ascentStartTime ) > pitchStartTime) + mode = AscentMode.INITIATE_TURN; //during the vertical ascent we just thrust straight up at max throttle attitudeTo(90); @@ -304,49 +304,53 @@ void DriveVerticalAscent(FlightCtrlState s) if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; - if (!vessel.LiftedOff() || vessel.Landed) status = "Awaiting liftoff"; - else status = "Vertical ascent"; + if (!vessel.LiftedOff() || vessel.Landed) { + status = "Awaiting liftoff"; + } + else + { + if (ascentStartTime == 0.0D) + ascentStartTime = vesselState.time; + double dt = pitchStartTime - ( vesselState.time - ascentStartTime ); + status = "Vertical ascent " + dt + " s"; + } } void DriveInitiateTurn(FlightCtrlState s) { - if ((90 - turnStartPitch) >= srfvelPitch()) + if ((vesselState.time - ascentStartTime ) > pitchEndTime) { mode = AscentMode.GRAVITY_TURN; 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; - } + double dt = vesselState.time - ascentStartTime - pitchStartTime; + double theta = dt * pitchRate; + attitudeTo(Math.Min(90, 90 - theta + pitchBias)); - attitudeTo(90 - turnStartPitch); - - status = "Initiate gravity turn"; + status = "Pitch program " + (pitchEndTime - pitchStartTime - dt) + " s"; } void DriveGravityTurn(FlightCtrlState s) { if (h >= hT) { + status = "Angular momentum target achieved"; core.thrust.targetThrottle = 0.0F; mode = AscentMode.EXIT; return; } - if (sane_guidance) { - attitudeTo(Math.Asin(A + C) * UtilMath.Rad2Deg); + if (saneGuidance) { + status = "Stable PEG Guidance"; + attitudeTo(guidancePitch); } else { // srfvelPitch == zero AoA - attitudeTo(srfvelPitch()); + status = "Unguided Gravity Turn"; + attitudeTo(Math.Min(90, srfvelPitch() + pitchBias)); } - - status = "Gravity turn"; } } } diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs new file mode 100644 index 000000000..8c77b4567 --- /dev/null +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -0,0 +1,65 @@ +using System; +using UnityEngine; + +namespace MuMech +{ + public class MechJebModuleAscentPEGMenu : MechJebModuleAscentMenuBase + { + public MechJebModuleAscentPEGMenu(MechJebCore core) + : base(core) + { + hidden = true; + } + + public MechJebModuleAscentPEG path { get { return autopilot.ascentPath as MechJebModuleAscentPEG; } } + public MechJebModuleAscentAutopilot autopilot; + + public override void OnStart(PartModule.StartState state) + { + autopilot = core.GetComputerModule(); + } + + public override GUILayoutOption[] WindowOptions() + { + return new GUILayoutOption[] { GUILayout.Width(300), GUILayout.Height(100) }; + } + + protected override void WindowGUI(int windowID) + { + if (path == null) + { + GUILayout.Label("Path is null!!!1!!1!1!1111!11eleven"); + base.WindowGUI(windowID); + return; + } + + GUILayout.BeginVertical(); + + GuiUtils.SimpleTextBox("Booster Pitch start:", path.pitchStartTime, "s"); + GuiUtils.SimpleTextBox("Booster Pitch rate:", path.pitchRate, "°/s"); + GuiUtils.SimpleTextBox("Booster Pitch end:", path.pitchEndTime, "s"); + GuiUtils.SimpleTextBox("Pitch adjustment:", path.pitchBias, "°"); + + GUILayout.EndVertical(); + GUILayout.BeginVertical(); + + GUILayout.Label("Burnout Stats"); + GUILayout.Label("delta-V: " + path.dV); + GUILayout.Label("time: " + path.T); + GUILayout.Label("pitch: " + path.guidancePitch); + + if (autopilot.enabled) + { + GUILayout.Label("Autopilot status: " + autopilot.status); + } + + GUILayout.EndVertical(); + base.WindowGUI(windowID); + } + + public override string GetName() + { + return "Atlas/Centaur PEG Pitch Program"; + } + } +} From 5bd10e6ccdb83d9acd10bb6e926b6f80e05f28fb Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sun, 25 Jun 2017 21:32:03 -0700 Subject: [PATCH 03/51] tweak menus Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 8 ++++---- MechJeb2/MechJebModuleAscentPEGMenu.cs | 7 ++++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index a50d45f4c..50dea4628 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -225,14 +225,14 @@ private void converge(double dt, bool initialize = false) double startingB = B; bool converged = false; - for(convergenceSteps = 0; convergenceSteps < 50; convergenceSteps++) { + for(convergenceSteps = 1; convergenceSteps <= 250; convergenceSteps++) { double oldT = T; if (convergenceSteps == 0) peg_estimate(dt); else peg_estimate(0); peg_solve(); - if ( Math.Abs(T - oldT) < 0.1 ) { + if ( Math.Abs(T - oldT) < 0.01 ) { converged = true; break; } @@ -312,7 +312,7 @@ void DriveVerticalAscent(FlightCtrlState s) if (ascentStartTime == 0.0D) ascentStartTime = vesselState.time; double dt = pitchStartTime - ( vesselState.time - ascentStartTime ); - status = "Vertical ascent " + dt + " s"; + status = String.Format("Vertical ascent {0:F2} s", dt); } } @@ -328,7 +328,7 @@ void DriveInitiateTurn(FlightCtrlState s) double theta = dt * pitchRate; attitudeTo(Math.Min(90, 90 - theta + pitchBias)); - status = "Pitch program " + (pitchEndTime - pitchStartTime - dt) + " s"; + status = String.Format("Pitch program {0:F2} s", pitchEndTime - pitchStartTime - dt); } void DriveGravityTurn(FlightCtrlState s) diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 8c77b4567..3053de994 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -44,9 +44,10 @@ protected override void WindowGUI(int windowID) GUILayout.BeginVertical(); GUILayout.Label("Burnout Stats"); - GUILayout.Label("delta-V: " + path.dV); - GUILayout.Label("time: " + path.T); - GUILayout.Label("pitch: " + path.guidancePitch); + GUILayout.Label(String.Format("delta-V: {0:F2}", path.dV)); + GUILayout.Label(String.Format("time: {0:F2}", path.T)); + GUILayout.Label(String.Format("pitch: {0:F2}", path.guidancePitch)); + GUILayout.Label(String.Format("steps: {0:F2}", path.convergenceSteps)); if (autopilot.enabled) { From 7ea2267599cc3cfa019395b8dee51941f6bbee1f Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sun, 25 Jun 2017 23:19:21 -0700 Subject: [PATCH 04/51] disable PEG Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 4 ++-- MechJeb2/MechJebModuleAscentPEGMenu.cs | 13 +++++++++++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 50dea4628..dd3c8e784 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -206,8 +206,8 @@ private bool bad_guidance() return Double.IsNaN(T) || Double.IsInfinity(T) || T <= 0.0D || Double.IsNaN(A) || Double.IsInfinity(A) || Double.IsNaN(B) || Double.IsInfinity(B); } + public bool guidanceEnabled = true; public bool saneGuidance = false; - public int convergenceSteps; private void converge(double dt, bool initialize = false) @@ -341,7 +341,7 @@ void DriveGravityTurn(FlightCtrlState s) return; } - if (saneGuidance) { + if (saneGuidance && guidanceEnabled) { status = "Stable PEG Guidance"; attitudeTo(guidancePitch); } diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 3053de994..d9c95393d 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -49,11 +49,24 @@ protected override void WindowGUI(int windowID) GUILayout.Label(String.Format("pitch: {0:F2}", path.guidancePitch)); GUILayout.Label(String.Format("steps: {0:F2}", path.convergenceSteps)); + if (path.guidanceEnabled) + { + if (GUILayout.Button("Disable PEG Guidance")) + path.guidanceEnabled = false; + } + else + { + if (GUILayout.Button("Enable PEG Guidance")) + path.guidanceEnabled = true; + } + + if (autopilot.enabled) { GUILayout.Label("Autopilot status: " + autopilot.status); } + GUILayout.EndVertical(); base.WindowGUI(windowID); } From d66120dd908ba4f99f6e8caf8e325eec27db48cd Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Mon, 26 Jun 2017 17:51:20 -0700 Subject: [PATCH 05/51] working terminal guidance and misc UI tweaks Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 57 +++++++++++++++++--------- MechJeb2/MechJebModuleAscentPEGMenu.cs | 23 ++++++++--- 2 files changed, 56 insertions(+), 24 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index dd3c8e784..bdb6b6987 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -12,7 +12,7 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase { public MechJebModuleAscentPEG(MechJebCore core) : base(core) { } - /* default pitch program here works seemingly decent at SLT of about 1.4 */ + /* default pitch program here works decently at SLT of about 1.4 */ [Persistent(pass = (int)(Pass.Type | Pass.Global))] public EditableDoubleMult pitchStartTime = new EditableDoubleMult(10); [Persistent(pass = (int)(Pass.Type | Pass.Global))] @@ -20,7 +20,12 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase [Persistent(pass = (int)(Pass.Type | Pass.Global))] public EditableDoubleMult pitchEndTime = new EditableDoubleMult(55); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult pitchBias = new EditableDoubleMult(0); + public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(100000, 1000); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult terminalGuidanceSecs = new EditableDoubleMult(10); + + /* this deliberately does not persist, it is for emergencies only */ + public EditableDoubleMult pitchBias = new EditableDoubleMult(0); private MechJebModuleStageStats stats { get { return core.GetComputerModule(); } } private FuelFlowSimulation.Stats[] vacStats { get { return stats.vacStats; } } @@ -148,10 +153,6 @@ private void peg_solve() private void peg_estimate(double dt, bool debug = false) { - double oldA = A; - double oldB = B; - double oldT = T; - /* update old guidance */ A = A + B * dt; B = B; @@ -209,6 +210,7 @@ private bool bad_guidance() public bool guidanceEnabled = true; public bool saneGuidance = false; public int convergenceSteps; + public bool terminalGuidance = false; private void converge(double dt, bool initialize = false) { @@ -224,23 +226,36 @@ private void converge(double dt, bool initialize = false) double startingA = A; double startingB = B; - bool converged = false; - for(convergenceSteps = 1; convergenceSteps <= 250; convergenceSteps++) { - double oldT = T; - if (convergenceSteps == 0) - peg_estimate(dt); - else - peg_estimate(0); - peg_solve(); - if ( Math.Abs(T - oldT) < 0.01 ) { - converged = true; - break; + bool stable = false; + + if (T < terminalGuidanceSecs) + { + peg_estimate(dt); + terminalGuidance = true; + stable = true; /* terminal guidance is always considered stable */ + } + else + { + for(convergenceSteps = 1; convergenceSteps <= 250; convergenceSteps++) { + double oldT = T; + if (convergenceSteps == 0) + peg_estimate(dt); + else + peg_estimate(0); + + peg_solve(); + + if ( Math.Abs(T - oldT) < 0.01 ) { + stable = true; + break; + } } + terminalGuidance = false; } Debug.Log("pitch = " + Math.Asin(A+C) + " dV = " + dV + " cycles = " + convergenceSteps); - if (!converged || bad_guidance() || bad_dV() || bad_pitch()) + if (!stable || bad_guidance() || bad_dV() || bad_pitch()) { /* FIXME: probably shouldn't scribble over globals then restore them if they're bad -- should scribble in local vars and then set them if they're good. */ @@ -342,7 +357,11 @@ void DriveGravityTurn(FlightCtrlState s) } if (saneGuidance && guidanceEnabled) { - status = "Stable PEG Guidance"; + if (terminalGuidance) + status = "Locked Terminal Guidance"; + else + status = "Stable PEG Guidance"; + attitudeTo(guidancePitch); } else diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index d9c95393d..49879a2dd 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -35,19 +35,30 @@ protected override void WindowGUI(int windowID) GUILayout.BeginVertical(); + GuiUtils.SimpleTextBox("Target Periapsis:", autopilot.desiredOrbitAltitude, "km"); + GuiUtils.SimpleTextBox("Target Apoapsis:", path.desiredApoapsis, "km"); + if ( path.desiredApoapsis < autopilot.desiredOrbitAltitude ) + { + GUIStyle s = new GUIStyle(GUI.skin.label); + s.normal.textColor = Color.yellow; + GUILayout.Label("Apoapsis < Periapsis: circularizing orbit at periapsis", s); + } + GuiUtils.SimpleTextBox("Booster Pitch start:", path.pitchStartTime, "s"); GuiUtils.SimpleTextBox("Booster Pitch rate:", path.pitchRate, "°/s"); GuiUtils.SimpleTextBox("Booster Pitch end:", path.pitchEndTime, "s"); - GuiUtils.SimpleTextBox("Pitch adjustment:", path.pitchBias, "°"); + GUILayout.Label(String.Format("ending pitch: {0:F1}°", (path.pitchEndTime - path.pitchStartTime)*path.pitchRate)); + GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); + GUILayout.EndVertical(); GUILayout.BeginVertical(); GUILayout.Label("Burnout Stats"); - GUILayout.Label(String.Format("delta-V: {0:F2}", path.dV)); - GUILayout.Label(String.Format("time: {0:F2}", path.T)); - GUILayout.Label(String.Format("pitch: {0:F2}", path.guidancePitch)); - GUILayout.Label(String.Format("steps: {0:F2}", path.convergenceSteps)); + GUILayout.Label(String.Format("delta-V: {0:F1}", path.dV)); + GUILayout.Label(String.Format("time: {0:F1}", path.T)); + GUILayout.Label(String.Format("pitch: {0:F1}", path.guidancePitch)); + GUILayout.Label(String.Format("steps: {0:D}", path.convergenceSteps)); if (path.guidanceEnabled) { @@ -60,6 +71,8 @@ protected override void WindowGUI(int windowID) path.guidanceEnabled = true; } + GuiUtils.SimpleTextBox("Emergency pitch adj.:", path.pitchBias, "°"); + if (autopilot.enabled) { From 09387245d41b53c2baabb3c8c36de717fea9b041 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Mon, 26 Jun 2017 19:27:02 -0700 Subject: [PATCH 06/51] working apoapsis control Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index bdb6b6987..feed82ca6 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -53,7 +53,7 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private double tau; /* current acceleration */ private double a0; - /* tangential velocity at burnout FIXME: circular for now */ + /* tangential velocity at burnout */ private double vT; /* radius at burnout */ private double rT; @@ -62,7 +62,7 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; /* gravParameter */ private double GM; - /* ending radial velocity (FIXME: circular for now) */ + /* ending radial velocity */ double rdT; /* current radial velocity */ double rd; @@ -77,7 +77,7 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; double w; /* mean radius */ double rbar; - /* angular momentum at burnout FIXME: circular for now */ + /* angular momentum at burnout */ double hT; /* angular momentum */ double h; @@ -90,11 +90,18 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; /* gravity + centrifugal force at burnout */ double CT; + public double sma() { + if ( desiredApoapsis > autopilot.desiredOrbitAltitude ) + return (autopilot.desiredOrbitAltitude + 2 * mainBody.Radius + desiredApoapsis) / 2; + else + return autopilot.desiredOrbitAltitude + mainBody.Radius; + } + private void update_rocket_stats() { /* sometimes the last stage in MJ has 0.0 dV and we have to search back for the actively burning stage */ for(int i = vacStats.Length - 1; i >= 0; i--) { - if ( vacStats[i].deltaV > 0.0D ) + if ( vacStats[i].deltaV > 0 ) { last_stage = i; break; @@ -106,16 +113,16 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; a0 = vesselState.currentThrustAccel; tau = v_e / a0; rT = autopilot.desiredOrbitAltitude + mainBody.Radius; - vT = Math.Sqrt(GM/rT); /* FIXME: assumes circular */ + vT = Math.Sqrt(GM * (2/rT - 1/sma())); /* FIXME: assumes periapsis insertion */ r = mainBody.position.magnitude; - rdT = 0; /* FIXME: assumes circular */ + rdT = 0; /* FIXME: assumes periapsis insertion */ rd = vesselState.speedVertical; wT = vT / rT; w = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude / (r * r); rbar = ( rT + r ) / 2.0D; - hT = rT * vT; /* FIXME: assumes circular */ + hT = rT * vT; /* FIXME: assumes periapsis insertion */ h = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude; dh = hT - h; aT = a0 / ( 1.0D - T / tau ); From 13de2931edebccc74a11692abe45e300c92a042e Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Mon, 26 Jun 2017 21:33:30 -0700 Subject: [PATCH 07/51] better A, B, T initialization Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index feed82ca6..842cee5b2 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -223,9 +223,9 @@ private void converge(double dt, bool initialize = false) { if (initialize || bad_guidance() || bad_pitch() || bad_dV() || !saneGuidance) { - T = 120.0D; - A = 0.0D; - B = 0.0D; + T = vacStats[last_stage].deltaTime; + A = -0.4; + B = 0.0036; dt = 0.0; } From 320b7099d5192309be9c79cfc8151c94490292b7 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Mon, 26 Jun 2017 22:43:38 -0700 Subject: [PATCH 08/51] fix annoyances Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 7 +++++++ MechJeb2/MechJebModuleAscentPEGMenu.cs | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 842cee5b2..fbbab6ec2 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -355,6 +355,13 @@ void DriveInitiateTurn(FlightCtrlState s) void DriveGravityTurn(FlightCtrlState s) { + if ((vesselState.time - ascentStartTime ) < pitchEndTime) + { + /* this can happen when users update the endtime box */ + mode = AscentMode.INITIATE_TURN; + return; + } + if (h >= hT) { status = "Angular momentum target achieved"; diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 49879a2dd..8f79b4adf 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -47,7 +47,7 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Booster Pitch start:", path.pitchStartTime, "s"); GuiUtils.SimpleTextBox("Booster Pitch rate:", path.pitchRate, "°/s"); GuiUtils.SimpleTextBox("Booster Pitch end:", path.pitchEndTime, "s"); - GUILayout.Label(String.Format("ending pitch: {0:F1}°", (path.pitchEndTime - path.pitchStartTime)*path.pitchRate)); + GUILayout.Label(String.Format("ending pitch: {0:F1}°", 90.0 - (path.pitchEndTime - path.pitchStartTime)*path.pitchRate)); GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); From 437a585c027630e45808c44841bcc8da01230b7d Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Tue, 27 Jun 2017 17:37:58 -0700 Subject: [PATCH 09/51] add strange looking dV estimate and remove debug loggin Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 34 ++++++++++++-------------- MechJeb2/MechJebModuleAscentPEGMenu.cs | 5 +++- 2 files changed, 20 insertions(+), 19 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index fbbab6ec2..61ac8ea6e 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -89,8 +89,14 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; double C; /* gravity + centrifugal force at burnout */ double CT; - - public double sma() { + /* specific orbital energy at burnout */ + double eT; + /* current specific orbital energy */ + double e0; + /* dV estimated from difference in specific orbital energy*/ + public double dVest; + + public double smaT() { if ( desiredApoapsis > autopilot.desiredOrbitAltitude ) return (autopilot.desiredOrbitAltitude + 2 * mainBody.Radius + desiredApoapsis) / 2; else @@ -113,9 +119,13 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; a0 = vesselState.currentThrustAccel; tau = v_e / a0; rT = autopilot.desiredOrbitAltitude + mainBody.Radius; - vT = Math.Sqrt(GM * (2/rT - 1/sma())); /* FIXME: assumes periapsis insertion */ + vT = Math.Sqrt(GM * (2/rT - 1/smaT())); /* FIXME: assumes periapsis insertion */ r = mainBody.position.magnitude; + eT = - GM / (2*smaT()); + e0 = - GM / (2*orbit.semiMajorAxis); + dVest = Math.Sqrt(2 * Math.Abs(eT - e0)); + rdT = 0; /* FIXME: assumes periapsis insertion */ rd = vesselState.speedVertical; @@ -129,11 +139,6 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; C = (GM / (r * r) - w * w * r ) / a0; CT = (GM / (rT * rT) - wT * wT * rT ) / aT; - - Debug.Log("GM = " + GM + " v_e = " + v_e + " tau = " + tau + "vT = " + vT); - Debug.Log("aT = " + aT + " rT = " + rT + " rdT = " + rdT + " wT = " + wT + " hT = " + hT); - Debug.Log("a0 = " + a0 + " r = " + r + " rd = " + rd + " w = " + w + " h = " + h + " rbar = " + rbar); - Debug.Log("C = " + C + " CT = " + CT); } private void peg_solve() @@ -151,14 +156,14 @@ private void peg_solve() } /* steering constants */ - private double A; - private double B; + public double A; + public double B; /* time to burnout */ public double T; /* dV to add */ public double dV; - private void peg_estimate(double dt, bool debug = false) + private void peg_estimate(double dt) { /* update old guidance */ A = A + B * dt; @@ -187,11 +192,6 @@ private void peg_estimate(double dt, bool debug = false) /* cos pitch accel */ double fdd_th = - ( fd_r * fd_r + fd_h * fd_h ) / 2.0D; - if (debug) { - Debug.Log("f_th = " + f_th + " fd_th = " + fd_th + " fdd_th = " + fdd_th); - Debug.Log("f_r = " + f_r + " f_rT = " + f_rT + " fd_r = " + fd_r); - } - /* updated estimate of dV to burn */ dV = ( dh / rbar + v_e * T * ( fd_th + fdd_th * tau ) + fdd_th * v_e * T * T / 2.0D ) / ( f_th + fd_th * tau + fdd_th * tau * tau ); @@ -260,8 +260,6 @@ private void converge(double dt, bool initialize = false) terminalGuidance = false; } - Debug.Log("pitch = " + Math.Asin(A+C) + " dV = " + dV + " cycles = " + convergenceSteps); - if (!stable || bad_guidance() || bad_dV() || bad_pitch()) { /* FIXME: probably shouldn't scribble over globals then restore them if they're bad -- diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 8f79b4adf..0472da2b6 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -55,7 +55,10 @@ protected override void WindowGUI(int windowID) GUILayout.BeginVertical(); GUILayout.Label("Burnout Stats"); - GUILayout.Label(String.Format("delta-V: {0:F1}", path.dV)); + GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); + GUILayout.Label(String.Format("delta-V (guidance): {0:F1}", path.dV)); + GUILayout.Label(String.Format("A: {0:F1}", path.A)); + GUILayout.Label(String.Format("B: {0:F1}", path.B)); GUILayout.Label(String.Format("time: {0:F1}", path.T)); GUILayout.Label(String.Format("pitch: {0:F1}", path.guidancePitch)); GUILayout.Label(String.Format("steps: {0:D}", path.convergenceSteps)); From f92d08427d68150cda35bd9a50d149990500cca1 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Tue, 27 Jun 2017 17:47:54 -0700 Subject: [PATCH 10/51] use vessel.launchTime and fix state machine bugs Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 61ac8ea6e..ae49919b0 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -289,6 +289,10 @@ public override bool DriveAscent(FlightCtrlState s) last_time = vesselState.time; + if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; + + core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); + switch (mode) { case AscentMode.VERTICAL_ASCENT: @@ -310,41 +314,36 @@ public override bool DriveAscent(FlightCtrlState s) return true; } - double ascentStartTime = 0.0D; - void DriveVerticalAscent(FlightCtrlState s) { - if (ascentStartTime > 0.0D && (vesselState.time - ascentStartTime ) > pitchStartTime) - mode = AscentMode.INITIATE_TURN; //during the vertical ascent we just thrust straight up at max throttle attitudeTo(90); - core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); - - if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; - if (!vessel.LiftedOff() || vessel.Landed) { status = "Awaiting liftoff"; } else { - if (ascentStartTime == 0.0D) - ascentStartTime = vesselState.time; - double dt = pitchStartTime - ( vesselState.time - ascentStartTime ); + if ((vesselState.time - vessel.launchTime ) > pitchStartTime) + { + mode = AscentMode.INITIATE_TURN; + return; + } + double dt = pitchStartTime - ( vesselState.time - vessel.launchTime ); status = String.Format("Vertical ascent {0:F2} s", dt); } } void DriveInitiateTurn(FlightCtrlState s) { - if ((vesselState.time - ascentStartTime ) > pitchEndTime) + if ((vesselState.time - vessel.launchTime ) > pitchEndTime) { mode = AscentMode.GRAVITY_TURN; return; } - double dt = vesselState.time - ascentStartTime - pitchStartTime; + double dt = vesselState.time - vessel.launchTime - pitchStartTime; double theta = dt * pitchRate; attitudeTo(Math.Min(90, 90 - theta + pitchBias)); @@ -353,7 +352,7 @@ void DriveInitiateTurn(FlightCtrlState s) void DriveGravityTurn(FlightCtrlState s) { - if ((vesselState.time - ascentStartTime ) < pitchEndTime) + if ((vesselState.time - vessel.launchTime ) < pitchEndTime) { /* this can happen when users update the endtime box */ mode = AscentMode.INITIATE_TURN; From bb0f4e24a849706a7d919c1446de041e5608482e Mon Sep 17 00:00:00 2001 From: Ahmed Charles Date: Tue, 27 Jun 2017 17:55:16 -0700 Subject: [PATCH 11/51] Do some cleanup. --- MechJeb2/MechJebModuleAscentAutopilot.cs | 4 +- MechJeb2/MechJebModuleAscentPEG.cs | 89 ++++++++++++------------ 2 files changed, 47 insertions(+), 46 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentAutopilot.cs b/MechJeb2/MechJebModuleAscentAutopilot.cs index e8f3861b8..208a71044 100644 --- a/MechJeb2/MechJebModuleAscentAutopilot.cs +++ b/MechJeb2/MechJebModuleAscentAutopilot.cs @@ -220,10 +220,10 @@ void DriveAscent(FlightCtrlState s) DriveSolarPanels(s); if ( ascentPath.DriveAscent(s) ) { - Debug.Log("Remaining in Ascent"); + if (GameSettings.VERBOSE_DEBUG_LOG) { Debug.Log("Remaining in Ascent"); } status = ascentPath.status; } else { - Debug.Log("Ascend -> Circularize"); + if (GameSettings.VERBOSE_DEBUG_LOG) { Debug.Log("Ascend -> Circularize"); } mode = AscentMode.CIRCULARIZE; } } diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index ae49919b0..e6deb48e5 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -1,5 +1,4 @@ using System; -using KSP.UI.Screens; using UnityEngine; /* @@ -14,15 +13,15 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase /* default pitch program here works decently at SLT of about 1.4 */ [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult pitchStartTime = new EditableDoubleMult(10); + public EditableDoubleMult pitchStartTime = new EditableDoubleMult(10); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult pitchRate = new EditableDoubleMult(0.75); + public EditableDoubleMult pitchRate = new EditableDoubleMult(0.75); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult pitchEndTime = new EditableDoubleMult(55); + public EditableDoubleMult pitchEndTime = new EditableDoubleMult(55); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(100000, 1000); + public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(100000, 1000); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult terminalGuidanceSecs = new EditableDoubleMult(10); + public EditableDoubleMult terminalGuidanceSecs = new EditableDoubleMult(10); /* this deliberately does not persist, it is for emergencies only */ public EditableDoubleMult pitchBias = new EditableDoubleMult(0); @@ -39,12 +38,29 @@ public override void OnModuleDisabled() { } - enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; - AscentMode mode; + private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; + private AscentMode mode; /* guidancePitchAngle -- output from the guidance algorithm, not 'manual' pitch */ public double guidancePitch { get { return Math.Asin(A + C) * UtilMath.Rad2Deg; } } + /* time to burnout */ + public double T { get; private set; } + /* dV to add */ + public double dV { get; private set; } + /* dV estimated from difference in specific orbital energy*/ + public double dVest { get; private set; } + + /* steering constants */ + public double A { get; private set; } + public double B { get; private set; } + + public bool guidanceEnabled = true; + public int convergenceSteps { get; private set; } + + private bool saneGuidance = false; + private bool terminalGuidance = false; + /* current MJ stage index */ private int last_stage; /* current exhaust velocity */ @@ -63,40 +79,38 @@ enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private double GM; /* ending radial velocity */ - double rdT; + private double rdT; /* current radial velocity */ - double rd; + private double rd; /* r: this is in the radial direction (altitude to gain) */ - double dr; + private double dr; /* rdot: also in the radial direction (upwards velocity to lose) */ - double drd; + private double drd; /* angular velocity at burnout */ - double wT; + private double wT; /* current angular velocity */ - double w; + private double w; /* mean radius */ - double rbar; + private double rbar; /* angular momentum at burnout */ - double hT; + private double hT; /* angular momentum */ - double h; + private double h; /* angular momentum to gain */ - double dh; + private double dh; /* acceleration at burnout */ - double aT; + private double aT; /* current gravity + centrifugal force term */ - double C; + private double C; /* gravity + centrifugal force at burnout */ - double CT; + private double CT; /* specific orbital energy at burnout */ - double eT; + private double eT; /* current specific orbital energy */ - double e0; - /* dV estimated from difference in specific orbital energy*/ - public double dVest; + private double e0; - public double smaT() { + private double smaT() { if ( desiredApoapsis > autopilot.desiredOrbitAltitude ) return (autopilot.desiredOrbitAltitude + 2 * mainBody.Radius + desiredApoapsis) / 2; else @@ -155,19 +169,11 @@ private void peg_solve() B = ( -c0 * ( rdT - rd ) + b0 * ( rT - r - rd * T ) ) / d; } - /* steering constants */ - public double A; - public double B; - /* time to burnout */ - public double T; - /* dV to add */ - public double dV; - private void peg_estimate(double dt) { /* update old guidance */ A = A + B * dt; - B = B; + /* B does not change. */ T = T - dt; aT = a0 / ( 1.0D - T / tau ); @@ -214,11 +220,6 @@ private bool bad_guidance() return Double.IsNaN(T) || Double.IsInfinity(T) || T <= 0.0D || Double.IsNaN(A) || Double.IsInfinity(A) || Double.IsNaN(B) || Double.IsInfinity(B); } - public bool guidanceEnabled = true; - public bool saneGuidance = false; - public int convergenceSteps; - public bool terminalGuidance = false; - private void converge(double dt, bool initialize = false) { if (initialize || bad_guidance() || bad_pitch() || bad_dV() || !saneGuidance) @@ -275,7 +276,7 @@ private void converge(double dt, bool initialize = false) } } - double last_time = 0.0D; + private double last_time = 0.0D; public override bool DriveAscent(FlightCtrlState s) { @@ -314,7 +315,7 @@ public override bool DriveAscent(FlightCtrlState s) return true; } - void DriveVerticalAscent(FlightCtrlState s) + private void DriveVerticalAscent(FlightCtrlState s) { //during the vertical ascent we just thrust straight up at max throttle @@ -335,7 +336,7 @@ void DriveVerticalAscent(FlightCtrlState s) } } - void DriveInitiateTurn(FlightCtrlState s) + private void DriveInitiateTurn(FlightCtrlState s) { if ((vesselState.time - vessel.launchTime ) > pitchEndTime) { @@ -350,7 +351,7 @@ void DriveInitiateTurn(FlightCtrlState s) status = String.Format("Pitch program {0:F2} s", pitchEndTime - pitchStartTime - dt); } - void DriveGravityTurn(FlightCtrlState s) + private void DriveGravityTurn(FlightCtrlState s) { if ((vesselState.time - vessel.launchTime ) < pitchEndTime) { From 88d067e668ea13f853d3f8025bd7383f2a265cdb Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Tue, 27 Jun 2017 21:53:00 -0700 Subject: [PATCH 12/51] fix the magic sauce that prevents launch wiggles other way is broken. Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index e6deb48e5..5be1579f5 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -292,8 +292,6 @@ public override bool DriveAscent(FlightCtrlState s) if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; - core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); - switch (mode) { case AscentMode.VERTICAL_ASCENT: @@ -321,6 +319,8 @@ private void DriveVerticalAscent(FlightCtrlState s) //during the vertical ascent we just thrust straight up at max throttle attitudeTo(90); + core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); + if (!vessel.LiftedOff() || vessel.Landed) { status = "Awaiting liftoff"; } From 515d01d90bd2137745942326308e5cc9bf7fa6da Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Tue, 27 Jun 2017 22:54:37 -0700 Subject: [PATCH 13/51] better dV estimate Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 5be1579f5..428671675 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -137,8 +137,8 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; r = mainBody.position.magnitude; eT = - GM / (2*smaT()); - e0 = - GM / (2*orbit.semiMajorAxis); - dVest = Math.Sqrt(2 * Math.Abs(eT - e0)); + /* XXX: estimate on the pad is very low, might need mean-radius or might need potential energy term, should be closer to 8100m/s */ + dVest = Math.Sqrt(2 * ( eT + GM / r )) - vessel.obt_velocity.magnitude; rdT = 0; /* FIXME: assumes periapsis insertion */ rd = vesselState.speedVertical; From 655a8a4bafb13356ee3eedf3a3c12f92709cc9d0 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 28 Jun 2017 23:39:29 -0700 Subject: [PATCH 14/51] janky start at stage tracking Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 46 ++++++++++++++++++++++++-- MechJeb2/MechJebModuleAscentPEGMenu.cs | 9 +++-- 2 files changed, 48 insertions(+), 7 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 428671675..4564194ef 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -1,5 +1,6 @@ using System; using UnityEngine; +using System.Collections.Generic; /* * Atlas/Centaur-style PEG launches for RSS/RO @@ -22,12 +23,15 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(100000, 1000); [Persistent(pass = (int)(Pass.Type | Pass.Global))] public EditableDoubleMult terminalGuidanceSecs = new EditableDoubleMult(10); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableDoubleMult stageLowDVLimit = new EditableDoubleMult(20); /* this deliberately does not persist, it is for emergencies only */ public EditableDoubleMult pitchBias = new EditableDoubleMult(0); private MechJebModuleStageStats stats { get { return core.GetComputerModule(); } } private FuelFlowSimulation.Stats[] vacStats { get { return stats.vacStats; } } + private FuelFlowSimulation.Stats[] atmoStats { get { return stats.atmoStats; } } public override void OnModuleEnabled() { @@ -36,6 +40,7 @@ public override void OnModuleEnabled() public override void OnModuleDisabled() { + stages = null; } private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; @@ -110,6 +115,42 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; /* current specific orbital energy */ private double e0; + /* stage-specific information */ + public List stages = new List(); + + public struct StageInfo + { + public double vac_dV; /* total vacuum deltaV */ + public double vac_ve; /* vacuum exhaust velocity */ + public double vac_a0; /* starting/current vacuum thrust acceleration */ + public double atm_ve; /* atmospheric exhaust velocity */ + public double atm_a0; /* starting/current atmospheric thrust acceleration */ + public double A; + public double B; + public double K; + public double T; + public int mjStage; + } + + public int numStages; + + /* these stages are indexed so the currently burning stage is stage 0 and the last stage + is at the end of the array */ + void UpdateStageStats() + { + stages.Clear(); + for( int i = vacStats.Length-1; i >= 0; i-- ) + { + if ( vacStats[i].deltaV > stageLowDVLimit ) + { + StageInfo stage = new StageInfo(); + stage.vac_dV = vacStats[i].deltaV; + stage.mjStage = i; + stages.Add( stage ); + } + } + } + private double smaT() { if ( desiredApoapsis > autopilot.desiredOrbitAltitude ) return (autopilot.desiredOrbitAltitude + 2 * mainBody.Radius + desiredApoapsis) / 2; @@ -117,7 +158,8 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; return autopilot.desiredOrbitAltitude + mainBody.Radius; } - private void update_rocket_stats() { + private void UpdateRocketStats() { + UpdateStageStats(); /* sometimes the last stage in MJ has 0.0 dV and we have to search back for the actively burning stage */ for(int i = vacStats.Length - 1; i >= 0; i--) { @@ -281,7 +323,7 @@ private void converge(double dt, bool initialize = false) public override bool DriveAscent(FlightCtrlState s) { stats.RequestUpdate(this); - update_rocket_stats(); + UpdateRocketStats(); if (last_time != 0.0D) converge(vesselState.time - last_time); diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 0472da2b6..914a42818 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -49,11 +49,10 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Booster Pitch end:", path.pitchEndTime, "s"); GUILayout.Label(String.Format("ending pitch: {0:F1}°", 90.0 - (path.pitchEndTime - path.pitchStartTime)*path.pitchRate)); GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); - - - GUILayout.EndVertical(); - GUILayout.BeginVertical(); - + GUILayout.Label("Stage Stats"); + for(int i = path.stages.Count - 1; i >= 0; i--) { + GUILayout.Label(String.Format("{0:D}: {1:F1}", i+1, path.stages[i].vac_dV)); + } GUILayout.Label("Burnout Stats"); GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); GUILayout.Label(String.Format("delta-V (guidance): {0:F1}", path.dV)); From 80fe7091d81444cfadef8bd9d0fed308f7736d86 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 29 Jun 2017 00:40:05 -0700 Subject: [PATCH 15/51] moving some things around Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 4564194ef..e89a5b81a 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -332,8 +332,6 @@ public override bool DriveAscent(FlightCtrlState s) last_time = vesselState.time; - if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; - switch (mode) { case AscentMode.VERTICAL_ASCENT: @@ -349,10 +347,7 @@ public override bool DriveAscent(FlightCtrlState s) break; } - if (mode == AscentMode.EXIT) - return false; - else - return true; + return (mode != AscentMode.EXIT); } private void DriveVerticalAscent(FlightCtrlState s) @@ -360,6 +355,7 @@ private void DriveVerticalAscent(FlightCtrlState s) //during the vertical ascent we just thrust straight up at max throttle attitudeTo(90); + if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); @@ -380,6 +376,7 @@ private void DriveVerticalAscent(FlightCtrlState s) private void DriveInitiateTurn(FlightCtrlState s) { + if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; if ((vesselState.time - vessel.launchTime ) > pitchEndTime) { mode = AscentMode.GRAVITY_TURN; @@ -395,6 +392,7 @@ private void DriveInitiateTurn(FlightCtrlState s) private void DriveGravityTurn(FlightCtrlState s) { + if (autopilot.autoThrottle) core.thrust.targetThrottle = 1.0F; if ((vesselState.time - vessel.launchTime ) < pitchEndTime) { /* this can happen when users update the endtime box */ From 52dc6b9dcaf5e587595e725645c3fc6ac02c4d2f Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sat, 1 Jul 2017 09:56:56 -0700 Subject: [PATCH 16/51] mostly working stage tracking Signed-off-by: Lamont Granquist --- MechJeb2/FuelFlowSimulation.cs | 48 ++++++++---- MechJeb2/MechJebModuleAscentPEG.cs | 104 +++++++++++++++++++++---- MechJeb2/MechJebModuleAscentPEGMenu.cs | 2 +- 3 files changed, 120 insertions(+), 34 deletions(-) diff --git a/MechJeb2/FuelFlowSimulation.cs b/MechJeb2/FuelFlowSimulation.cs index 61ed081ba..824e53522 100644 --- a/MechJeb2/FuelFlowSimulation.cs +++ b/MechJeb2/FuelFlowSimulation.cs @@ -15,6 +15,7 @@ public class FuelFlowSimulation public int simStage; //the simulated rocket's current stage readonly List nodes = new List(); //a list of FuelNodes representing all the parts of the ship readonly Dictionary nodeLookup = new Dictionary(); + readonly Dictionary partLookup = new Dictionary(); private double KpaToAtmospheres; @@ -22,16 +23,18 @@ public class FuelFlowSimulation public void Init(List parts, bool dVLinearThrust) { KpaToAtmospheres = PhysicsGlobals.KpaToAtmospheres; - + // Create FuelNodes corresponding to each Part nodes.Clear(); nodeLookup.Clear(); + partLookup.Clear(); for (int index = 0; index < parts.Count; index++) { Part part = parts[index]; FuelNode node = FuelNode.Borrow(part, dVLinearThrust); nodeLookup[part] = node; + partLookup[node] = part; nodes.Add(node); } // Determine when each part will be decoupled @@ -46,7 +49,7 @@ public void Init(List parts, bool dVLinearThrust) } simStage = StageManager.LastStage + 1; - + // Add a fake stage if we are beyond the first one // Mostly useful for the Node Executor who use the last stage info // and fail to get proper info when the ship was never staged and @@ -112,6 +115,14 @@ private Stats SimulateStage(float throttle, double staticPressure, double atmDen stats.deltaTime = 0; stats.deltaV = 0; + // track active engines to "fingerprint" this stage + // (could improve by adding fuel tanks being drained and thereby support drop-tanks) + stats.parts = new List(); + var engines = FindActiveEngines().value; + for(int i = 0; i < engines.Count; i++) { + stats.parts.Add(partLookup[engines[i]]); + } + const int maxSteps = 100; int step; for (step = 0; step < maxSteps; step++) @@ -367,6 +378,8 @@ public struct Stats public double StartTWR(double geeASL) { return startMass > 0 ? startThrust / (9.80665 * geeASL * startMass) : 0; } public double MaxTWR(double geeASL) { return maxAccel / (9.80665 * geeASL); } + public List parts; + //Computes the deltaV from the other fields. Only valid when the thrust is constant over the time interval represented. public void ComputeTimeStepDeltaV() { @@ -392,6 +405,7 @@ public Stats Append(Stats s) maxAccel = Math.Max(this.maxAccel, s.maxAccel), deltaTime = this.deltaTime + (s.deltaTime < float.MaxValue && !double.IsInfinity(s.deltaTime) ? s.deltaTime : 0), deltaV = this.deltaV + s.deltaV, + parts = this.parts, isp = this.startMass == s.endMass ? 0 : (this.deltaV + s.deltaV) / (9.80665f * Math.Log(this.startMass / s.endMass)) }; } @@ -402,7 +416,7 @@ public Stats Append(Stats s) public class FuelNode { readonly DefaultableDictionary resources = new DefaultableDictionary(0); //the resources contained in the part - readonly KeyableDictionary resourceConsumptions = new KeyableDictionary(); //the resources this part consumes per unit time when active at full throttle + readonly KeyableDictionary resourceConsumptions = new KeyableDictionary(); //the resources this part consumes per unit time when active at full throttle readonly DefaultableDictionary resourceDrains = new DefaultableDictionary(0); //the resources being drained from this part per unit time at the current simulation time readonly DefaultableDictionary freeResources = new DefaultableDictionary(false); //the resources that are "free" and assumed to be infinite like IntakeAir @@ -422,9 +436,9 @@ public class FuelNode KeyableDictionary propellantRatios = new KeyableDictionary(); //ratios of propellants used by this engine KeyableDictionary propellantFlows = new KeyableDictionary(); //flow modes of propellants since the engine can override them float propellantSumRatioTimesDensity; //a number used in computing propellant consumption rates - + readonly List crossfeedSources = new List(); - + float maxFuelFlow = 0; //max fuel flow of this part float minFuelFlow = 0; //min fuel flow of this part @@ -485,16 +499,16 @@ private void Init(Part part, bool dVLinearThrust) propellantRatios.Clear(); propellantFlows.Clear(); - + crossfeedSources.Clear(); - + isEngine = false; dryMass = 0; modulesStagedMass = 0; decoupledInStage = int.MinValue; - + modulesUnstagedMass = 0; if (!part.IsLaunchClamp()) { @@ -505,7 +519,7 @@ private void Init(Part part, bool dVLinearThrust) modulesStagedMass = part.GetModuleMassNoAlloc((float) dryMass, ModifierStagingSituation.STAGED); float currentModulesMass = part.GetModuleMassNoAlloc((float) dryMass, ModifierStagingSituation.CURRENT); - + // if it was manually staged if (currentModulesMass == modulesStagedMass) { @@ -644,7 +658,7 @@ public void AssignDecoupledInStage(Part p, Dictionary nodeLookup for (int i = 0; i < p.Modules.Count; i++) { PartModule m = p.Modules[i]; - + ModuleDecouple mDecouple = m as ModuleDecouple; if (mDecouple != null) { @@ -701,14 +715,14 @@ public void AssignDecoupledInStage(Part p, Dictionary nodeLookup //print("AssignDecoupledInStage ModuleDecouple " + p.partInfo.name + "(" + p.inverseStage + ") decoupling " + attach.attachedPart + "(" + attach.attachedPart.inverseStage + "). not the parent " + decoupledInStage); // The part we decouple is dropped when we decouple nodeLookup[attach.attachedPart].AssignDecoupledInStage(attach.attachedPart, nodeLookup, p.inverseStage); - + } } } break; // Hopefully no one made part with multiple decoupler modules ? } } - + ModuleAnchoredDecoupler mAnchoredDecoupler = m as ModuleAnchoredDecoupler; if (mAnchoredDecoupler != null) { @@ -723,7 +737,7 @@ public void AssignDecoupledInStage(Part p, Dictionary nodeLookup { attach = p.srfAttachNode; } - + if (attach != null && attach.attachedPart != null) { if (attach.attachedPart == p.parent) @@ -807,7 +821,7 @@ public void AssignDecoupledInStage(Part p, Dictionary nodeLookup decoupledInStage = parentDecoupledInStage; //print("AssignDecoupledInStage " + p.partInfo.name + "(" + p.inverseStage + ")" + decoupledInStage); } - + isSepratron = isEngine && (inverseStage == decoupledInStage); for (int i = 0; i < p.children.Count; i++) @@ -857,7 +871,7 @@ public void AddCrossfeedSouces(HashSet parts, Dictionary n } } } - + //call this when a node no longer exists, so that this node knows that it's no longer a valid source public void RemoveSourceNode(FuelNode n) { @@ -943,7 +957,7 @@ public bool CanDrawNeededResources(List vessel) //check if we contain the needed resource: if (resources[type] < resourceRequestRemainingThreshold) return false; break; - + case ResourceFlowMode.ALL_VESSEL: case ResourceFlowMode.ALL_VESSEL_BALANCE: case ResourceFlowMode.STAGE_PRIORITY_FLOW: @@ -1004,7 +1018,7 @@ public void AssignResourceDrainRates(List vessel) case ResourceFlowMode.STAGE_PRIORITY_FLOW_BALANCE: AssignFuelDrainRateStagePriorityFlow(type, amount, true, vessel); break; - + case ResourceFlowMode.STAGE_STACK_FLOW: case ResourceFlowMode.STAGE_STACK_FLOW_BALANCE: case ResourceFlowMode.STACK_PRIORITY_SEARCH: diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index e89a5b81a..bd6f8712a 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -36,6 +36,7 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase public override void OnModuleEnabled() { mode = AscentMode.VERTICAL_ASCENT; + InitStageStats(); } public override void OnModuleDisabled() @@ -66,7 +67,7 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private bool saneGuidance = false; private bool terminalGuidance = false; - /* current MJ stage index */ + /* current KSP stage index */ private int last_stage; /* current exhaust velocity */ private double v_e; @@ -115,39 +116,110 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; /* current specific orbital energy */ private double e0; - /* stage-specific information */ + /* + * handle tracking ksp/mechjeb stage information and keep state about stages + * (this is mildly awful because of everything that can dynamically happen to + * stages in flight, including players manually rearranging stages). + */ + public List stages = new List(); - public struct StageInfo + public class StageInfo { public double vac_dV; /* total vacuum deltaV */ - public double vac_ve; /* vacuum exhaust velocity */ - public double vac_a0; /* starting/current vacuum thrust acceleration */ - public double atm_ve; /* atmospheric exhaust velocity */ - public double atm_a0; /* starting/current atmospheric thrust acceleration */ public double A; public double B; public double K; public double T; - public int mjStage; + public List parts; + public int kspStage; } - public int numStages; + void UpdateStageFromMechJeb(StageInfo stage) + { + /* stage.kspStage must be corrected before calling this */ + int s = stage.kspStage; + stage.vac_dV = vacStats[s].deltaV; + } - /* these stages are indexed so the currently burning stage is stage 0 and the last stage - is at the end of the array */ - void UpdateStageStats() + public List skippedParts = new List(); + + void InitStageStats() { stages.Clear(); - for( int i = vacStats.Length-1; i >= 0; i-- ) + skippedParts.Clear(); + for ( int i = vacStats.Length-1; i >= 0; i-- ) { if ( vacStats[i].deltaV > stageLowDVLimit ) { StageInfo stage = new StageInfo(); - stage.vac_dV = vacStats[i].deltaV; - stage.mjStage = i; + stage.parts = vacStats[i].parts; + stage.kspStage = i; + UpdateStageFromMechJeb(stage); stages.Add( stage ); } + else + { + skippedParts.AddRange( vacStats[i].parts ); + } + } + } + + bool PartsListsMatch(List one, List two) + { + for(int i = 0; i < one.Count; i++) + { + /* skip burned sepratrons that wind up in the stage, etc */ + if ( skippedParts.Contains(one[i]) ) + continue; + + if ( !two.Contains(one[i]) ) + return false; + } + for(int i = 0; i < two.Count; i++) + { + if ( skippedParts.Contains(two[i]) ) + continue; + + if ( !one.Contains(two[i]) ) + return false; + } + return true; + } + + int FixKSPStage(int oldstage, List parts) + { + if (oldstage < vacStats.Length && PartsListsMatch(vacStats[oldstage].parts, parts)) + return oldstage; + + for( int i = 0; i < vacStats.Length; i++ ) + { + if (PartsListsMatch(vacStats[i].parts, parts)) + return i; + } + return -1; + } + + void UpdateStageStats() + { + for ( int i = 0; i < stages.Count; i++ ) + { + StageInfo stage = stages[i]; + + /* skip already burned stage */ + if ( stage.kspStage < 0 ) + continue; + + stage.kspStage = FixKSPStage(stage.kspStage, stage.parts); + if ( stage.kspStage < 0 ) + { + /* we staged */ + stage.vac_dV = 0; + } + else + { + UpdateStageFromMechJeb(stage); + } } } @@ -161,7 +233,7 @@ void UpdateStageStats() private void UpdateRocketStats() { UpdateStageStats(); /* sometimes the last stage in MJ has 0.0 dV and we have to search back for the actively burning stage */ - for(int i = vacStats.Length - 1; i >= 0; i--) + for ( int i = vacStats.Length - 1; i >= 0; i-- ) { if ( vacStats[i].deltaV > 0 ) { diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 914a42818..6cc6b37c4 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -51,7 +51,7 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); GUILayout.Label("Stage Stats"); for(int i = path.stages.Count - 1; i >= 0; i--) { - GUILayout.Label(String.Format("{0:D}: {1:F1}", i+1, path.stages[i].vac_dV)); + GUILayout.Label(String.Format("{0:D}: {1:F1} {2:D}", i+1, path.stages[i].vac_dV, path.stages[i].kspStage)); } GUILayout.Label("Burnout Stats"); GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); From b5e2e43d95480bc4b5b61190da188af446da39f3 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sat, 1 Jul 2017 11:17:12 -0700 Subject: [PATCH 17/51] fix the annoying menu bugginess Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentAutopilot.cs | 2 ++ MechJeb2/MechJebModuleAscentGuidance.cs | 17 ++++++++--------- MechJeb2/MechJebModuleAscentPEG.cs | 2 +- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentAutopilot.cs b/MechJeb2/MechJebModuleAscentAutopilot.cs index 208a71044..bcbe17996 100644 --- a/MechJeb2/MechJebModuleAscentAutopilot.cs +++ b/MechJeb2/MechJebModuleAscentAutopilot.cs @@ -16,6 +16,8 @@ public class MechJebModuleAscentAutopilot : ComputerModule //input parameters: [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public int ascentPathIdx; + // this is not persisted, the ascentPathIdx controls it very indirectly (via being set from AscentGuidance) public MechJebModuleAscentBase ascentPath; [Persistent(pass = (int)(Pass.Type | Pass.Global))] public EditableDoubleMult desiredOrbitAltitude = new EditableDoubleMult(100000, 1000); diff --git a/MechJeb2/MechJebModuleAscentGuidance.cs b/MechJeb2/MechJebModuleAscentGuidance.cs index 758f8f90e..e0e7cc831 100644 --- a/MechJeb2/MechJebModuleAscentGuidance.cs +++ b/MechJeb2/MechJebModuleAscentGuidance.cs @@ -17,16 +17,16 @@ public class MechJebModuleAscentGuidance : DisplayModule public bool launchingToInterplanetary = false; public double interplanetaryWindowUT; - MechJebModuleAscentAutopilot autopilot; + public MechJebModuleAscentAutopilot autopilot { get { return core.GetComputerModule(); } } + public MechJebModuleAscentBase path; + public MechJebModuleAscentMenuBase editor; + MechJebModuleAscentNavBall navBall; - MechJebModuleAscentBase path; - MechJebModuleAscentMenuBase editor; - /* FIXME: this probably needs to get persisted? */ - public int ascentPathIdx = 0; + /* XXX: this is all a bit janky, could rub some reflection on it */ + public int ascentPathIdx { get { return autopilot.ascentPathIdx; } set { autopilot.ascentPathIdx = value; } } public string[] ascentPathList = { "Classic Ascent Profile", "Stock-style GravityTurn™", "Powered Explicit Guidance (RSS/RO)" }; - /* XXX: this is all a bit janky, could rub some reflection on it */ private void get_path_and_editor(int i, out MechJebModuleAscentBase p, out MechJebModuleAscentMenuBase e) { if ( i == 0 ) @@ -74,12 +74,13 @@ private void enable_path_module(int index) get_path_and_editor(index, out path, out editor); if (path != null) path.enabled = true; + + autopilot.ascentPath = path; /* the editor is not necessarily enabled by this, or the window will always pop up */ } public override void OnStart(PartModule.StartState state) { - autopilot = core.GetComputerModule(); if (autopilot != null) { desiredInclination = autopilot.desiredInclination; @@ -332,8 +333,6 @@ protected override void WindowGUI(int windowID) editor.enabled = last_enabled; } - autopilot.ascentPath = path; - if (editor != null) editor.enabled = GUILayout.Toggle(editor.enabled, "Edit ascent path"); GUILayout.EndVertical(); diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index bd6f8712a..08c08c6a8 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -41,7 +41,7 @@ public override void OnModuleEnabled() public override void OnModuleDisabled() { - stages = null; + stages.Clear(); } private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; From 1771ba7c78e4f068012a023eca3e3d07ecd17392 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sat, 1 Jul 2017 17:01:12 -0700 Subject: [PATCH 18/51] stage tracking working pretty well Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 38 ++++++++++++++++++-------- MechJeb2/MechJebModuleAscentPEGMenu.cs | 2 +- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 08c08c6a8..c3984f7dd 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -126,7 +126,9 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; public class StageInfo { - public double vac_dV; /* total vacuum deltaV */ + public double dV; + public double v_e; + public double deltaTime; public double A; public double B; public double K; @@ -139,13 +141,16 @@ void UpdateStageFromMechJeb(StageInfo stage) { /* stage.kspStage must be corrected before calling this */ int s = stage.kspStage; - stage.vac_dV = vacStats[s].deltaV; + stage.dV = vacStats[s].deltaV; + stage.deltaTime = vacStats[s].deltaTime; + stage.v_e = vacStats[s].isp * 9.80665; } public List skippedParts = new List(); void InitStageStats() { + Debug.Log("MechJebModuleAscentPEG.InitStageStats()"); stages.Clear(); skippedParts.Clear(); for ( int i = vacStats.Length-1; i >= 0; i-- ) @@ -163,6 +168,13 @@ void InitStageStats() skippedParts.AddRange( vacStats[i].parts ); } } + /* sometimes parts wind up in zero dV stages and we need to remove them here */ + for ( int i = 0; i < stages.Count; i++ ) { + for ( int j = 0; j < stages[i].parts.Count; j++ ) { + if ( skippedParts.Contains(stages[i].parts[j]) ) + skippedParts.Remove(stages[i].parts[j]); + } + } } bool PartsListsMatch(List one, List two) @@ -190,7 +202,9 @@ bool PartsListsMatch(List one, List two) int FixKSPStage(int oldstage, List parts) { if (oldstage < vacStats.Length && PartsListsMatch(vacStats[oldstage].parts, parts)) + { return oldstage; + } for( int i = 0; i < vacStats.Length; i++ ) { @@ -202,25 +216,25 @@ int FixKSPStage(int oldstage, List parts) void UpdateStageStats() { + if ( stages.Count == 0 ) + InitStageStats(); + for ( int i = 0; i < stages.Count; i++ ) { StageInfo stage = stages[i]; - /* skip already burned stage */ - if ( stage.kspStage < 0 ) - continue; - stage.kspStage = FixKSPStage(stage.kspStage, stage.parts); - if ( stage.kspStage < 0 ) - { - /* we staged */ - stage.vac_dV = 0; - } - else + + if ( stage.kspStage >= 0 ) { UpdateStageFromMechJeb(stage); } } + for ( int i = stages.Count - 1; i >= 0; i-- ) + { + if ( stages[i].kspStage < 0 ) + stages.RemoveAt(i); + } } private double smaT() { diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 6cc6b37c4..61ed02431 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -51,7 +51,7 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); GUILayout.Label("Stage Stats"); for(int i = path.stages.Count - 1; i >= 0; i--) { - GUILayout.Label(String.Format("{0:D}: {1:F1} {2:D}", i+1, path.stages[i].vac_dV, path.stages[i].kspStage)); + GUILayout.Label(String.Format("{0:D}: {1:F1} {2:F1} {3:F1} {4:D}", i, path.stages[i].dV, path.stages[i].v_e, path.stages[i].deltaTime, path.stages[i].kspStage)); } GUILayout.Label("Burnout Stats"); GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); From 9e24ad062880da2a3807efbbc918f7eaf79f34e3 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sat, 1 Jul 2017 17:24:52 -0700 Subject: [PATCH 19/51] get PEG information from our stage information Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index c3984f7dd..67887ed17 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -67,8 +67,6 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private bool saneGuidance = false; private bool terminalGuidance = false; - /* current KSP stage index */ - private int last_stage; /* current exhaust velocity */ private double v_e; /* time to burn the entire vehicle */ @@ -128,6 +126,7 @@ public class StageInfo { public double dV; public double v_e; + public double a0; public double deltaTime; public double A; public double B; @@ -144,6 +143,7 @@ void UpdateStageFromMechJeb(StageInfo stage) stage.dV = vacStats[s].deltaV; stage.deltaTime = vacStats[s].deltaTime; stage.v_e = vacStats[s].isp * 9.80665; + stage.a0 = vacStats[s].startThrust / vacStats[s].startMass; } public List skippedParts = new List(); @@ -246,19 +246,10 @@ void UpdateStageStats() private void UpdateRocketStats() { UpdateStageStats(); - /* sometimes the last stage in MJ has 0.0 dV and we have to search back for the actively burning stage */ - for ( int i = vacStats.Length - 1; i >= 0; i-- ) - { - if ( vacStats[i].deltaV > 0 ) - { - last_stage = i; - break; - } - } GM = mainBody.gravParameter; - v_e = vacStats[last_stage].isp * 9.80665; - a0 = vesselState.currentThrustAccel; + v_e = stages[0].v_e; + a0 = stages[0].a0; tau = v_e / a0; rT = autopilot.desiredOrbitAltitude + mainBody.Radius; vT = Math.Sqrt(GM * (2/rT - 1/smaT())); /* FIXME: assumes periapsis insertion */ @@ -352,7 +343,7 @@ private void converge(double dt, bool initialize = false) { if (initialize || bad_guidance() || bad_pitch() || bad_dV() || !saneGuidance) { - T = vacStats[last_stage].deltaTime; + T = stages[0].deltaTime; A = -0.4; B = 0.0036; dt = 0.0; From 444d335e046c3fc0c2f1fecff09e329cf354b6ae Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sat, 1 Jul 2017 19:38:27 -0700 Subject: [PATCH 20/51] some atmo stats Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 67887ed17..75b16a329 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -136,14 +136,24 @@ public class StageInfo public int kspStage; } - void UpdateStageFromMechJeb(StageInfo stage) + void UpdateStageFromMechJeb(StageInfo stage, bool atmo = false) { /* stage.kspStage must be corrected before calling this */ int s = stage.kspStage; - stage.dV = vacStats[s].deltaV; - stage.deltaTime = vacStats[s].deltaTime; - stage.v_e = vacStats[s].isp * 9.80665; - stage.a0 = vacStats[s].startThrust / vacStats[s].startMass; + if (atmo) /* really "current" stats */ + { + stage.dV = atmoStats[s].deltaV; + stage.deltaTime = atmoStats[s].deltaTime; + stage.v_e = atmoStats[s].isp * 9.80665; + stage.a0 = atmoStats[s].startThrust / atmoStats[s].startMass; + } + else + { + stage.dV = vacStats[s].deltaV; + stage.deltaTime = vacStats[s].deltaTime; + stage.v_e = vacStats[s].isp * 9.80665; + stage.a0 = vacStats[s].startThrust / vacStats[s].startMass; + } } public List skippedParts = new List(); @@ -160,7 +170,7 @@ void InitStageStats() StageInfo stage = new StageInfo(); stage.parts = vacStats[i].parts; stage.kspStage = i; - UpdateStageFromMechJeb(stage); + UpdateStageFromMechJeb(stage, i == 0); stages.Add( stage ); } else @@ -400,6 +410,7 @@ private void converge(double dt, bool initialize = false) public override bool DriveAscent(FlightCtrlState s) { stats.RequestUpdate(this); + stats.liveSLT = true; /* yes, this disables the button, yes, it is important we do this */ UpdateRocketStats(); if (last_time != 0.0D) From 1e1a59f25747020d7f1c67241fbaed6e76906ec5 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sun, 2 Jul 2017 00:39:09 -0700 Subject: [PATCH 21/51] PEG refactor Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 120 ++++++++++--------------- MechJeb2/MechJebModuleAscentPEGMenu.cs | 6 +- 2 files changed, 52 insertions(+), 74 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 75b16a329..bbc1e763b 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -48,31 +48,19 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private AscentMode mode; /* guidancePitchAngle -- output from the guidance algorithm, not 'manual' pitch */ - public double guidancePitch { get { return Math.Asin(A + C) * UtilMath.Rad2Deg; } } + public double guidancePitch { get { return Math.Asin(stages[0].A + stages[0].C) * UtilMath.Rad2Deg; } } - /* time to burnout */ - public double T { get; private set; } /* dV to add */ public double dV { get; private set; } /* dV estimated from difference in specific orbital energy*/ public double dVest { get; private set; } - /* steering constants */ - public double A { get; private set; } - public double B { get; private set; } - public bool guidanceEnabled = true; public int convergenceSteps { get; private set; } private bool saneGuidance = false; private bool terminalGuidance = false; - /* current exhaust velocity */ - private double v_e; - /* time to burn the entire vehicle */ - private double tau; - /* current acceleration */ - private double a0; /* tangential velocity at burnout */ private double vT; /* radius at burnout */ @@ -86,10 +74,6 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private double rdT; /* current radial velocity */ private double rd; - /* r: this is in the radial direction (altitude to gain) */ - private double dr; - /* rdot: also in the radial direction (upwards velocity to lose) */ - private double drd; /* angular velocity at burnout */ private double wT; @@ -103,16 +87,8 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private double h; /* angular momentum to gain */ private double dh; - /* acceleration at burnout */ - private double aT; - /* current gravity + centrifugal force term */ - private double C; - /* gravity + centrifugal force at burnout */ - private double CT; /* specific orbital energy at burnout */ private double eT; - /* current specific orbital energy */ - private double e0; /* * handle tracking ksp/mechjeb stage information and keep state about stages @@ -130,6 +106,7 @@ public class StageInfo public double deltaTime; public double A; public double B; + public double C; public double K; public double T; public List parts; @@ -258,9 +235,6 @@ void UpdateStageStats() UpdateStageStats(); GM = mainBody.gravParameter; - v_e = stages[0].v_e; - a0 = stages[0].a0; - tau = v_e / a0; rT = autopilot.desiredOrbitAltitude + mainBody.Radius; vT = Math.Sqrt(GM * (2/rT - 1/smaT())); /* FIXME: assumes periapsis insertion */ r = mainBody.position.magnitude; @@ -278,14 +252,14 @@ void UpdateStageStats() hT = rT * vT; /* FIXME: assumes periapsis insertion */ h = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude; dh = hT - h; - aT = a0 / ( 1.0D - T / tau ); - - C = (GM / (r * r) - w * w * r ) / a0; - CT = (GM / (rT * rT) - wT * wT * rT ) / aT; } - private void peg_solve() + private void peg_solve1(StageInfo stage) { + double T = stage.T; + double v_e = stage.v_e; + double a0 = stage.a0; + double tau = v_e / a0; double b0 = -v_e * Math.Log(1.0D - T/tau); double b1 = b0 * tau - v_e * T; @@ -294,19 +268,32 @@ private void peg_solve() double d = b0 * c1 - b1 * c0; - A = ( c1 * ( rdT - rd ) - b1 * ( rT - r - rd * T ) ) / d; - B = ( -c0 * ( rdT - rd ) + b0 * ( rT - r - rd * T ) ) / d; + stage.A = ( c1 * ( rdT - rd ) - b1 * ( rT - r - rd * T ) ) / d; + stage.B = ( -c0 * ( rdT - rd ) + b0 * ( rT - r - rd * T ) ) / d; + } + + private void peg_update(double dt, StageInfo stage) + { + /* update old guidance */ + stage.A = stage.A + stage.B * dt; + /* B does not update */ + stage.T = stage.T - dt; } - private void peg_estimate(double dt) + private void peg_estimate(StageInfo stage) { /* update old guidance */ - A = A + B * dt; - /* B does not change. */ - T = T - dt; + double A = stage.A; + double B = stage.B; + double T = stage.T; - aT = a0 / ( 1.0D - T / tau ); - CT = (GM / (rT * rT) - wT * wT * rT ) / aT; + double v_e = stage.v_e; + double a0 = stage.a0; + double tau = v_e / a0; + + double aT = a0 / ( 1.0D - T / tau ); + double C = stage.C = (GM / (r * r) - w * w * r ) / a0; + double CT = (GM / (rT * rT) - wT * wT * rT ) / aT; /* current sin pitch */ double f_r = A + C; @@ -331,11 +318,12 @@ private void peg_estimate(double dt) dV = ( dh / rbar + v_e * T * ( fd_th + fdd_th * tau ) + fdd_th * v_e * T * T / 2.0D ) / ( f_th + fd_th * tau + fdd_th * tau * tau ); /* updated estimate of T */ - T = tau * ( 1 - Math.Exp( - dV / v_e ) ); + stage.T = tau * ( 1 - Math.Exp( - dV / v_e ) ); } private bool bad_dV() { + /* FIXME: this could look for other obviously insane values */ return dV <= 0.0D; } @@ -344,65 +332,55 @@ private bool bad_pitch() return Double.IsNaN(guidancePitch); } - private bool bad_guidance() + private bool bad_guidance(StageInfo stage) { + double A = stage.A; + double T = stage.T; + double B = stage.B; return Double.IsNaN(T) || Double.IsInfinity(T) || T <= 0.0D || Double.IsNaN(A) || Double.IsInfinity(A) || Double.IsNaN(B) || Double.IsInfinity(B); } private void converge(double dt, bool initialize = false) { - if (initialize || bad_guidance() || bad_pitch() || bad_dV() || !saneGuidance) + if (initialize || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance) { - T = stages[0].deltaTime; - A = -0.4; - B = 0.0036; + stages[0].T = stages[0].deltaTime; + stages[0].A = -0.4; + stages[0].B = 0.0036; dt = 0.0; } - double startingT = T; - double startingA = A; - double startingB = B; - bool stable = false; - if (T < terminalGuidanceSecs) + peg_update(dt, stages[0]); + + if (stages[0].T < terminalGuidanceSecs) { - peg_estimate(dt); + peg_estimate(stages[0]); terminalGuidance = true; stable = true; /* terminal guidance is always considered stable */ } else { - for(convergenceSteps = 1; convergenceSteps <= 250; convergenceSteps++) { - double oldT = T; - if (convergenceSteps == 0) - peg_estimate(dt); - else - peg_estimate(0); + for(convergenceSteps = 1; convergenceSteps <= 50; convergenceSteps++) { + double oldT = stages[0].T; - peg_solve(); + peg_estimate(stages[0]); + peg_solve1(stages[0]); - if ( Math.Abs(T - oldT) < 0.01 ) { + if ( Math.Abs(stages[0].T - oldT) < 0.01 ) { stable = true; break; } + /* FIXME: consider breaking out on bad_guidance() here */ } terminalGuidance = false; } - if (!stable || bad_guidance() || bad_dV() || bad_pitch()) + if (!stable || bad_guidance(stages[0]) || bad_dV() || bad_pitch()) { - /* FIXME: probably shouldn't scribble over globals then restore them if they're bad -- - should scribble in local vars and then set them if they're good. */ - A = startingA; - B = startingB; - T = startingT; saneGuidance = false; } - else - { - saneGuidance = true; - } } private double last_time = 0.0D; diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 61ed02431..23151c524 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -56,9 +56,9 @@ protected override void WindowGUI(int windowID) GUILayout.Label("Burnout Stats"); GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); GUILayout.Label(String.Format("delta-V (guidance): {0:F1}", path.dV)); - GUILayout.Label(String.Format("A: {0:F1}", path.A)); - GUILayout.Label(String.Format("B: {0:F1}", path.B)); - GUILayout.Label(String.Format("time: {0:F1}", path.T)); + GUILayout.Label(String.Format("A: {0:F1}", path.stages[0].A)); + GUILayout.Label(String.Format("B: {0:F1}", path.stages[0].B)); + GUILayout.Label(String.Format("time: {0:F1}", path.stages[0].T)); GUILayout.Label(String.Format("pitch: {0:F1}", path.guidancePitch)); GUILayout.Label(String.Format("steps: {0:D}", path.convergenceSteps)); From 4059a5623ce74ccf9043c3f4f8aeda8e6452a337 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Sun, 2 Jul 2017 21:12:54 -0700 Subject: [PATCH 22/51] back to Stable Guidance! Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 196 ++++++++++++++++++++----- MechJeb2/MechJebModuleAscentPEGMenu.cs | 4 +- 2 files changed, 158 insertions(+), 42 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index bbc1e763b..b0b0cb176 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -51,7 +51,7 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; public double guidancePitch { get { return Math.Asin(stages[0].A + stages[0].C) * UtilMath.Rad2Deg; } } /* dV to add */ - public double dV { get; private set; } + /* public double dV { get; private set; } */ /* dV estimated from difference in specific orbital energy*/ public double dVest { get; private set; } @@ -72,8 +72,6 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; /* ending radial velocity */ private double rdT; - /* current radial velocity */ - private double rd; /* angular velocity at burnout */ private double wT; @@ -100,37 +98,47 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; public class StageInfo { - public double dV; + public double avail_dV; public double v_e; + public double tau; public double a0; public double deltaTime; public double A; public double B; public double C; - public double K; + public double dV; public double T; + public double rd0; + public double dr; + public double drd; public List parts; public int kspStage; + public override string ToString() + { + return "A = " + A + "\n" + + "B = " + B + "\n" + + "C = " + C + "\n" + + "T = " + T + "\n" + + "a0 = " + a0 + "\n" + + "v_e = " + v_e + "\n" + + "tau = " + tau + "\n" + + "rd0 = " + rd0 + "\n" + + "dr = " + dr + "\n" + + "drd = " + drd + "\n"; + } } void UpdateStageFromMechJeb(StageInfo stage, bool atmo = false) { /* stage.kspStage must be corrected before calling this */ int s = stage.kspStage; - if (atmo) /* really "current" stats */ - { - stage.dV = atmoStats[s].deltaV; - stage.deltaTime = atmoStats[s].deltaTime; - stage.v_e = atmoStats[s].isp * 9.80665; - stage.a0 = atmoStats[s].startThrust / atmoStats[s].startMass; - } - else - { - stage.dV = vacStats[s].deltaV; - stage.deltaTime = vacStats[s].deltaTime; - stage.v_e = vacStats[s].isp * 9.80665; - stage.a0 = vacStats[s].startThrust / vacStats[s].startMass; - } + FuelFlowSimulation.Stats[] mjstats = atmo ? atmoStats : vacStats; + + stage.avail_dV = mjstats[s].deltaV; + stage.deltaTime = mjstats[s].deltaTime; + stage.v_e = mjstats[s].isp * 9.80665; + stage.a0 = mjstats[s].startThrust / mjstats[s].startMass; + stage.tau = stage.v_e / stage.a0; } public List skippedParts = new List(); @@ -155,7 +163,7 @@ void InitStageStats() skippedParts.AddRange( vacStats[i].parts ); } } - /* sometimes parts wind up in zero dV stages and we need to remove them here */ + /* sometimes parts we want also wind up in zero dV stages and we need to remove them here */ for ( int i = 0; i < stages.Count; i++ ) { for ( int j = 0; j < stages[i].parts.Count; j++ ) { if ( skippedParts.Contains(stages[i].parts[j]) ) @@ -244,7 +252,7 @@ void UpdateStageStats() dVest = Math.Sqrt(2 * ( eT + GM / r )) - vessel.obt_velocity.magnitude; rdT = 0; /* FIXME: assumes periapsis insertion */ - rd = vesselState.speedVertical; + stages[0].rd0 = vesselState.speedVertical; wT = vT / rT; w = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude / (r * r); @@ -254,22 +262,99 @@ void UpdateStageStats() dh = hT - h; } - private void peg_solve1(StageInfo stage) + /* FIXME: some memoization */ + private double b(int n, int snum) { - double T = stage.T; - double v_e = stage.v_e; - double a0 = stage.a0; - double tau = v_e / a0; + StageInfo stage = stages[snum]; + if (n == 0) + return stage.dV; - double b0 = -v_e * Math.Log(1.0D - T/tau); - double b1 = b0 * tau - v_e * T; - double c0 = b0 * T - b1; - double c1 = c0 * tau - v_e * T * T / 2.0D; + return b(n-1, snum) * stage.tau - stage.v_e * Math.Pow(stage.T, n) / n; + } + + /* FIXME: some memoization */ + private double c(int n, int snum) + { + StageInfo stage = stages[snum]; + if (n == 0) + return b(0, snum) * stage.T - b(1, snum); + + return c(n-1, snum) * stage.tau - stage.v_e * Math.Pow(stage.T, n+1) / ( n * (n + 1 ) ); + } + + private double alpha(int snum) + { + double sum = 0; + for(int l = 0; l <= snum; l++) + sum += b(0, l); + return sum; + } - double d = b0 * c1 - b1 * c0; + private double beta(int snum) + { + double sum = 0; + for(int l = 0; l <= snum; l++) + { + double sum2 = 0; + for(int k = 0; k <= (l-1); k++) + { + sum2 += stages[k].T; + } - stage.A = ( c1 * ( rdT - rd ) - b1 * ( rT - r - rd * T ) ) / d; - stage.B = ( -c0 * ( rdT - rd ) + b0 * ( rT - r - rd * T ) ) / d; + sum += b(1, l) + b(0, l) * sum2; + } + return sum; + } + + private double gamma(int snum) + { + double sum = 0; + for(int l = 0; l <= snum; l++) + { + double sum2 = 0; + for(int k = 0; k <= (l-1); k++) { + sum += b(0, k); + } + + sum += c(0, l) + stages[l].T * sum2; + } + return sum; + } + + private double delta(int snum) + { + double sum = 0; + for(int l = 0; l <= snum; l++) + { + double sum2 = 0; + for(int k = 0; k <= (l-1); k++) { + double sum3 = 0; + for(int i = 0; i <= (k - 1); i++) { + sum3 += stages[i].T; + } + sum2 += c(0, l) * stages[k].T + b(1, k) * stages[l].T + b(0, k) * stages[l].T * sum3; + } + sum += c(1, l) + sum2; + } + return sum; + } + + private void peg_solve(int snum) + { + double dr = stages[snum].dr; + double drd = stages[snum].drd; + + double a = alpha(snum); + double b = beta(snum); + double g = gamma(snum); + double d = delta(snum); + + double D = a * d - b * g; + + //Debug.Log(snum + " a:" + a + " b:" + b + " g:" + g + " d:" + d + " D:" + D); + + stages[0].A = ( d * drd - b * dr ) / D; + stages[0].B = ( a * dr - g * drd ) / D; } private void peg_update(double dt, StageInfo stage) @@ -280,8 +365,10 @@ private void peg_update(double dt, StageInfo stage) stage.T = stage.T - dt; } - private void peg_estimate(StageInfo stage) + private void peg_estimate(int snum) { + StageInfo stage = stages[snum]; + /* update old guidance */ double A = stage.A; double B = stage.B; @@ -315,16 +402,19 @@ private void peg_estimate(StageInfo stage) double fdd_th = - ( fd_r * fd_r + fd_h * fd_h ) / 2.0D; /* updated estimate of dV to burn */ - dV = ( dh / rbar + v_e * T * ( fd_th + fdd_th * tau ) + fdd_th * v_e * T * T / 2.0D ) / ( f_th + fd_th * tau + fdd_th * tau * tau ); + stage.dV = ( dh / rbar + v_e * T * ( fd_th + fdd_th * tau ) + fdd_th * v_e * T * T / 2.0D ) / ( f_th + fd_th * tau + fdd_th * tau * tau ); /* updated estimate of T */ - stage.T = tau * ( 1 - Math.Exp( - dV / v_e ) ); + stage.T = tau * ( 1 - Math.Exp( - stage.dV / v_e ) ); + + stage.drd = rdT - stage.rd0; + stage.dr = rT - r - stage.rd0 * T; } private bool bad_dV() { /* FIXME: this could look for other obviously insane values */ - return dV <= 0.0D; + return stages[0].dV <= 0.0D; } private bool bad_pitch() @@ -354,9 +444,12 @@ private void converge(double dt, bool initialize = false) peg_update(dt, stages[0]); + //Debug.Log("ONE:"); + //Debug.Log(stages[0]); + if (stages[0].T < terminalGuidanceSecs) { - peg_estimate(stages[0]); + peg_estimate(0); terminalGuidance = true; stable = true; /* terminal guidance is always considered stable */ } @@ -365,8 +458,18 @@ private void converge(double dt, bool initialize = false) for(convergenceSteps = 1; convergenceSteps <= 50; convergenceSteps++) { double oldT = stages[0].T; - peg_estimate(stages[0]); - peg_solve1(stages[0]); + peg_estimate(0); + //if (convergenceSteps == 1) + //{ + // Debug.Log("TWO:"); + // Debug.Log(stages[0]); + //} + peg_solve(0); + //if (convergenceSteps == 1) + //{ + // Debug.Log("THREE:"); + // Debug.Log(stages[0]); + //} if ( Math.Abs(stages[0].T - oldT) < 0.01 ) { stable = true; @@ -377,10 +480,23 @@ private void converge(double dt, bool initialize = false) terminalGuidance = false; } + if (!stable) + Debug.Log("unstable"); + if (bad_guidance(stages[0])) + Debug.Log("bad guidance"); + if (bad_dV()) + Debug.Log("bad dV"); + if (bad_pitch()) + Debug.Log("bad pitch"); + if (!stable || bad_guidance(stages[0]) || bad_dV() || bad_pitch()) { saneGuidance = false; } + else + { + saneGuidance = true; + } } private double last_time = 0.0D; diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 23151c524..6822d8954 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -51,11 +51,11 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); GUILayout.Label("Stage Stats"); for(int i = path.stages.Count - 1; i >= 0; i--) { - GUILayout.Label(String.Format("{0:D}: {1:F1} {2:F1} {3:F1} {4:D}", i, path.stages[i].dV, path.stages[i].v_e, path.stages[i].deltaTime, path.stages[i].kspStage)); + GUILayout.Label(String.Format("{0:D}: {1:F1} {2:F1} {3:F1} {4:D}", i, path.stages[i].avail_dV, path.stages[i].v_e, path.stages[i].deltaTime, path.stages[i].kspStage)); } GUILayout.Label("Burnout Stats"); GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); - GUILayout.Label(String.Format("delta-V (guidance): {0:F1}", path.dV)); + GUILayout.Label(String.Format("delta-V (guidance): {0:F1}", path.stages[0].dV)); GUILayout.Label(String.Format("A: {0:F1}", path.stages[0].A)); GUILayout.Label(String.Format("B: {0:F1}", path.stages[0].B)); GUILayout.Label(String.Format("time: {0:F1}", path.stages[0].T)); From 16850bbe3d06083124bdff2b204d72f63c4e0396 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Mon, 3 Jul 2017 09:59:10 -0700 Subject: [PATCH 23/51] remove debug log spam again Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 9 --------- 1 file changed, 9 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index b0b0cb176..d1bf2fc00 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -480,15 +480,6 @@ private void converge(double dt, bool initialize = false) terminalGuidance = false; } - if (!stable) - Debug.Log("unstable"); - if (bad_guidance(stages[0])) - Debug.Log("bad guidance"); - if (bad_dV()) - Debug.Log("bad dV"); - if (bad_pitch()) - Debug.Log("bad pitch"); - if (!stable || bad_guidance(stages[0]) || bad_dV() || bad_pitch()) { saneGuidance = false; From f8dfa517a2e2fe4ae264a0a181388690567e1565 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 5 Jul 2017 00:59:36 -0700 Subject: [PATCH 24/51] almost working 2-stage PEG Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 306 +++++++++++++++++-------- MechJeb2/MechJebModuleAscentPEGMenu.cs | 22 +- 2 files changed, 227 insertions(+), 101 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index d1bf2fc00..3dc841e9e 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -48,45 +48,40 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private AscentMode mode; /* guidancePitchAngle -- output from the guidance algorithm, not 'manual' pitch */ - public double guidancePitch { get { return Math.Asin(stages[0].A + stages[0].C) * UtilMath.Rad2Deg; } } + public double guidancePitch { get { return Math.Asin(stages[0].A + stages[0].G / stages[0].a0 ) * UtilMath.Rad2Deg; } } /* dV to add */ /* public double dV { get; private set; } */ /* dV estimated from difference in specific orbital energy*/ - public double dVest { get; private set; } + // public double dVest { get; private set; } public bool guidanceEnabled = true; public int convergenceSteps { get; private set; } private bool saneGuidance = false; - private bool terminalGuidance = false; + public bool terminalGuidance = false; /* tangential velocity at burnout */ - private double vT; + private double v_burnout; /* radius at burnout */ - private double rT; - /* radius now */ - private double r; + private double r_burnout; /* gravParameter */ private double GM; - /* ending radial velocity */ - private double rdT; - + private double rd_burnout; /* angular velocity at burnout */ - private double wT; - /* current angular velocity */ - private double w; - /* mean radius */ - private double rbar; + private double w_burnout; /* angular momentum at burnout */ - private double hT; - /* angular momentum */ - private double h; - /* angular momentum to gain */ - private double dh; + private double h_burnout; + /* G at burnout */ + private double G_burnout; /* specific orbital energy at burnout */ private double eT; + /* average change in the angle of thrust over the whole burn */ + private double fd_r; + + public double total_T; + public double total_dV; /* * handle tracking ksp/mechjeb stage information and keep state about stages @@ -98,33 +93,64 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; public class StageInfo { - public double avail_dV; - public double v_e; + /* updated directly from vehicle stats */ + public double v_e; /* current/initial exhaust velocity */ public double tau; - public double a0; - public double deltaTime; + public double a0; /* current/initial acceleration */ + public double avail_dV; /* stage stat from MJ */ + public double avail_T; /* stage stat from MJ */ + + public double dV; /* lower stages are == avail_dV, upper stage needs estimation */ + public double T; /* lower stages are == avail_T, upper stage needs estimation */ + + /* steering constants */ public double A; public double B; - public double C; - public double dV; - public double T; - public double rd0; - public double dr; - public double drd; + public double dA; + public double dB; + + /* output from estimation */ + public double G; /* current G */ + public double GT; /* final G */ + public double aT; /* final acceleration */ + + /* input */ + public double r; /* current radius */ + public double rd; /* initial upwards velocity */ + public double h; /* current angular momentum */ + + /* output deltas */ + public double dr; /* delta r */ + public double drd; /* delta rdot */ + public double dh; /* delta h */ + + /* output estimates */ + public double rT; /* ending radius */ + public double rdT; /* final upwards velocity */ + public double hT; /* ending angular momentum */ + + /* steering */ + public double f_r; /* current/initial steering sin angle */ + public double f_rT; /* ending steering sin angle */ + + /* misc */ public List parts; public int kspStage; public override string ToString() { return "A = " + A + "\n" + "B = " + B + "\n" + - "C = " + C + "\n" + + "G = " + G + "\n" + "T = " + T + "\n" + "a0 = " + a0 + "\n" + "v_e = " + v_e + "\n" + "tau = " + tau + "\n" + - "rd0 = " + rd0 + "\n" + + "r = " + r + "\n" + + "rd = " + rd + "\n" + + "h = " + h + "\n" + "dr = " + dr + "\n" + - "drd = " + drd + "\n"; + "drd = " + drd + "\n" + + "dh = " + dh + "\n"; } } @@ -135,7 +161,7 @@ void UpdateStageFromMechJeb(StageInfo stage, bool atmo = false) FuelFlowSimulation.Stats[] mjstats = atmo ? atmoStats : vacStats; stage.avail_dV = mjstats[s].deltaV; - stage.deltaTime = mjstats[s].deltaTime; + stage.avail_T = mjstats[s].deltaTime; stage.v_e = mjstats[s].isp * 9.80665; stage.a0 = mjstats[s].startThrust / mjstats[s].startMass; stage.tau = stage.v_e / stage.a0; @@ -227,8 +253,10 @@ void UpdateStageStats() } for ( int i = stages.Count - 1; i >= 0; i-- ) { - if ( stages[i].kspStage < 0 ) + if ( stages[i].kspStage < 0 ) { + num_stages--; /* FIXME this needs to be way smarter */ stages.RemoveAt(i); + } } } @@ -243,23 +271,20 @@ void UpdateStageStats() UpdateStageStats(); GM = mainBody.gravParameter; - rT = autopilot.desiredOrbitAltitude + mainBody.Radius; - vT = Math.Sqrt(GM * (2/rT - 1/smaT())); /* FIXME: assumes periapsis insertion */ - r = mainBody.position.magnitude; + stages[num_stages - 1].rT = r_burnout = autopilot.desiredOrbitAltitude + mainBody.Radius; + + v_burnout = Math.Sqrt(GM * (2/r_burnout - 1/smaT())); /* FIXME: assumes periapsis insertion */ + stages[0].r = mainBody.position.magnitude; eT = - GM / (2*smaT()); /* XXX: estimate on the pad is very low, might need mean-radius or might need potential energy term, should be closer to 8100m/s */ - dVest = Math.Sqrt(2 * ( eT + GM / r )) - vessel.obt_velocity.magnitude; + // dVest = Math.Sqrt(2 * ( eT + GM / r )) - vessel.obt_velocity.magnitude; - rdT = 0; /* FIXME: assumes periapsis insertion */ - stages[0].rd0 = vesselState.speedVertical; + stages[num_stages - 1].rdT = rd_burnout = 0; /* FIXME: assumes periapsis insertion */ + stages[0].rd = vesselState.speedVertical; - wT = vT / rT; - w = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude / (r * r); - rbar = ( rT + r ) / 2.0D; - hT = rT * vT; /* FIXME: assumes periapsis insertion */ - h = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude; - dh = hT - h; + stages[num_stages - 1].h = h_burnout = r_burnout * v_burnout; /* FIXME: assumes periapsis insertion */ + stages[0].h = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude; } /* FIXME: some memoization */ @@ -357,19 +382,32 @@ private void peg_solve(int snum) stages[0].B = ( a * dr - g * drd ) / D; } - private void peg_update(double dt, StageInfo stage) + private void peg_update(double dt) { - /* update old guidance */ - stage.A = stage.A + stage.B * dt; - /* B does not update */ - stage.T = stage.T - dt; + stages[0].A = stages[0].A + stages[0].B * dt; + if (num_stages == 0) { + /* if we only have one stage, count down the estimate */ + stages[0].T -= dt; + } + for (int i = 0; i < num_stages - 1 ; i++) { + /* all the lower stages are assumed to burn completely */ + stages[i].dV = stages[i].avail_dV; + stages[i].T = stages[i].avail_T; + } + for (int i = 0; i < num_stages; i++) { + stages[i].dA = 0.0; + stages[i].dB = 0.0; + } } + int num_stages = 2; + private void peg_estimate(int snum) { StageInfo stage = stages[snum]; - /* update old guidance */ + bool upper = ( snum == num_stages - 1); /* final stage */ + double A = stage.A; double B = stage.B; double T = stage.T; @@ -377,38 +415,93 @@ private void peg_estimate(int snum) double v_e = stage.v_e; double a0 = stage.a0; double tau = v_e / a0; + double r = stage.r; + double rd = stage.rd; + double h = stage.h; - double aT = a0 / ( 1.0D - T / tau ); - double C = stage.C = (GM / (r * r) - w * w * r ) / a0; - double CT = (GM / (rT * rT) - wT * wT * rT ) / aT; + double G = stage.G = ( GM - h * h / r ) / ( r * r ); + + double f_r = stage.f_r = A + G / a0; /* current sin pitch */ - double f_r = A + C; + // double f_r = A + G / a0; /* sin pitch at burnout */ - double f_rT = A + B * T + CT; + // double f_rT = A + B * T + GT/aT; /* appx rate of sin pitch */ - double fd_r = ( f_rT - f_r ) / T; - - /* placeholder for future yaw term */ - double f_h = 0.0D; - /* placeholder for future yaw rate term */ - double fd_h = 0.0D; + // double fd_r = ( f_rT - f_r ) / T; /* cos pitch */ - double f_th = 1.0D - f_r * f_r / 2.0D - f_h * f_h / 2.0D; + double f_th = 1.0D - stage.f_r * stage.f_r / 2.0D; /* cos pitch rate */ - double fd_th = - ( f_r * fd_r + f_h * fd_h ); + double fd_th = - stage.f_r * fd_r; /* cos pitch accel */ - double fdd_th = - ( fd_r * fd_r + fd_h * fd_h ) / 2.0D; + double fdd_th = - ( fd_r * fd_r ) / 2.0D; + + double rT; + double rdT; + double hT; + + + if (upper) { + double dh = stage.dh = 2 * ( h_burnout - h ) / ( r_burnout + r ); + hT = stage.hT = h + dh; + + /* updated estimate of dV to burn */ + stage.dV = ( dh + v_e * T * ( fd_th + fdd_th * ( tau + T / 2.0 ) ) ) / ( ( fdd_th * tau + fd_th ) * tau + f_th ); + + /* updated estimate of T */ + T = stage.T = tau * ( 1 - Math.Exp( - stage.dV / v_e ) ); + + total_T = 0.0; + for(int i = 0; i < num_stages; i++) + { + total_T += stages[i].T; + } + + double drd = stage.drd = rd_burnout - stages[0].rd - b(0,1) * stage.dA - b(1, 1) * stage.dB; + double dr = stage.dr = r_burnout - stages[0].r - stages[0].rd * total_T - c(0,1) * stage.dA - c(1, 1) * stage.dB; + + rT = stage.rT = r + dr; + rdT = stage.rdT = rd + drd; + } else { /* boosters */ + double dr = stage.dr = rd * T + c(0,0) * A + c(1, 0) * B; + double drd = stage.drd = b(0,0) * A + b(1, 0) * B; + rT = stage.rT = r + dr; + rdT = stage.rdT = rd + drd; + double dh = stage.dh = ( r + rT ) / 2.0 * ( f_th * b(0, 0) + fd_th * b(1, 0) + fdd_th * b(2, 0)); + hT = stage.hT = h + dh; + } - /* updated estimate of dV to burn */ - stage.dV = ( dh / rbar + v_e * T * ( fd_th + fdd_th * tau ) + fdd_th * v_e * T * T / 2.0D ) / ( f_th + fd_th * tau + fdd_th * tau * tau ); + double aT = stage.aT = a0 / ( 1.0D - T / tau ); + double GT = stage.GT = ( GM - hT * hT / rT ) / ( rT * rT ); - /* updated estimate of T */ - stage.T = tau * ( 1 - Math.Exp( - stage.dV / v_e ) ); + double f_rT = A + B * T + GT / aT; - stage.drd = rdT - stage.rd0; - stage.dr = rT - r - stage.rd0 * T; + /* set next stages initial conditions */ + if (!upper) { + double dA = GT * ( 1.0 / aT - 1.0 / stages[snum+1].a0 ); + double dB = GT * ( 1.0 / stages[snum+1].v_e - 1.0 / v_e ) + ( 3 * hT * hT / rT - 2 * GM ) * rdT / ( rT * rT * rT ) * ( 1.0 / aT - 1.0 / stages[snum+1].a0 ); + + stages[snum+1].r = rT; + stages[snum+1].rd = rdT; + stages[snum+1].h = hT; + stages[snum+1].A = A + dA; + stages[snum+1].B = B + dB; + stages[snum+1].dA = dA; + stages[snum+1].dB = dB; + } + } + + private void peg_final() { + total_T = 0.0; + total_dV = 0.0; + for(int i = 0; i < num_stages; i++) + { + total_T += stages[i].T; + total_dV += stages[i].dV; + } + /* appx rate of sin pitch over the whole burn */ + fd_r = ( stages[num_stages-1].f_rT - stages[0].f_r ) / total_T; } private bool bad_dV() @@ -432,46 +525,67 @@ private bool bad_guidance(StageInfo stage) private void converge(double dt, bool initialize = false) { - if (initialize || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance) - { - stages[0].T = stages[0].deltaTime; - stages[0].A = -0.4; - stages[0].B = 0.0036; - dt = 0.0; + double Astart = stages[0].A; + double Bstart = stages[0].B; + double Tstart = stages[num_stages-1].T; + + if (num_stages == 1 && stages[0].T < terminalGuidanceSecs && saneGuidance) + terminalGuidance = true; + + if (!terminalGuidance) { + if (initialize || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance ) + { + stages[num_stages-1].T = stages[num_stages-1].avail_T; + stages[0].A = -0.9; + stages[0].B = 0.0036; + fd_r = -0.001; + dt = 0.0; + } } bool stable = false; - peg_update(dt, stages[0]); + peg_update(dt); //Debug.Log("ONE:"); //Debug.Log(stages[0]); - if (stages[0].T < terminalGuidanceSecs) + if (terminalGuidance) { peg_estimate(0); - terminalGuidance = true; + peg_final(); stable = true; /* terminal guidance is always considered stable */ } else { + Debug.Log("=========== START ================"); for(convergenceSteps = 1; convergenceSteps <= 50; convergenceSteps++) { double oldT = stages[0].T; - peg_estimate(0); + for(int i = 0; i < num_stages; i++) + peg_estimate(i); + + peg_final(); + //if (convergenceSteps == 1) //{ // Debug.Log("TWO:"); // Debug.Log(stages[0]); //} - peg_solve(0); - //if (convergenceSteps == 1) - //{ - // Debug.Log("THREE:"); - // Debug.Log(stages[0]); - //} + peg_solve(num_stages - 1); + if (convergenceSteps == 1) + { + Debug.Log("BOOSTER:"); + Debug.Log(stages[0]); + } + if (convergenceSteps == 1) + { + Debug.Log("UPPER:"); + Debug.Log(stages[1]); + } - if ( Math.Abs(stages[0].T - oldT) < 0.01 ) { + Debug.Log(" deltaT = " + (stages[0].T - oldT)); + if ( Math.Abs(stages[0].T - oldT) < 0.1 ) { stable = true; break; } @@ -482,7 +596,15 @@ private void converge(double dt, bool initialize = false) if (!stable || bad_guidance(stages[0]) || bad_dV() || bad_pitch()) { - saneGuidance = false; + stages[0].A = Astart; + stages[0].B = Bstart; + stages[num_stages-1].T = Tstart; + + /* if we're within 30 secs of burnout and we just lost guidance, switch to terminal guidance */ + if (saneGuidance && num_stages == 1 && stages[0].T <= 30) + terminalGuidance = true; + else + saneGuidance = false; } else { @@ -573,7 +695,7 @@ private void DriveGravityTurn(FlightCtrlState s) return; } - if (h >= hT) + if (stages[0].h >= h_burnout) { status = "Angular momentum target achieved"; core.thrust.targetThrottle = 0.0F; diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 6822d8954..d42dcf168 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -51,16 +51,18 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); GUILayout.Label("Stage Stats"); for(int i = path.stages.Count - 1; i >= 0; i--) { - GUILayout.Label(String.Format("{0:D}: {1:F1} {2:F1} {3:F1} {4:D}", i, path.stages[i].avail_dV, path.stages[i].v_e, path.stages[i].deltaTime, path.stages[i].kspStage)); + GUILayout.Label(String.Format("{0:D}: {1:D} {2:F1} {3:F1} {4:F1} {5:F1} {6:F1}", i, path.stages[i].kspStage, path.stages[i].v_e, path.stages[i].avail_dV, path.stages[i].avail_T, path.stages[i].dV, path.stages[i].T)); } GUILayout.Label("Burnout Stats"); - GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); - GUILayout.Label(String.Format("delta-V (guidance): {0:F1}", path.stages[0].dV)); - GUILayout.Label(String.Format("A: {0:F1}", path.stages[0].A)); - GUILayout.Label(String.Format("B: {0:F1}", path.stages[0].B)); - GUILayout.Label(String.Format("time: {0:F1}", path.stages[0].T)); - GUILayout.Label(String.Format("pitch: {0:F1}", path.guidancePitch)); - GUILayout.Label(String.Format("steps: {0:D}", path.convergenceSteps)); + if (path.stages.Count > 0) { + // GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); + GUILayout.Label(String.Format("delta-V (guidance): {0:F1}", path.total_dV)); + GUILayout.Label(String.Format("A: {0:F4}", path.stages[0].A)); + GUILayout.Label(String.Format("B: {0:F4}", path.stages[0].B)); + GUILayout.Label(String.Format("time: {0:F1}", path.total_T)); + GUILayout.Label(String.Format("pitch: {0:F1}", path.guidancePitch)); + GUILayout.Label(String.Format("steps: {0:D}", path.convergenceSteps)); + } if (path.guidanceEnabled) { @@ -69,8 +71,10 @@ protected override void WindowGUI(int windowID) } else { - if (GUILayout.Button("Enable PEG Guidance")) + if (GUILayout.Button("Enable PEG Guidance")) { path.guidanceEnabled = true; + path.terminalGuidance = false; + } } GuiUtils.SimpleTextBox("Emergency pitch adj.:", path.pitchBias, "°"); From 1df683e949b8a21463f42f25251af95074252c53 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 5 Jul 2017 18:29:53 -0700 Subject: [PATCH 25/51] fix some more niggles, still not working upper stage single-PEG still works fine, dV values are very close Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 46 +++++++++++++++++++++--------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 3dc841e9e..87a0f3efc 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -140,17 +140,27 @@ public override string ToString() { return "A = " + A + "\n" + "B = " + B + "\n" + + "dA = " + dA + "\n" + + "dB = " + dB + "\n" + "G = " + G + "\n" + + "GT = " + GT + "\n" + "T = " + T + "\n" + + "dV = " + dV + "\n" + "a0 = " + a0 + "\n" + + "aT = " + aT + "\n" + "v_e = " + v_e + "\n" + "tau = " + tau + "\n" + "r = " + r + "\n" + - "rd = " + rd + "\n" + - "h = " + h + "\n" + "dr = " + dr + "\n" + + "rT = " + rT + "\n" + + "rd = " + rd + "\n" + "drd = " + drd + "\n" + - "dh = " + dh + "\n"; + "rdT = " + rdT + "\n" + + "h = " + h + "\n" + + "dh = " + dh + "\n" + + "hT = " + hT + "\n" + + "f_r = " + f_r + "\n" + + "f_rT = " + f_rT + "\n"; } } @@ -443,11 +453,14 @@ private void peg_estimate(int snum) if (upper) { - double dh = stage.dh = 2 * ( h_burnout - h ) / ( r_burnout + r ); + double dh = stage.dh = h_burnout - h; + double rbar = (r_burnout + r ) / 2.0; + hT = stage.hT = h + dh; /* updated estimate of dV to burn */ - stage.dV = ( dh + v_e * T * ( fd_th + fdd_th * ( tau + T / 2.0 ) ) ) / ( ( fdd_th * tau + fd_th ) * tau + f_th ); + //stage.dV = ( dh / rbar + v_e * T * ( fd_th + fdd_th * ( tau + T / 2.0 ) ) ) / ( ( fdd_th * tau + fd_th ) * tau + f_th ); + stage.dV = ( dh / rbar + v_e * T * ( fd_th + fdd_th * tau ) + ( fdd_th * v_e * T * T ) / 2.0 ) / ( f_th + fd_th * tau + fdd_th * tau * tau ); /* updated estimate of T */ T = stage.T = tau * ( 1 - Math.Exp( - stage.dV / v_e ) ); @@ -458,16 +471,19 @@ private void peg_estimate(int snum) total_T += stages[i].T; } + // FIXME: move from peg_estimate upper phase to peg_solve, since these are drd/dr values over the whole trajectory double drd = stage.drd = rd_burnout - stages[0].rd - b(0,1) * stage.dA - b(1, 1) * stage.dB; double dr = stage.dr = r_burnout - stages[0].r - stages[0].rd * total_T - c(0,1) * stage.dA - c(1, 1) * stage.dB; - rT = stage.rT = r + dr; - rdT = stage.rdT = rd + drd; + rT = stage.rT; + rdT = stage.rdT; } else { /* boosters */ double dr = stage.dr = rd * T + c(0,0) * A + c(1, 0) * B; double drd = stage.drd = b(0,0) * A + b(1, 0) * B; rT = stage.rT = r + dr; rdT = stage.rdT = rd + drd; + Debug.Log("f_th: " + f_th + " fd_th: " + fd_th + " fdd_th: " + fdd_th); + Debug.Log("b(0,0): " + b(0,0) + " b(1,0): " + b(1,0) + " b(2,0): " + b(2,0)); double dh = stage.dh = ( r + rT ) / 2.0 * ( f_th * b(0, 0) + fd_th * b(1, 0) + fdd_th * b(2, 0)); hT = stage.hT = h + dh; } @@ -475,7 +491,7 @@ private void peg_estimate(int snum) double aT = stage.aT = a0 / ( 1.0D - T / tau ); double GT = stage.GT = ( GM - hT * hT / rT ) / ( rT * rT ); - double f_rT = A + B * T + GT / aT; + double f_rT = stage.f_rT = A + B * T + GT / aT; /* set next stages initial conditions */ if (!upper) { @@ -485,7 +501,7 @@ private void peg_estimate(int snum) stages[snum+1].r = rT; stages[snum+1].rd = rdT; stages[snum+1].h = hT; - stages[snum+1].A = A + dA; + stages[snum+1].A = A + B * T + dA; stages[snum+1].B = B + dB; stages[snum+1].dA = dA; stages[snum+1].dB = dB; @@ -596,13 +612,15 @@ private void converge(double dt, bool initialize = false) if (!stable || bad_guidance(stages[0]) || bad_dV() || bad_pitch()) { - stages[0].A = Astart; - stages[0].B = Bstart; - stages[num_stages-1].T = Tstart; - - /* if we're within 30 secs of burnout and we just lost guidance, switch to terminal guidance */ + /* if we're within 30 secs of burnout and we just lost guidance, restore A,B,T and switch to terminal guidance */ if (saneGuidance && num_stages == 1 && stages[0].T <= 30) + { + + stages[0].A = Astart; + stages[0].B = Bstart; + stages[num_stages-1].T = Tstart; terminalGuidance = true; + } else saneGuidance = false; } From 2ab41cb5e85e19cf4678ad561ffa828b513acf0b Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 5 Jul 2017 19:21:01 -0700 Subject: [PATCH 26/51] iterating properly on the lower stage Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 87a0f3efc..de767a692 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -549,10 +549,10 @@ private void converge(double dt, bool initialize = false) terminalGuidance = true; if (!terminalGuidance) { - if (initialize || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance ) + if (initialize || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance || mode != AscentMode.GRAVITY_TURN ) { stages[num_stages-1].T = stages[num_stages-1].avail_T; - stages[0].A = -0.9; + stages[0].A = Math.Sin(srfvelPitch()) - stages[0].G / stages[0].a0; stages[0].B = 0.0036; fd_r = -0.001; dt = 0.0; @@ -576,7 +576,7 @@ private void converge(double dt, bool initialize = false) { Debug.Log("=========== START ================"); for(convergenceSteps = 1; convergenceSteps <= 50; convergenceSteps++) { - double oldT = stages[0].T; + double oldT = stages[num_stages-1].T; for(int i = 0; i < num_stages; i++) peg_estimate(i); @@ -600,8 +600,8 @@ private void converge(double dt, bool initialize = false) Debug.Log(stages[1]); } - Debug.Log(" deltaT = " + (stages[0].T - oldT)); - if ( Math.Abs(stages[0].T - oldT) < 0.1 ) { + Debug.Log(" deltaT = " + (stages[num_stages-1].T - oldT)); + if ( Math.Abs(stages[num_stages-1].T - oldT) < 0.1 ) { stable = true; break; } From 3bb1f2845f1d895dff8510a71f5ee28cf31872cf Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 5 Jul 2017 21:43:28 -0700 Subject: [PATCH 27/51] stable 2-stage PEG Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index de767a692..ead61ff98 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -319,6 +319,9 @@ private double c(int n, int snum) private double alpha(int snum) { + if (snum == 1) + return b(0, 0) + b(0, 1); + double sum = 0; for(int l = 0; l <= snum; l++) sum += b(0, l); @@ -327,6 +330,9 @@ private double alpha(int snum) private double beta(int snum) { + if (snum == 1) + return b(1, 0) + b(1, 1) + b(0, 1) * stages[0].T; + double sum = 0; for(int l = 0; l <= snum; l++) { @@ -343,6 +349,9 @@ private double beta(int snum) private double gamma(int snum) { + if (snum == 1) + return c(0, 0) + c(0, 1) + b(0, 0) * stages[1].T; + double sum = 0; for(int l = 0; l <= snum; l++) { @@ -358,6 +367,9 @@ private double gamma(int snum) private double delta(int snum) { + if (snum == 1) + return c(1, 0) + c(1,1) + b(1,0) * stages[1].T + c(0,1) * stages[0].T; + double sum = 0; for(int l = 0; l <= snum; l++) { @@ -379,6 +391,8 @@ private void peg_solve(int snum) double dr = stages[snum].dr; double drd = stages[snum].drd; + Debug.Log("peg_solve: dr = " + dr + " drd = " + drd); + double a = alpha(snum); double b = beta(snum); double g = gamma(snum); @@ -441,7 +455,7 @@ private void peg_estimate(int snum) // double fd_r = ( f_rT - f_r ) / T; /* cos pitch */ - double f_th = 1.0D - stage.f_r * stage.f_r / 2.0D; + double f_th = Math.Sqrt(1.0D - stage.f_r * stage.f_r); /* cos pitch rate */ double fd_th = - stage.f_r * fd_r; /* cos pitch accel */ @@ -589,12 +603,12 @@ private void converge(double dt, bool initialize = false) // Debug.Log(stages[0]); //} peg_solve(num_stages - 1); - if (convergenceSteps == 1) + //if (convergenceSteps == 1) { Debug.Log("BOOSTER:"); Debug.Log(stages[0]); } - if (convergenceSteps == 1) + //if (convergenceSteps == 1) { Debug.Log("UPPER:"); Debug.Log(stages[1]); From 3cf6f6fe76b1493bafbd314e9f50e1eb18694976 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 5 Jul 2017 22:42:12 -0700 Subject: [PATCH 28/51] comment all the Debug.Log spam Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index ead61ff98..dd2c098f5 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -391,7 +391,7 @@ private void peg_solve(int snum) double dr = stages[snum].dr; double drd = stages[snum].drd; - Debug.Log("peg_solve: dr = " + dr + " drd = " + drd); + //Debug.Log("peg_solve: dr = " + dr + " drd = " + drd); double a = alpha(snum); double b = beta(snum); @@ -496,8 +496,8 @@ private void peg_estimate(int snum) double drd = stage.drd = b(0,0) * A + b(1, 0) * B; rT = stage.rT = r + dr; rdT = stage.rdT = rd + drd; - Debug.Log("f_th: " + f_th + " fd_th: " + fd_th + " fdd_th: " + fdd_th); - Debug.Log("b(0,0): " + b(0,0) + " b(1,0): " + b(1,0) + " b(2,0): " + b(2,0)); + //Debug.Log("f_th: " + f_th + " fd_th: " + fd_th + " fdd_th: " + fdd_th); + //Debug.Log("b(0,0): " + b(0,0) + " b(1,0): " + b(1,0) + " b(2,0): " + b(2,0)); double dh = stage.dh = ( r + rT ) / 2.0 * ( f_th * b(0, 0) + fd_th * b(1, 0) + fdd_th * b(2, 0)); hT = stage.hT = h + dh; } @@ -588,7 +588,7 @@ private void converge(double dt, bool initialize = false) } else { - Debug.Log("=========== START ================"); + //Debug.Log("=========== START ================"); for(convergenceSteps = 1; convergenceSteps <= 50; convergenceSteps++) { double oldT = stages[num_stages-1].T; @@ -604,17 +604,17 @@ private void converge(double dt, bool initialize = false) //} peg_solve(num_stages - 1); //if (convergenceSteps == 1) - { - Debug.Log("BOOSTER:"); - Debug.Log(stages[0]); - } + //{ + //Debug.Log("BOOSTER:"); + //Debug.Log(stages[0]); + //} //if (convergenceSteps == 1) - { - Debug.Log("UPPER:"); - Debug.Log(stages[1]); - } + //{ + //Debug.Log("UPPER:"); + //Debug.Log(stages[1]); + //} - Debug.Log(" deltaT = " + (stages[num_stages-1].T - oldT)); + //Debug.Log(" deltaT = " + (stages[num_stages-1].T - oldT)); if ( Math.Abs(stages[num_stages-1].T - oldT) < 0.1 ) { stable = true; break; From 70256e5c62fe9df92292915a875c211443ae4eef Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 00:18:09 -0700 Subject: [PATCH 29/51] minimum viable nathan-proofing Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 9 ++++++--- MechJeb2/MechJebModuleAscentPEGMenu.cs | 1 + 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index dd2c098f5..c1afaf312 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -25,6 +25,8 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase public EditableDoubleMult terminalGuidanceSecs = new EditableDoubleMult(10); [Persistent(pass = (int)(Pass.Type | Pass.Global))] public EditableDoubleMult stageLowDVLimit = new EditableDoubleMult(20); + [Persistent(pass = (int)(Pass.Type | Pass.Global))] + public EditableInt num_stages = new EditableInt(2); /* this deliberately does not persist, it is for emergencies only */ public EditableDoubleMult pitchBias = new EditableDoubleMult(0); @@ -264,7 +266,10 @@ void UpdateStageStats() for ( int i = stages.Count - 1; i >= 0; i-- ) { if ( stages[i].kspStage < 0 ) { - num_stages--; /* FIXME this needs to be way smarter */ + /* if someone runs a booster program though the full boster we don't consume a PEG stage */ + /* also if PEG is disabled manually we don't consume stages */ + if ( mode == AscentMode.GRAVITY_TURN && guidanceEnabled ) + num_stages = Math.Max(1, num_stages - 1); stages.RemoveAt(i); } } @@ -424,8 +429,6 @@ private void peg_update(double dt) } } - int num_stages = 2; - private void peg_estimate(int snum) { StageInfo stage = stages[snum]; diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index d42dcf168..50409648a 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -49,6 +49,7 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Booster Pitch end:", path.pitchEndTime, "s"); GUILayout.Label(String.Format("ending pitch: {0:F1}°", 90.0 - (path.pitchEndTime - path.pitchStartTime)*path.pitchRate)); GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); + GuiUtils.SimpleTextBox("Num Stages:", path.num_stages); GUILayout.Label("Stage Stats"); for(int i = path.stages.Count - 1; i >= 0; i--) { GUILayout.Label(String.Format("{0:D}: {1:D} {2:F1} {3:F1} {4:F1} {5:F1} {6:F1}", i, path.stages[i].kspStage, path.stages[i].v_e, path.stages[i].avail_dV, path.stages[i].avail_T, path.stages[i].dV, path.stages[i].T)); From 39ddee3002ee1f159be065d11e65f2ceac943014 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 11:54:43 -0700 Subject: [PATCH 30/51] fix bug in gamma and clamp sqrt the s/sum/sum2/ in the gamma function is the one that was trolling for literally days. Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index c1afaf312..19039f54a 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -324,20 +324,15 @@ private double c(int n, int snum) private double alpha(int snum) { - if (snum == 1) - return b(0, 0) + b(0, 1); - double sum = 0; for(int l = 0; l <= snum; l++) sum += b(0, l); + return sum; } private double beta(int snum) { - if (snum == 1) - return b(1, 0) + b(1, 1) + b(0, 1) * stages[0].T; - double sum = 0; for(int l = 0; l <= snum; l++) { @@ -349,32 +344,28 @@ private double beta(int snum) sum += b(1, l) + b(0, l) * sum2; } + return sum; } private double gamma(int snum) { - if (snum == 1) - return c(0, 0) + c(0, 1) + b(0, 0) * stages[1].T; - double sum = 0; for(int l = 0; l <= snum; l++) { double sum2 = 0; for(int k = 0; k <= (l-1); k++) { - sum += b(0, k); + sum2 += b(0, k); } sum += c(0, l) + stages[l].T * sum2; } + return sum; } private double delta(int snum) { - if (snum == 1) - return c(1, 0) + c(1,1) + b(1,0) * stages[1].T + c(0,1) * stages[0].T; - double sum = 0; for(int l = 0; l <= snum; l++) { @@ -388,6 +379,7 @@ private double delta(int snum) } sum += c(1, l) + sum2; } + return sum; } @@ -458,7 +450,7 @@ private void peg_estimate(int snum) // double fd_r = ( f_rT - f_r ) / T; /* cos pitch */ - double f_th = Math.Sqrt(1.0D - stage.f_r * stage.f_r); + double f_th = Math.Sqrt(MuUtils.Clamp(1.0D - stage.f_r * stage.f_r, 0.0, 1.0)); /* cos pitch rate */ double fd_th = - stage.f_r * fd_r; /* cos pitch accel */ From 468b9673f1c25e05f02e4dff4a38d9573417ac58 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 16:08:38 -0700 Subject: [PATCH 31/51] push the dr + drd into the peg_solve routine Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 38 ++++++++++-------------------- 1 file changed, 13 insertions(+), 25 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 19039f54a..ecd4e7eb4 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -52,11 +52,6 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; /* guidancePitchAngle -- output from the guidance algorithm, not 'manual' pitch */ public double guidancePitch { get { return Math.Asin(stages[0].A + stages[0].G / stages[0].a0 ) * UtilMath.Rad2Deg; } } - /* dV to add */ - /* public double dV { get; private set; } */ - /* dV estimated from difference in specific orbital energy*/ - // public double dVest { get; private set; } - public bool guidanceEnabled = true; public int convergenceSteps { get; private set; } @@ -322,7 +317,7 @@ private double c(int n, int snum) return c(n-1, snum) * stage.tau - stage.v_e * Math.Pow(stage.T, n+1) / ( n * (n + 1 ) ); } - private double alpha(int snum) + private double Alpha(int snum) { double sum = 0; for(int l = 0; l <= snum; l++) @@ -331,7 +326,7 @@ private double alpha(int snum) return sum; } - private double beta(int snum) + private double Beta(int snum) { double sum = 0; for(int l = 0; l <= snum; l++) @@ -348,7 +343,7 @@ private double beta(int snum) return sum; } - private double gamma(int snum) + private double Gamma(int snum) { double sum = 0; for(int l = 0; l <= snum; l++) @@ -364,7 +359,7 @@ private double gamma(int snum) return sum; } - private double delta(int snum) + private double Delta(int snum) { double sum = 0; for(int l = 0; l <= snum; l++) @@ -385,22 +380,18 @@ private double delta(int snum) private void peg_solve(int snum) { - double dr = stages[snum].dr; - double drd = stages[snum].drd; - - //Debug.Log("peg_solve: dr = " + dr + " drd = " + drd); - - double a = alpha(snum); - double b = beta(snum); - double g = gamma(snum); - double d = delta(snum); + double drd = rd_burnout - stages[0].rd - b(0,1) * stages[1].dA - b(1,1) * stages[1].dB; + double dr = r_burnout - stages[0].r - stages[0].rd * total_T - c(0,1) * stages[1].dA - c(1, 1) * stages[1].dB; - double D = a * d - b * g; + double alpha = Alpha(snum); + double beta = Beta(snum); + double gamma = Gamma(snum); + double delta = Delta(snum); - //Debug.Log(snum + " a:" + a + " b:" + b + " g:" + g + " d:" + d + " D:" + D); + double D = alpha * delta - beta * gamma; - stages[0].A = ( d * drd - b * dr ) / D; - stages[0].B = ( a * dr - g * drd ) / D; + stages[0].A = ( delta * drd - beta * dr ) / D; + stages[0].B = ( alpha * dr - gamma * drd ) / D; } private void peg_update(double dt) @@ -480,9 +471,6 @@ private void peg_estimate(int snum) total_T += stages[i].T; } - // FIXME: move from peg_estimate upper phase to peg_solve, since these are drd/dr values over the whole trajectory - double drd = stage.drd = rd_burnout - stages[0].rd - b(0,1) * stage.dA - b(1, 1) * stage.dB; - double dr = stage.dr = r_burnout - stages[0].r - stages[0].rd * total_T - c(0,1) * stage.dA - c(1, 1) * stage.dB; rT = stage.rT; rdT = stage.rdT; From e9bfa505b14434e798a66063b3cdd17f367c5f63 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 17:02:36 -0700 Subject: [PATCH 32/51] revert the Sqrt clamp due to terminal guidance wiggles - its actually useful to let that expression explode - allow guidance to converge during booster phase (reduces grinding) - abort guidance cycles as soon as we get NaN or other bad values (reduces grinding) Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 32 ++++++++---------------------- 1 file changed, 8 insertions(+), 24 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index ecd4e7eb4..e330d4c89 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -441,7 +441,8 @@ private void peg_estimate(int snum) // double fd_r = ( f_rT - f_r ) / T; /* cos pitch */ - double f_th = Math.Sqrt(MuUtils.Clamp(1.0D - stage.f_r * stage.f_r, 0.0, 1.0)); + // note that when this Sqrt goes NaN that is a useful condition to terminate guidance, clamping it will increase terminal guidance wiggles + double f_th = Math.Sqrt(1.0D - stage.f_r * stage.f_r); /* cos pitch rate */ double fd_th = - stage.f_r * fd_r; /* cos pitch accel */ @@ -546,7 +547,7 @@ private void converge(double dt, bool initialize = false) terminalGuidance = true; if (!terminalGuidance) { - if (initialize || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance || mode != AscentMode.GRAVITY_TURN ) + if (initialize || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance ) { stages[num_stages-1].T = stages[num_stages-1].avail_T; stages[0].A = Math.Sin(srfvelPitch()) - stages[0].G / stages[0].a0; @@ -560,9 +561,6 @@ private void converge(double dt, bool initialize = false) peg_update(dt); - //Debug.Log("ONE:"); - //Debug.Log(stages[0]); - if (terminalGuidance) { peg_estimate(0); @@ -571,7 +569,6 @@ private void converge(double dt, bool initialize = false) } else { - //Debug.Log("=========== START ================"); for(convergenceSteps = 1; convergenceSteps <= 50; convergenceSteps++) { double oldT = stages[num_stages-1].T; @@ -580,29 +577,16 @@ private void converge(double dt, bool initialize = false) peg_final(); - //if (convergenceSteps == 1) - //{ - // Debug.Log("TWO:"); - // Debug.Log(stages[0]); - //} peg_solve(num_stages - 1); - //if (convergenceSteps == 1) - //{ - //Debug.Log("BOOSTER:"); - //Debug.Log(stages[0]); - //} - //if (convergenceSteps == 1) - //{ - //Debug.Log("UPPER:"); - //Debug.Log(stages[1]); - //} - - //Debug.Log(" deltaT = " + (stages[num_stages-1].T - oldT)); + if ( Math.Abs(stages[num_stages-1].T - oldT) < 0.1 ) { stable = true; break; } - /* FIXME: consider breaking out on bad_guidance() here */ + + // abort cycles if we hit NaN values or something insane + if (bad_guidance(stages[0])) + break; } terminalGuidance = false; } From 9763b781b0151c2021c8c871c7b8c70a2c6ac915 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 17:22:42 -0700 Subject: [PATCH 33/51] skip prelaunch if vessel is already in flight Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentAutopilot.cs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/MechJeb2/MechJebModuleAscentAutopilot.cs b/MechJeb2/MechJebModuleAscentAutopilot.cs index bcbe17996..df02d6b01 100644 --- a/MechJeb2/MechJebModuleAscentAutopilot.cs +++ b/MechJeb2/MechJebModuleAscentAutopilot.cs @@ -180,6 +180,12 @@ void DriveSolarPanels(FlightCtrlState s) void DrivePrelaunch(FlightCtrlState s) { + if (vessel.LiftedOff() && !vessel.Landed) { + status = "Vessel is not landed, skipping pre-launch"; + mode = AscentMode.ASCEND; + return; + } + if (autoThrottle) { Debug.Log("prelaunch killing throttle"); core.thrust.targetThrottle = 0.0f; From 0c5f1dfba283d2de83bf6c814a52b027c0a914ed Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 17:59:55 -0700 Subject: [PATCH 34/51] Math.Pow with small int exponents is eeeevil Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 4 ++-- MechJeb2/MuUtils.cs | 7 +++++++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index e330d4c89..f5c335c61 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -304,7 +304,7 @@ private double b(int n, int snum) if (n == 0) return stage.dV; - return b(n-1, snum) * stage.tau - stage.v_e * Math.Pow(stage.T, n) / n; + return b(n-1, snum) * stage.tau - stage.v_e * MuUtils.IntPow(stage.T, n) / n; } /* FIXME: some memoization */ @@ -314,7 +314,7 @@ private double c(int n, int snum) if (n == 0) return b(0, snum) * stage.T - b(1, snum); - return c(n-1, snum) * stage.tau - stage.v_e * Math.Pow(stage.T, n+1) / ( n * (n + 1 ) ); + return c(n-1, snum) * stage.tau - stage.v_e * MuUtils.IntPow(stage.T, n+1) / ( n * (n + 1 ) ); } private double Alpha(int snum) diff --git a/MechJeb2/MuUtils.cs b/MechJeb2/MuUtils.cs index 2acc51069..493bd61f9 100644 --- a/MechJeb2/MuUtils.cs +++ b/MechJeb2/MuUtils.cs @@ -116,6 +116,13 @@ public static double ClampRadiansPi(double angle) return angle; } + public static double IntPow(double val, int exp) { + double result = val; + for(int i=1;i Date: Thu, 6 Jul 2017 18:16:56 -0700 Subject: [PATCH 35/51] add GUI tools for stage dv cutoff and re-scanning Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 2 +- MechJeb2/MechJebModuleAscentPEGMenu.cs | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index f5c335c61..1a018877c 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -176,7 +176,7 @@ void UpdateStageFromMechJeb(StageInfo stage, bool atmo = false) public List skippedParts = new List(); - void InitStageStats() + public void InitStageStats() { Debug.Log("MechJebModuleAscentPEG.InitStageStats()"); stages.Clear(); diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 50409648a..8c494dd62 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -51,6 +51,10 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); GuiUtils.SimpleTextBox("Num Stages:", path.num_stages); GUILayout.Label("Stage Stats"); + if (GUILayout.Button("Reinitialize Stage Analysis")) + path.InitStageStats(); + GuiUtils.SimpleTextBox("Stage Minimum dV Limit:", path.stageLowDVLimit, "m/s"); + for(int i = path.stages.Count - 1; i >= 0; i--) { GUILayout.Label(String.Format("{0:D}: {1:D} {2:F1} {3:F1} {4:F1} {5:F1} {6:F1}", i, path.stages[i].kspStage, path.stages[i].v_e, path.stages[i].avail_dV, path.stages[i].avail_T, path.stages[i].dV, path.stages[i].T)); } From 2ccd4f9f62898f651c9821639e545a906e615588 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 18:45:22 -0700 Subject: [PATCH 36/51] add some caching state to be used later also converts num_stages back to a plain-old-int Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 36 ++++++++++++++++++++++++-- MechJeb2/MechJebModuleAscentPEGMenu.cs | 2 +- 2 files changed, 35 insertions(+), 3 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 1a018877c..5f17cbac6 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -26,7 +26,8 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase [Persistent(pass = (int)(Pass.Type | Pass.Global))] public EditableDoubleMult stageLowDVLimit = new EditableDoubleMult(20); [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableInt num_stages = new EditableInt(2); + public EditableInt edit_num_stages = new EditableInt(2); + private int num_stages; /* this deliberately does not persist, it is for emergencies only */ public EditableDoubleMult pitchBias = new EditableDoubleMult(0); @@ -37,6 +38,7 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase public override void OnModuleEnabled() { + SetNumStages(edit_num_stages); mode = AscentMode.VERTICAL_ASCENT; InitStageStats(); } @@ -88,6 +90,29 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; public List stages = new List(); + private void SetNumStages(int n) + { + edit_num_stages = n; + if (num_stages != n) + { + num_stages = n; + InitConstantCache(); + } + } + + private void InitConstantCache() + { + for(int i = 0; i < num_stages; i++) { + stages[i].b = new double[num_stages+1]; + stages[i].c = new double[num_stages+1]; + stages[i].b_dirty = new bool[num_stages+1]; + stages[i].c_dirty = new bool[num_stages+1]; + for(int j = 0; i <= num_stages; j++) { + stages[i].b_dirty[j] = stages[i].c_dirty[j] = true; + } + } + } + public class StageInfo { /* updated directly from vehicle stats */ @@ -100,6 +125,12 @@ public class StageInfo public double dV; /* lower stages are == avail_dV, upper stage needs estimation */ public double T; /* lower stages are == avail_T, upper stage needs estimation */ + /* constant cache */ + public double[] b; + public double[] c; + public bool[] b_dirty; + public bool[] c_dirty; + /* steering constants */ public double A; public double B; @@ -264,7 +295,7 @@ void UpdateStageStats() /* if someone runs a booster program though the full boster we don't consume a PEG stage */ /* also if PEG is disabled manually we don't consume stages */ if ( mode == AscentMode.GRAVITY_TURN && guidanceEnabled ) - num_stages = Math.Max(1, num_stages - 1); + SetNumStages( Math.Max(1, num_stages - 1) ); stages.RemoveAt(i); } } @@ -617,6 +648,7 @@ public override bool DriveAscent(FlightCtrlState s) { stats.RequestUpdate(this); stats.liveSLT = true; /* yes, this disables the button, yes, it is important we do this */ + SetNumStages(edit_num_stages); UpdateRocketStats(); if (last_time != 0.0D) diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 8c494dd62..17528a1f0 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -49,7 +49,7 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Booster Pitch end:", path.pitchEndTime, "s"); GUILayout.Label(String.Format("ending pitch: {0:F1}°", 90.0 - (path.pitchEndTime - path.pitchStartTime)*path.pitchRate)); GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); - GuiUtils.SimpleTextBox("Num Stages:", path.num_stages); + GuiUtils.SimpleTextBox("Num Stages:", path.edit_num_stages); GUILayout.Label("Stage Stats"); if (GUILayout.Button("Reinitialize Stage Analysis")) path.InitStageStats(); From f87cefe04287585fc4d64c6f13ae8ccc639b61b8 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 19:10:18 -0700 Subject: [PATCH 37/51] add memoization for b and c values Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 48 ++++++++++++++++++++++++------ 1 file changed, 39 insertions(+), 9 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 5f17cbac6..10ab1c3cd 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -100,6 +100,13 @@ private void SetNumStages(int n) } } + private void DirtyCacheForStage(int snum) + { + for(int i = 0; i <= num_stages; i++) { + stages[snum].b_dirty[i] = stages[snum].c_dirty[i] = true; + } + } + private void InitConstantCache() { for(int i = 0; i < num_stages; i++) { @@ -107,9 +114,7 @@ private void InitConstantCache() stages[i].c = new double[num_stages+1]; stages[i].b_dirty = new bool[num_stages+1]; stages[i].c_dirty = new bool[num_stages+1]; - for(int j = 0; i <= num_stages; j++) { - stages[i].b_dirty[j] = stages[i].c_dirty[j] = true; - } + DirtyCacheForStage(i); } } @@ -299,6 +304,8 @@ void UpdateStageStats() stages.RemoveAt(i); } } + // dirty the constant cache for the bottom stage because v_e and such is live and may be changing (even exoatmospheric due to Agathorn) + DirtyCacheForStage(0); } private double smaT() { @@ -328,24 +335,44 @@ void UpdateStageStats() stages[0].h = Vector3.Cross(mainBody.position, vessel.obt_velocity).magnitude; } - /* FIXME: some memoization */ private double b(int n, int snum) { StageInfo stage = stages[snum]; + + if (!stage.b_dirty[n]) + return stage.b[n]; + + double ret; + if (n == 0) - return stage.dV; + ret = stage.dV; + else + ret = b(n-1, snum) * stage.tau - stage.v_e * MuUtils.IntPow(stage.T, n) / n; + + stage.b[n] = ret; + stage.b_dirty[n] = false; - return b(n-1, snum) * stage.tau - stage.v_e * MuUtils.IntPow(stage.T, n) / n; + return ret; } - /* FIXME: some memoization */ private double c(int n, int snum) { StageInfo stage = stages[snum]; + + if (!stage.c_dirty[n]) + return stage.c[n]; + + double ret; + if (n == 0) - return b(0, snum) * stage.T - b(1, snum); + ret = b(0, snum) * stage.T - b(1, snum); + else + ret = c(n-1, snum) * stage.tau - stage.v_e * MuUtils.IntPow(stage.T, n+1) / ( n * (n + 1 ) ); - return c(n-1, snum) * stage.tau - stage.v_e * MuUtils.IntPow(stage.T, n+1) / ( n * (n + 1 ) ); + stage.c[n] = ret; + stage.c_dirty[n] = false; + + return ret; } private double Alpha(int snum) @@ -497,6 +524,9 @@ private void peg_estimate(int snum) /* updated estimate of T */ T = stage.T = tau * ( 1 - Math.Exp( - stage.dV / v_e ) ); + /* now that we have the updated T and dV values, dirty the upper stages constant cache */ + DirtyCacheForStage(snum); + total_T = 0.0; for(int i = 0; i < num_stages; i++) { From 601ab8ccd3fbd2c4682a886fe083b982997e6775 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 20:00:38 -0700 Subject: [PATCH 38/51] fix index bugs Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 10ab1c3cd..4274ed327 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -96,7 +96,6 @@ private void SetNumStages(int n) if (num_stages != n) { num_stages = n; - InitConstantCache(); } } @@ -109,7 +108,7 @@ private void DirtyCacheForStage(int snum) private void InitConstantCache() { - for(int i = 0; i < num_stages; i++) { + for(int i = 0; i < num_stages && i < stages.Count; i++) { stages[i].b = new double[num_stages+1]; stages[i].c = new double[num_stages+1]; stages[i].b_dirty = new bool[num_stages+1]; @@ -239,6 +238,7 @@ public void InitStageStats() skippedParts.Remove(stages[i].parts[j]); } } + InitConstantCache(); } bool PartsListsMatch(List one, List two) From 0ecece188330c5ee6d18d14912c66673fecb3ddb Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Thu, 6 Jul 2017 21:15:22 -0700 Subject: [PATCH 39/51] fix a few bugs back to stable 2-stage PEG after performance improvements Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 4274ed327..af8c62611 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -101,6 +101,9 @@ private void SetNumStages(int n) private void DirtyCacheForStage(int snum) { + if (snum >= stages.Count) + return; + for(int i = 0; i <= num_stages; i++) { stages[snum].b_dirty[i] = stages[snum].c_dirty[i] = true; } @@ -438,8 +441,19 @@ private double Delta(int snum) private void peg_solve(int snum) { - double drd = rd_burnout - stages[0].rd - b(0,1) * stages[1].dA - b(1,1) * stages[1].dB; - double dr = r_burnout - stages[0].r - stages[0].rd * total_T - c(0,1) * stages[1].dA - c(1, 1) * stages[1].dB; + double drd; + double dr; + + if (num_stages == 2) + { + drd = rd_burnout - stages[0].rd - b(0,1) * stages[1].dA - b(1,1) * stages[1].dB; + dr = r_burnout - stages[0].r - stages[0].rd * total_T - c(0,1) * stages[1].dA - c(1, 1) * stages[1].dB; + } + else + { + drd = rd_burnout - stages[0].rd; + dr = r_burnout - stages[0].r - stages[0].rd * total_T; + } double alpha = Alpha(snum); double beta = Beta(snum); From 74aa299f2d5d034f30b89b2c086d15846ef62203 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Fri, 7 Jul 2017 15:49:13 -0700 Subject: [PATCH 40/51] stable 3-stage PEG most likely this is stable N-stage PEG, there's only a few cracks where 4+ stage PEG bugs could be hiding really... Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 89 +++++++++++++++++--------- MechJeb2/MechJebModuleAscentPEGMenu.cs | 2 +- 2 files changed, 58 insertions(+), 33 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index af8c62611..84aaf7434 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -199,7 +199,7 @@ public override string ToString() } } - void UpdateStageFromMechJeb(StageInfo stage, bool atmo = false) + void UpdateStageFromMechJeb(StageInfo stage, bool atmo) { /* stage.kspStage must be corrected before calling this */ int s = stage.kspStage; @@ -294,7 +294,7 @@ void UpdateStageStats() if ( stage.kspStage >= 0 ) { - UpdateStageFromMechJeb(stage); + UpdateStageFromMechJeb(stage, i == 0); } } for ( int i = stages.Count - 1; i >= 0; i-- ) @@ -439,21 +439,48 @@ private double Delta(int snum) return sum; } - private void peg_solve(int snum) + private double Drd(int snum) { - double drd; - double dr; - - if (num_stages == 2) - { - drd = rd_burnout - stages[0].rd - b(0,1) * stages[1].dA - b(1,1) * stages[1].dB; - dr = r_burnout - stages[0].r - stages[0].rd * total_T - c(0,1) * stages[1].dA - c(1, 1) * stages[1].dB; + double sum = 0; + for(int l = 0; l <= snum; l++) { + double sum2 = 0; + for(int k = 0; k <= (l-1); k++) { + double sum3 = 0; + for(int i = 0; i <= (k-1); i++) { + sum3 += stages[i].dB; + } + sum2 += b(0, l) * stages[k].dA + b(0, l) * stages[k].T * sum3 + b(1, l) * stages[k].dB; + } + sum += sum2; } - else - { - drd = rd_burnout - stages[0].rd; - dr = r_burnout - stages[0].r - stages[0].rd * total_T; + return rd_burnout - stages[0].rd - sum; + } + + private double Dr(int snum) + { + double sum = 0; + for(int l = 0; l <= snum; l++) { + double sum2 = 0; + for(int k = 0; k<= (l-1); k++) { + double sum3 = 0; + for(int i = 0; i <= (k-1); i++) { + double sum4 = 0; + for(int m = 0; m <= (i-1); m++) { + sum4 += stages[m].dB; + } + sum3 += b(0, k) * stages[l].T * stages[i].dA + b(0,k) * stages[i].T * stages[l].T * sum4 + b(1,k) * stages[l].T * stages[i].dB + c(0,l) * stages[k].T * stages[i].dB; + } + sum2 += c(0,l) * stages[k].dA + c(1,l) * stages[k].dB + sum3; + } + sum += sum2; } + return r_burnout - stages[0].r - stages[0].rd * total_T - sum; + } + + private void peg_solve(int snum) + { + double drd = Drd(snum); + double dr = Dr(snum); double alpha = Alpha(snum); double beta = Beta(snum); @@ -469,10 +496,8 @@ private void peg_solve(int snum) private void peg_update(double dt) { stages[0].A = stages[0].A + stages[0].B * dt; - if (num_stages == 0) { - /* if we only have one stage, count down the estimate */ - stages[0].T -= dt; - } + stages[num_stages - 1].T -= dt; + for (int i = 0; i < num_stages - 1 ; i++) { /* all the lower stages are assumed to burn completely */ stages[i].dV = stages[i].avail_dV; @@ -541,23 +566,22 @@ private void peg_estimate(int snum) /* now that we have the updated T and dV values, dirty the upper stages constant cache */ DirtyCacheForStage(snum); - total_T = 0.0; - for(int i = 0; i < num_stages; i++) - { - total_T += stages[i].T; - } - + total_T = 0.0; + for(int i = 0; i < num_stages; i++) + { + total_T += stages[i].T; + } rT = stage.rT; rdT = stage.rdT; } else { /* boosters */ - double dr = stage.dr = rd * T + c(0,0) * A + c(1, 0) * B; - double drd = stage.drd = b(0,0) * A + b(1, 0) * B; + double dr = stage.dr = rd * T + c(0,snum) * A + c(1, snum) * B; + double drd = stage.drd = b(0,snum) * A + b(1, snum) * B; rT = stage.rT = r + dr; rdT = stage.rdT = rd + drd; //Debug.Log("f_th: " + f_th + " fd_th: " + fd_th + " fdd_th: " + fdd_th); //Debug.Log("b(0,0): " + b(0,0) + " b(1,0): " + b(1,0) + " b(2,0): " + b(2,0)); - double dh = stage.dh = ( r + rT ) / 2.0 * ( f_th * b(0, 0) + fd_th * b(1, 0) + fdd_th * b(2, 0)); + double dh = stage.dh = ( r + rT ) / 2.0 * ( f_th * b(0, snum) + fd_th * b(1, snum) + fdd_th * b(2, snum) ); hT = stage.hT = h + dh; } @@ -568,16 +592,13 @@ private void peg_estimate(int snum) /* set next stages initial conditions */ if (!upper) { - double dA = GT * ( 1.0 / aT - 1.0 / stages[snum+1].a0 ); - double dB = GT * ( 1.0 / stages[snum+1].v_e - 1.0 / v_e ) + ( 3 * hT * hT / rT - 2 * GM ) * rdT / ( rT * rT * rT ) * ( 1.0 / aT - 1.0 / stages[snum+1].a0 ); - + double dA = stage.dA = GT * ( 1.0 / aT - 1.0 / stages[snum+1].a0 ); + double dB = stage.dB = GT * ( 1.0 / stages[snum+1].v_e - 1.0 / v_e ) + ( 3 * hT * hT / rT - 2 * GM ) * rdT / ( rT * rT * rT ) * ( 1.0 / aT - 1.0 / stages[snum+1].a0 ); stages[snum+1].r = rT; stages[snum+1].rd = rdT; stages[snum+1].h = hT; stages[snum+1].A = A + B * T + dA; stages[snum+1].B = B + dB; - stages[snum+1].dA = dA; - stages[snum+1].dB = dB; } } @@ -663,6 +684,10 @@ private void converge(double dt, bool initialize = false) if (bad_guidance(stages[0])) break; } + + for(int i = 0; i < num_stages; i++) + Debug.Log(stages[i]); + terminalGuidance = false; } diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 17528a1f0..ab57bac57 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -56,7 +56,7 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Stage Minimum dV Limit:", path.stageLowDVLimit, "m/s"); for(int i = path.stages.Count - 1; i >= 0; i--) { - GUILayout.Label(String.Format("{0:D}: {1:D} {2:F1} {3:F1} {4:F1} {5:F1} {6:F1}", i, path.stages[i].kspStage, path.stages[i].v_e, path.stages[i].avail_dV, path.stages[i].avail_T, path.stages[i].dV, path.stages[i].T)); + GUILayout.Label(String.Format("{0:D}: {1:D} {2:F1} {3:F1} {4:F1} {5:F1} ({6:F1})", i, path.stages[i].kspStage, path.stages[i].avail_T, path.stages[i].avail_dV, path.stages[i].T, path.stages[i].dV, path.stages[i].avail_dV - path.stages[i].dV)); } GUILayout.Label("Burnout Stats"); if (path.stages.Count > 0) { From 6c75470b92db3f2878c3591b61b2d989d0c2f7e6 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Fri, 7 Jul 2017 15:53:08 -0700 Subject: [PATCH 41/51] remove Debug.Log spam Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 3 --- 1 file changed, 3 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 84aaf7434..ae63ab22a 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -685,9 +685,6 @@ private void converge(double dt, bool initialize = false) break; } - for(int i = 0; i < num_stages; i++) - Debug.Log(stages[i]); - terminalGuidance = false; } From 20984fdbb56679b5500e07436e94459b0c997fe0 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Mon, 10 Jul 2017 10:18:45 -0700 Subject: [PATCH 42/51] better specify where things persist Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index ae63ab22a..8f5e5928b 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -13,19 +13,19 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase public MechJebModuleAscentPEG(MechJebCore core) : base(core) { } /* default pitch program here works decently at SLT of about 1.4 */ - [Persistent(pass = (int)(Pass.Type | Pass.Global))] + [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult pitchStartTime = new EditableDoubleMult(10); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] + [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult pitchRate = new EditableDoubleMult(0.75); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] + [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult pitchEndTime = new EditableDoubleMult(55); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] - public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(100000, 1000); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] + [Persistent(pass = (int)(Pass.Global))] + public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(100000, -1); + [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult terminalGuidanceSecs = new EditableDoubleMult(10); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] + [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult stageLowDVLimit = new EditableDoubleMult(20); - [Persistent(pass = (int)(Pass.Type | Pass.Global))] + [Persistent(pass = (int)(Pass.Type))] public EditableInt edit_num_stages = new EditableInt(2); private int num_stages; From 0c9ad1328c4f46f2cb6257833fc678c8ccce5bc4 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Mon, 10 Jul 2017 19:50:19 -0700 Subject: [PATCH 43/51] fix NRE logspammage Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 31 ++++++++++++++++++++------ MechJeb2/MechJebModuleAscentPEGMenu.cs | 2 +- 2 files changed, 25 insertions(+), 8 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 8f5e5928b..53dae37d8 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -27,7 +27,8 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase public EditableDoubleMult stageLowDVLimit = new EditableDoubleMult(20); [Persistent(pass = (int)(Pass.Type))] public EditableInt edit_num_stages = new EditableInt(2); - private int num_stages; + private int last_edit_num_stages; // need this to track if the user updated the edit_num_stages value + public int num_stages; // need this so that PEG can decreate the num_stages independently of edit_num_stages /* this deliberately does not persist, it is for emergencies only */ public EditableDoubleMult pitchBias = new EditableDoubleMult(0); @@ -39,6 +40,7 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase public override void OnModuleEnabled() { SetNumStages(edit_num_stages); + last_edit_num_stages = edit_num_stages; mode = AscentMode.VERTICAL_ASCENT; InitStageStats(); } @@ -92,11 +94,8 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; private void SetNumStages(int n) { - edit_num_stages = n; - if (num_stages != n) - { - num_stages = n; - } + num_stages = n; + InitConstantCache(); } private void DirtyCacheForStage(int snum) @@ -104,6 +103,18 @@ private void DirtyCacheForStage(int snum) if (snum >= stages.Count) return; + if (stages[snum].b_dirty == null) + { + Debug.Log("[MechJeb] MechJebModuleAscentPEG DirtyCacheForStage() called when b_dirty is null"); + return; + } + + if (stages[snum].c_dirty == null) + { + Debug.Log("[MechJeb] MechJebModuleAscentPEG DirtyCacheForStage() called when c_dirty is null"); + return; + } + for(int i = 0; i <= num_stages; i++) { stages[snum].b_dirty[i] = stages[snum].c_dirty[i] = true; } @@ -305,6 +316,7 @@ void UpdateStageStats() if ( mode == AscentMode.GRAVITY_TURN && guidanceEnabled ) SetNumStages( Math.Max(1, num_stages - 1) ); stages.RemoveAt(i); + InitConstantCache(); } } // dirty the constant cache for the bottom stage because v_e and such is live and may be changing (even exoatmospheric due to Agathorn) @@ -714,7 +726,12 @@ public override bool DriveAscent(FlightCtrlState s) { stats.RequestUpdate(this); stats.liveSLT = true; /* yes, this disables the button, yes, it is important we do this */ - SetNumStages(edit_num_stages); + if (last_edit_num_stages != edit_num_stages) + { + SetNumStages(edit_num_stages); + last_edit_num_stages = edit_num_stages; + } + UpdateRocketStats(); if (last_time != 0.0D) diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index ab57bac57..0d510b80f 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -49,7 +49,7 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Booster Pitch end:", path.pitchEndTime, "s"); GUILayout.Label(String.Format("ending pitch: {0:F1}°", 90.0 - (path.pitchEndTime - path.pitchStartTime)*path.pitchRate)); GuiUtils.SimpleTextBox("Terminal Guidance Period:", path.terminalGuidanceSecs, "s"); - GuiUtils.SimpleTextBox("Num Stages:", path.edit_num_stages); + GuiUtils.SimpleTextBox(String.Format("Num Stages: ({0:D})", path.num_stages), path.edit_num_stages); GUILayout.Label("Stage Stats"); if (GUILayout.Button("Reinitialize Stage Analysis")) path.InitStageStats(); From 29d9d4b512ed9ecb22930abb1bd4b22b109da95f Mon Sep 17 00:00:00 2001 From: NathanKell Date: Mon, 10 Jul 2017 20:00:58 -0700 Subject: [PATCH 44/51] Fix flow from ignored props, fix copy-paste issue --- MechJeb2/FuelFlowSimulation.cs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/MechJeb2/FuelFlowSimulation.cs b/MechJeb2/FuelFlowSimulation.cs index 824e53522..b4128f273 100644 --- a/MechJeb2/FuelFlowSimulation.cs +++ b/MechJeb2/FuelFlowSimulation.cs @@ -613,7 +613,7 @@ private void Init(Part part, bool dVLinearThrust) // Some brilliant engine mod seems to consider that FuelFlow is not something they should properly initialize if (minFuelFlow == 0 && engine.minThrust > 0) { - maxFuelFlow = engine.minThrust / (engine.atmosphereCurve.Evaluate(0f) * engine.g); + minFuelFlow = engine.minThrust / (engine.atmosphereCurve.Evaluate(0f) * engine.g); } if (maxFuelFlow == 0 && engine.maxThrust > 0) { @@ -630,6 +630,9 @@ private void Init(Part part, bool dVLinearThrust) velCurve = new FloatCurve(engine.velCurve.Curve.keys); propellantSumRatioTimesDensity = engine.propellants.Slinq().Select(prop => prop.ratio * MuUtils.ResourceDensity(prop.id)).Sum(); + float ratip = propellantSumRatioTimesDensity / engine.propellants.Slinq().Where(prop => !prop.ignoreForIsp).Select(prop => prop.ratio * MuUtils.ResourceDensity(prop.id)).Sum(); + maxFuelFlow *= ratio; + minFuelFlow *= ratio; propellantRatios.Clear(); propellantFlows.Clear(); var dics = new Tuple, KeyableDictionary>(propellantRatios, propellantFlows); From dd9ecd4690f843a2d068f47abe3ee0334d8205e0 Mon Sep 17 00:00:00 2001 From: NathanKell Date: Mon, 10 Jul 2017 20:01:48 -0700 Subject: [PATCH 45/51] Typo fix --- MechJeb2/FuelFlowSimulation.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/FuelFlowSimulation.cs b/MechJeb2/FuelFlowSimulation.cs index b4128f273..d5d1dc4a5 100644 --- a/MechJeb2/FuelFlowSimulation.cs +++ b/MechJeb2/FuelFlowSimulation.cs @@ -630,7 +630,7 @@ private void Init(Part part, bool dVLinearThrust) velCurve = new FloatCurve(engine.velCurve.Curve.keys); propellantSumRatioTimesDensity = engine.propellants.Slinq().Select(prop => prop.ratio * MuUtils.ResourceDensity(prop.id)).Sum(); - float ratip = propellantSumRatioTimesDensity / engine.propellants.Slinq().Where(prop => !prop.ignoreForIsp).Select(prop => prop.ratio * MuUtils.ResourceDensity(prop.id)).Sum(); + float ratio = propellantSumRatioTimesDensity / engine.propellants.Slinq().Where(prop => !prop.ignoreForIsp).Select(prop => prop.ratio * MuUtils.ResourceDensity(prop.id)).Sum(); maxFuelFlow *= ratio; minFuelFlow *= ratio; propellantRatios.Clear(); From 0ea85708beb704bbf1d7df69d2e6f65447340158 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 12 Jul 2017 09:27:39 -0700 Subject: [PATCH 46/51] scientific notation Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEGMenu.cs | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index 0d510b80f..bc399f2d8 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -60,12 +60,11 @@ protected override void WindowGUI(int windowID) } GUILayout.Label("Burnout Stats"); if (path.stages.Count > 0) { - // GUILayout.Label(String.Format("delta-V (estimate): {0:F1}", path.dVest)); - GUILayout.Label(String.Format("delta-V (guidance): {0:F1}", path.total_dV)); - GUILayout.Label(String.Format("A: {0:F4}", path.stages[0].A)); - GUILayout.Label(String.Format("B: {0:F4}", path.stages[0].B)); - GUILayout.Label(String.Format("time: {0:F1}", path.total_T)); - GUILayout.Label(String.Format("pitch: {0:F1}", path.guidancePitch)); + GUILayout.Label(String.Format("delta-V (guidance): {0:G5}", path.total_dV)); + GUILayout.Label(String.Format("A: {0:G5}", path.stages[0].A)); + GUILayout.Label(String.Format("B: {0:G5}", path.stages[0].B)); + GUILayout.Label(String.Format("time: {0:G5}", path.total_T)); + GUILayout.Label(String.Format("pitch: {0:G5}", path.guidancePitch)); GUILayout.Label(String.Format("steps: {0:D}", path.convergenceSteps)); } From ccfe97e723db6841523744a0e705ac7e025869e1 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 12 Jul 2017 09:27:46 -0700 Subject: [PATCH 47/51] initializing stage stats should clear terminal guidance Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 53dae37d8..96c9902fb 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -228,6 +228,7 @@ void UpdateStageFromMechJeb(StageInfo stage, bool atmo) public void InitStageStats() { Debug.Log("MechJebModuleAscentPEG.InitStageStats()"); + terminalGuidance = false stages.Clear(); skippedParts.Clear(); for ( int i = vacStats.Length-1; i >= 0; i-- ) From 991ff4bf101b66d8bcdb99d7fbf13a58996feb8b Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 12 Jul 2017 09:28:38 -0700 Subject: [PATCH 48/51] a semicolon Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 96c9902fb..e8abf9fe1 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -228,7 +228,7 @@ void UpdateStageFromMechJeb(StageInfo stage, bool atmo) public void InitStageStats() { Debug.Log("MechJebModuleAscentPEG.InitStageStats()"); - terminalGuidance = false + terminalGuidance = false; stages.Clear(); skippedParts.Clear(); for ( int i = vacStats.Length-1; i >= 0; i-- ) From 8cb54c49a1498ee7740ccc3d58a39f73b22c255d Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 12 Jul 2017 18:22:24 -0700 Subject: [PATCH 49/51] reinit should clear all state and reconverge again Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index e8abf9fe1..7df44acdd 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -43,6 +43,7 @@ public override void OnModuleEnabled() last_edit_num_stages = edit_num_stages; mode = AscentMode.VERTICAL_ASCENT; InitStageStats(); + needsConverge = true; } public override void OnModuleDisabled() @@ -59,8 +60,19 @@ private enum AscentMode { VERTICAL_ASCENT, INITIATE_TURN, GRAVITY_TURN, EXIT }; public bool guidanceEnabled = true; public int convergenceSteps { get; private set; } + /* saneGuidance tracks the state of the last iteration and if it was sane or not */ private bool saneGuidance = false; + /* terminal guidance is set when we're in terminal guidance phase */ public bool terminalGuidance = false; + /* needsConverge is set when we need to re-converge again */ + public bool needsConverge = true; + + /* clears state so that we fully reconverge next time */ + public void ResetBooleans() { + saneGuidance = false; + terminalGuidance = false; + needsConverge = true; + } /* tangential velocity at burnout */ private double v_burnout; @@ -254,6 +266,7 @@ public void InitStageStats() } } InitConstantCache(); + ResetBooleans(); } bool PartsListsMatch(List one, List two) @@ -646,7 +659,7 @@ private bool bad_guidance(StageInfo stage) return Double.IsNaN(T) || Double.IsInfinity(T) || T <= 0.0D || Double.IsNaN(A) || Double.IsInfinity(A) || Double.IsNaN(B) || Double.IsInfinity(B); } - private void converge(double dt, bool initialize = false) + private void converge(double dt) { double Astart = stages[0].A; double Bstart = stages[0].B; @@ -656,7 +669,7 @@ private void converge(double dt, bool initialize = false) terminalGuidance = true; if (!terminalGuidance) { - if (initialize || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance ) + if (needsConverge || bad_guidance(stages[0]) || bad_pitch() || bad_dV() || !saneGuidance ) { stages[num_stages-1].T = stages[num_stages-1].avail_T; stages[0].A = Math.Sin(srfvelPitch()) - stages[0].G / stages[0].a0; @@ -717,6 +730,7 @@ private void converge(double dt, bool initialize = false) } else { + needsConverge = false; saneGuidance = true; } } From 03a62a189d437319dde113dbb194bc5dd86aebd0 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 12 Jul 2017 21:00:46 -0700 Subject: [PATCH 50/51] fix silly apoapsis GUI bug Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index 7df44acdd..c2071f4a7 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -20,7 +20,7 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult pitchEndTime = new EditableDoubleMult(55); [Persistent(pass = (int)(Pass.Global))] - public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(100000, -1); + public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(-1, 1000); [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult terminalGuidanceSecs = new EditableDoubleMult(10); [Persistent(pass = (int)(Pass.Type))] From fa4d544b3693e89508c84f4e30d1eac4fecf26d0 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Fri, 14 Jul 2017 21:04:46 -0700 Subject: [PATCH 51/51] add negative apoapsis hyperbolic orbits Signed-off-by: Lamont Granquist --- MechJeb2/MechJebModuleAscentPEG.cs | 8 ++++---- MechJeb2/MechJebModuleAscentPEGMenu.cs | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/MechJeb2/MechJebModuleAscentPEG.cs b/MechJeb2/MechJebModuleAscentPEG.cs index c2071f4a7..dbac720e5 100644 --- a/MechJeb2/MechJebModuleAscentPEG.cs +++ b/MechJeb2/MechJebModuleAscentPEG.cs @@ -20,7 +20,7 @@ public class MechJebModuleAscentPEG : MechJebModuleAscentBase [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult pitchEndTime = new EditableDoubleMult(55); [Persistent(pass = (int)(Pass.Global))] - public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(-1, 1000); + public EditableDoubleMult desiredApoapsis = new EditableDoubleMult(0, 1000); [Persistent(pass = (int)(Pass.Type))] public EditableDoubleMult terminalGuidanceSecs = new EditableDoubleMult(10); [Persistent(pass = (int)(Pass.Type))] @@ -338,10 +338,10 @@ void UpdateStageStats() } private double smaT() { - if ( desiredApoapsis > autopilot.desiredOrbitAltitude ) - return (autopilot.desiredOrbitAltitude + 2 * mainBody.Radius + desiredApoapsis) / 2; - else + if ( desiredApoapsis >= 0 && desiredApoapsis < autopilot.desiredOrbitAltitude ) return autopilot.desiredOrbitAltitude + mainBody.Radius; + else + return (autopilot.desiredOrbitAltitude + 2 * mainBody.Radius + desiredApoapsis) / 2; } private void UpdateRocketStats() { diff --git a/MechJeb2/MechJebModuleAscentPEGMenu.cs b/MechJeb2/MechJebModuleAscentPEGMenu.cs index bc399f2d8..600358a4a 100644 --- a/MechJeb2/MechJebModuleAscentPEGMenu.cs +++ b/MechJeb2/MechJebModuleAscentPEGMenu.cs @@ -37,7 +37,7 @@ protected override void WindowGUI(int windowID) GuiUtils.SimpleTextBox("Target Periapsis:", autopilot.desiredOrbitAltitude, "km"); GuiUtils.SimpleTextBox("Target Apoapsis:", path.desiredApoapsis, "km"); - if ( path.desiredApoapsis < autopilot.desiredOrbitAltitude ) + if ( path.desiredApoapsis >= 0 && path.desiredApoapsis < autopilot.desiredOrbitAltitude ) { GUIStyle s = new GUIStyle(GUI.skin.label); s.normal.textColor = Color.yellow;