diff --git a/MechJeb2/AutopilotModule.cs b/MechJeb2/AutopilotModule.cs index 1784ef86..58803150 100644 --- a/MechJeb2/AutopilotModule.cs +++ b/MechJeb2/AutopilotModule.cs @@ -5,77 +5,73 @@ namespace MuMech { public class AutopilotModule : ComputerModule { - public AutopilotModule(MechJebCore core) : base(core) + protected AutopilotModule(MechJebCore core) : base(core) { } public override void Drive(FlightCtrlState s) { - if (CurrentStep != null) + if (CurrentStep == null) return; + + try + { + CurrentStep = CurrentStep.Drive(s); + } + catch (Exception ex) { - try - { - CurrentStep = CurrentStep.Drive(s); - } - catch (Exception ex) - { - Debug.LogException(ex); - } + Debug.LogException(ex); } } public override void OnFixedUpdate() { - if (CurrentStep != null) + if (CurrentStep == null) return; + + try + { + CurrentStep = CurrentStep.OnFixedUpdate(); + } + catch (Exception ex) { - try - { - CurrentStep = CurrentStep.OnFixedUpdate(); - } - catch (Exception ex) - { - Debug.LogException(ex); - } + Debug.LogException(ex); } } - public void setStep(AutopilotStep step) + protected void SetStep(AutopilotStep step) { CurrentStep = step; } - public string status + public string Status { get { - if (CurrentStep == null) - return "Off"; - return CurrentStep.status; + return CurrentStep == null ? "Off" : CurrentStep.Status; } } - public bool active => CurrentStep != null; + protected bool Active => CurrentStep != null; public AutopilotStep CurrentStep { get; private set; } } public class AutopilotStep { - public MechJebCore core; + protected readonly MechJebCore Core; //conveniences: - public VesselState vesselState => core.vesselState; - public Vessel vessel => core.part.vessel; - public CelestialBody mainBody => core.part.vessel.mainBody; - public Orbit orbit => core.part.vessel.orbit; + protected VesselState VesselState => Core.vesselState; + protected Vessel Vessel => Core.part.vessel; + protected CelestialBody MainBody => Core.part.vessel.mainBody; + protected Orbit Orbit => Core.part.vessel.orbit; - public AutopilotStep(MechJebCore core) + protected AutopilotStep(MechJebCore core) { - this.core = core; + this.Core = core; } public virtual AutopilotStep Drive(FlightCtrlState s) { return this; } public virtual AutopilotStep OnFixedUpdate() { return this; } - public string status { get; protected set; } + public string Status { get; protected set; } } } diff --git a/MechJeb2/LandingAutopilot/CoastToDeceleration.cs b/MechJeb2/LandingAutopilot/CoastToDeceleration.cs index b9893399..6a3245f2 100644 --- a/MechJeb2/LandingAutopilot/CoastToDeceleration.cs +++ b/MechJeb2/LandingAutopilot/CoastToDeceleration.cs @@ -15,116 +15,115 @@ public CoastToDeceleration(MechJebCore core) : base(core) public override AutopilotStep Drive(FlightCtrlState s) { - if (!core.landing.PredictionReady) + if (!Core.landing.PredictionReady) return this; - Vector3d deltaV = core.landing.ComputeCourseCorrection(true); + Vector3d deltaV = Core.landing.ComputeCourseCorrection(true); - if (core.landing.rcsAdjustment) - { - if (deltaV.magnitude > 3) - core.rcs.enabled = true; - else if (deltaV.magnitude < 0.01) - core.rcs.enabled = false; + if (!Core.landing.RCSAdjustment) return this; - if (core.rcs.enabled) - core.rcs.SetWorldVelocityError(deltaV); - } + if (deltaV.magnitude > 3) + Core.rcs.enabled = true; + else if (deltaV.magnitude < 0.01) + Core.rcs.enabled = false; + + if (Core.rcs.enabled) + Core.rcs.SetWorldVelocityError(deltaV); return this; } - private bool warpReady; + private bool _warpReady; public override AutopilotStep OnFixedUpdate() { - core.thrust.targetThrottle = 0; + Core.thrust.targetThrottle = 0; // If the atmospheric drag is has started to act on the vessel then we are in a position to start considering when to deploy the parachutes. - if (core.landing.deployChutes) + if (Core.landing.DeployChutes) { - if (core.landing.ParachutesDeployable()) + if (Core.landing.ParachutesDeployable()) { - core.landing.ControlParachutes(); + Core.landing.ControlParachutes(); } } - double maxAllowedSpeed = core.landing.MaxAllowedSpeed(); - if (vesselState.speedSurface > 0.9 * maxAllowedSpeed) + double maxAllowedSpeed = Core.landing.MaxAllowedSpeed(); + if (VesselState.speedSurface > 0.9 * maxAllowedSpeed) { - core.warp.MinimumWarp(); - if (core.landing.rcsAdjustment) - core.rcs.enabled = false; - return new DecelerationBurn(core); + Core.warp.MinimumWarp(); + if (Core.landing.RCSAdjustment) + Core.rcs.enabled = false; + return new DecelerationBurn(Core); } - status = Localizer.Format("#MechJeb_LandingGuidance_Status1"); //"Coasting toward deceleration burn" + Status = Localizer.Format("#MechJeb_LandingGuidance_Status1"); //"Coasting toward deceleration burn" - if (core.landing.landAtTarget) + if (Core.landing.LandAtTarget) { - double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), core.landing.LandingSite); + double currentError = Vector3d.Distance(Core.target.GetPositionTargetPosition(), Core.landing.LandingSite); if (currentError > 1000) { - if (!vesselState.parachuteDeployed && - vesselState.drag <= + if (!VesselState.parachuteDeployed && + VesselState.drag <= 0.1) // However if there is already a parachute deployed or drag is high, then do not bother trying to correct the course as we will not have any attitude control anyway. { - core.warp.MinimumWarp(); - if (core.landing.rcsAdjustment) - core.rcs.enabled = false; - return new CourseCorrection(core); + Core.warp.MinimumWarp(); + if (Core.landing.RCSAdjustment) + Core.rcs.enabled = false; + return new CourseCorrection(Core); } } else { - Vector3d deltaV = core.landing.ComputeCourseCorrection(true); - status += "\n" + Localizer.Format("#MechJeb_LandingGuidance_Status2", + Vector3d deltaV = Core.landing.ComputeCourseCorrection(true); + Status += "\n" + Localizer.Format("#MechJeb_LandingGuidance_Status2", deltaV.magnitude.ToString("F3")); //"Course correction DV: " + + " m/s" } } // If we're already low, skip directly to the Deceleration burn - if (vesselState.altitudeASL < core.landing.DecelerationEndAltitude() + 5) + if (VesselState.altitudeASL < Core.landing.DecelerationEndAltitude() + 5) { - core.warp.MinimumWarp(); - if (core.landing.rcsAdjustment) - core.rcs.enabled = false; - return new DecelerationBurn(core); + Core.warp.MinimumWarp(); + if (Core.landing.RCSAdjustment) + Core.rcs.enabled = false; + return new DecelerationBurn(Core); } - if (core.attitude.attitudeAngleFromTarget() < 1) { warpReady = true; } // less warp start warp stop jumping + if (Core.attitude.attitudeAngleFromTarget() < 1) { _warpReady = true; } // less warp start warp stop jumping - if (core.attitude.attitudeAngleFromTarget() > 5) { warpReady = false; } // hopefully + if (Core.attitude.attitudeAngleFromTarget() > 5) { _warpReady = false; } // hopefully - if (core.landing.PredictionReady) + if (Core.landing.PredictionReady) { - if (vesselState.drag < 0.01) + if (VesselState.drag < 0.01) { - double decelerationStartTime = core.landing.Prediction.Trajectory.Any() - ? core.landing.Prediction.Trajectory.First().UT - : vesselState.time; - Vector3d decelerationStartAttitude = -orbit.WorldOrbitalVelocityAtUT(decelerationStartTime); - decelerationStartAttitude += mainBody.getRFrmVel(orbit.WorldPositionAtUT(decelerationStartTime)); + double decelerationStartTime = Core.landing.Prediction.Trajectory.Any() + ? Core.landing.Prediction.Trajectory.First().UT + : VesselState.time; + Vector3d decelerationStartAttitude = -Orbit.WorldOrbitalVelocityAtUT(decelerationStartTime); + decelerationStartAttitude += MainBody.getRFrmVel(Orbit.WorldPositionAtUT(decelerationStartTime)); decelerationStartAttitude = decelerationStartAttitude.normalized; - core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, core.landing); + Core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, Core.landing); } else { - core.attitude.attitudeTo(Vector3.back, AttitudeReference.SURFACE_VELOCITY, core.landing); + Core.attitude.attitudeTo(Vector3.back, AttitudeReference.SURFACE_VELOCITY, Core.landing); } } //Warp at a rate no higher than the rate that would have us impacting the ground 10 seconds from now: - if (warpReady && core.node.autowarp) + if (_warpReady && Core.node.autowarp) { // Make sure if we're hovering that we don't go straight into too fast of a warp // (g * 5 is average velocity falling for 10 seconds from a hover) - double velocityGuess = Math.Max(Math.Abs(vesselState.speedVertical), vesselState.localg * 5); - core.warp.WarpRegularAtRate((float)(vesselState.altitudeASL / (10 * velocityGuess))); + double velocityGuess = Math.Max(Math.Abs(VesselState.speedVertical), VesselState.localg * 5); + Core.warp.WarpRegularAtRate((float)(VesselState.altitudeASL / (10 * velocityGuess))); } else { - core.warp.MinimumWarp(); + Core.warp.MinimumWarp(); } return this; diff --git a/MechJeb2/LandingAutopilot/CourseCorrection.cs b/MechJeb2/LandingAutopilot/CourseCorrection.cs index caf1cf4a..f8a07d9b 100644 --- a/MechJeb2/LandingAutopilot/CourseCorrection.cs +++ b/MechJeb2/LandingAutopilot/CourseCorrection.cs @@ -6,7 +6,7 @@ namespace Landing { public class CourseCorrection : AutopilotStep { - private bool courseCorrectionBurning; + private bool _courseCorrectionBurning; public CourseCorrection(MechJebCore core) : base(core) { @@ -14,68 +14,68 @@ public CourseCorrection(MechJebCore core) : base(core) public override AutopilotStep Drive(FlightCtrlState s) { - if (!core.landing.PredictionReady) + if (!Core.landing.PredictionReady) return this; // If the atomospheric drag is at least 100mm/s2 then start trying to target the overshoot using the parachutes - if (core.landing.deployChutes) + if (Core.landing.DeployChutes) { - if (core.landing.ParachutesDeployable()) + if (Core.landing.ParachutesDeployable()) { - core.landing.ControlParachutes(); + Core.landing.ControlParachutes(); } } - double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), core.landing.LandingSite); + double currentError = Vector3d.Distance(Core.target.GetPositionTargetPosition(), Core.landing.LandingSite); if (currentError < 150) { - core.thrust.targetThrottle = 0; - if (core.landing.rcsAdjustment) - core.rcs.enabled = true; - return new CoastToDeceleration(core); + Core.thrust.targetThrottle = 0; + if (Core.landing.RCSAdjustment) + Core.rcs.enabled = true; + return new CoastToDeceleration(Core); } // If we're off course, but already too low, skip the course correction - if (vesselState.altitudeASL < core.landing.DecelerationEndAltitude() + 5) + if (VesselState.altitudeASL < Core.landing.DecelerationEndAltitude() + 5) { - return new DecelerationBurn(core); + return new DecelerationBurn(Core); } // If a parachute has already been deployed then we will not be able to control attitude anyway, so move back to the coast to deceleration step. - if (vesselState.parachuteDeployed) + if (VesselState.parachuteDeployed) { - core.thrust.targetThrottle = 0; - return new CoastToDeceleration(core); + Core.thrust.targetThrottle = 0; + return new CoastToDeceleration(Core); } // We are not in .90 anymore. Turning while under drag is a bad idea - if (vesselState.drag > 0.1) + if (VesselState.drag > 0.1) { - return new CoastToDeceleration(core); + return new CoastToDeceleration(Core); } - Vector3d deltaV = core.landing.ComputeCourseCorrection(true); + Vector3d deltaV = Core.landing.ComputeCourseCorrection(true); - status = Localizer.Format("#MechJeb_LandingGuidance_Status3", + Status = Localizer.Format("#MechJeb_LandingGuidance_Status3", deltaV.magnitude.ToString("F1")); //"Performing course correction of about " + + " m/s" - core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, core.landing); + Core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, Core.landing); - if (core.attitude.attitudeAngleFromTarget() < 2) - courseCorrectionBurning = true; - else if (core.attitude.attitudeAngleFromTarget() > 30) - courseCorrectionBurning = false; + if (Core.attitude.attitudeAngleFromTarget() < 2) + _courseCorrectionBurning = true; + else if (Core.attitude.attitudeAngleFromTarget() > 30) + _courseCorrectionBurning = false; - if (courseCorrectionBurning) + if (_courseCorrectionBurning) { - const double timeConstant = 2.0; - core.thrust.ThrustForDV(deltaV.magnitude, timeConstant); + const double TIME_CONSTANT = 2.0; + Core.thrust.ThrustForDV(deltaV.magnitude, TIME_CONSTANT); } else { - core.thrust.targetThrottle = 0; + Core.thrust.targetThrottle = 0; } return this; diff --git a/MechJeb2/LandingAutopilot/DecelerationBurn.cs b/MechJeb2/LandingAutopilot/DecelerationBurn.cs index 6b1d10e3..bbc4d2fe 100644 --- a/MechJeb2/LandingAutopilot/DecelerationBurn.cs +++ b/MechJeb2/LandingAutopilot/DecelerationBurn.cs @@ -15,71 +15,71 @@ public DecelerationBurn(MechJebCore core) : base(core) public override AutopilotStep OnFixedUpdate() { - if (vesselState.altitudeASL < core.landing.DecelerationEndAltitude() + 5) + if (VesselState.altitudeASL < Core.landing.DecelerationEndAltitude() + 5) { - core.warp.MinimumWarp(); + Core.warp.MinimumWarp(); - if (core.landing.UseAtmosphereToBrake()) - return new FinalDescent(core); - return new KillHorizontalVelocity(core); + if (Core.landing.UseAtmosphereToBrake()) + return new FinalDescent(Core); + return new KillHorizontalVelocity(Core); } double decelerationStartTime = - core.landing.Prediction.Trajectory.Any() ? core.landing.Prediction.Trajectory.First().UT : vesselState.time; - if (decelerationStartTime - vesselState.time > 5) + Core.landing.Prediction.Trajectory.Any() ? Core.landing.Prediction.Trajectory.First().UT : VesselState.time; + if (decelerationStartTime - VesselState.time > 5) { - core.thrust.targetThrottle = 0; + Core.thrust.targetThrottle = 0; - status = Localizer.Format("#MechJeb_LandingGuidance_Status4"); //"Warping to start of braking burn." + Status = Localizer.Format("#MechJeb_LandingGuidance_Status4"); //"Warping to start of braking burn." //warp to deceleration start - Vector3d decelerationStartAttitude = -orbit.WorldOrbitalVelocityAtUT(decelerationStartTime); - decelerationStartAttitude += mainBody.getRFrmVel(orbit.WorldPositionAtUT(decelerationStartTime)); + Vector3d decelerationStartAttitude = -Orbit.WorldOrbitalVelocityAtUT(decelerationStartTime); + decelerationStartAttitude += MainBody.getRFrmVel(Orbit.WorldPositionAtUT(decelerationStartTime)); decelerationStartAttitude = decelerationStartAttitude.normalized; - core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, core.landing); - bool warpReady = core.attitude.attitudeAngleFromTarget() < 5; + Core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, Core.landing); + bool warpReady = Core.attitude.attitudeAngleFromTarget() < 5; - if (warpReady && core.node.autowarp) - core.warp.WarpToUT(decelerationStartTime - 5); + if (warpReady && Core.node.autowarp) + Core.warp.WarpToUT(decelerationStartTime - 5); else if (!MuUtils.PhysicsRunning()) - core.warp.MinimumWarp(); + Core.warp.MinimumWarp(); return this; } - Vector3d desiredThrustVector = -vesselState.surfaceVelocity.normalized; + Vector3d desiredThrustVector = -VesselState.surfaceVelocity.normalized; - Vector3d courseCorrection = core.landing.ComputeCourseCorrection(false); - double correctionAngle = courseCorrection.magnitude / (2.0 * vesselState.limitedMaxThrustAccel); + Vector3d courseCorrection = Core.landing.ComputeCourseCorrection(false); + double correctionAngle = courseCorrection.magnitude / (2.0 * VesselState.limitedMaxThrustAccel); correctionAngle = Math.Min(0.1, correctionAngle); desiredThrustVector = (desiredThrustVector + correctionAngle * courseCorrection.normalized).normalized; - if (Vector3d.Dot(vesselState.surfaceVelocity, vesselState.up) > 0 - || Vector3d.Dot(vesselState.forward, desiredThrustVector) < 0.75) + if (Vector3d.Dot(VesselState.surfaceVelocity, VesselState.up) > 0 + || Vector3d.Dot(VesselState.forward, desiredThrustVector) < 0.75) { - core.thrust.targetThrottle = 0; - status = Localizer.Format("#MechJeb_LandingGuidance_Status5"); //"Braking" + Core.thrust.targetThrottle = 0; + Status = Localizer.Format("#MechJeb_LandingGuidance_Status5"); //"Braking" } else { double controlledSpeed = - vesselState.speedSurface * - Math.Sign(Vector3d.Dot(vesselState.surfaceVelocity, vesselState.up)); //positive if we are ascending, negative if descending - double desiredSpeed = -core.landing.MaxAllowedSpeed(); - double desiredSpeedAfterDt = -core.landing.MaxAllowedSpeedAfterDt(vesselState.deltaT); - double minAccel = -vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.surfaceVelocity.normalized, vesselState.up)); - double maxAccel = vesselState.maxThrustAccel * Vector3d.Dot(vesselState.forward, -vesselState.surfaceVelocity.normalized) - - vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.surfaceVelocity.normalized, vesselState.up)); - const double speedCorrectionTimeConstant = 0.3; + VesselState.speedSurface * + Math.Sign(Vector3d.Dot(VesselState.surfaceVelocity, VesselState.up)); //positive if we are ascending, negative if descending + double desiredSpeed = -Core.landing.MaxAllowedSpeed(); + double desiredSpeedAfterDt = -Core.landing.MaxAllowedSpeedAfterDt(VesselState.deltaT); + double minAccel = -VesselState.localg * Math.Abs(Vector3d.Dot(VesselState.surfaceVelocity.normalized, VesselState.up)); + double maxAccel = VesselState.maxThrustAccel * Vector3d.Dot(VesselState.forward, -VesselState.surfaceVelocity.normalized) - + VesselState.localg * Math.Abs(Vector3d.Dot(VesselState.surfaceVelocity.normalized, VesselState.up)); + const double SPEED_CORRECTION_TIME_CONSTANT = 0.3; double speedError = desiredSpeed - controlledSpeed; - double desiredAccel = speedError / speedCorrectionTimeConstant + (desiredSpeedAfterDt - desiredSpeed) / vesselState.deltaT; + double desiredAccel = speedError / SPEED_CORRECTION_TIME_CONSTANT + (desiredSpeedAfterDt - desiredSpeed) / VesselState.deltaT; if (maxAccel - minAccel > 0) - core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F); - else core.thrust.targetThrottle = 0; - status = Localizer.Format("#MechJeb_LandingGuidance_Status6", + Core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F); + else Core.thrust.targetThrottle = 0; + Status = Localizer.Format("#MechJeb_LandingGuidance_Status6", desiredSpeed >= double.MaxValue ? "∞" : Math.Abs(desiredSpeed).ToString("F1")); //"Braking: target speed = " + + " m/s" } - core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, core.landing); + Core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, Core.landing); return this; } diff --git a/MechJeb2/LandingAutopilot/DeorbitBurn.cs b/MechJeb2/LandingAutopilot/DeorbitBurn.cs index 929aa325..2d11381d 100644 --- a/MechJeb2/LandingAutopilot/DeorbitBurn.cs +++ b/MechJeb2/LandingAutopilot/DeorbitBurn.cs @@ -10,7 +10,7 @@ namespace Landing { public class DeorbitBurn : AutopilotStep { - private bool deorbitBurnTriggered; + private bool _deorbitBurnTriggered; public DeorbitBurn(MechJebCore core) : base(core) { @@ -18,53 +18,53 @@ public DeorbitBurn(MechJebCore core) : base(core) public override AutopilotStep Drive(FlightCtrlState s) { - if (deorbitBurnTriggered && core.attitude.attitudeAngleFromTarget() < 5) - core.thrust.targetThrottle = 1.0F; + if (_deorbitBurnTriggered && Core.attitude.attitudeAngleFromTarget() < 5) + Core.thrust.targetThrottle = 1.0F; else - core.thrust.targetThrottle = 0; + Core.thrust.targetThrottle = 0; return this; } public override AutopilotStep OnFixedUpdate() { - //if we don't want to deorbit but we're already on a reentry trajectory, we can't wait until the ideal point + //if we don't want to deorbit but we're already on a reentry trajectory, we can't wait until the ideal point //in the orbit to deorbt; we already have deorbited. - if (orbit.ApA < mainBody.RealMaxAtmosphereAltitude()) + if (Orbit.ApA < MainBody.RealMaxAtmosphereAltitude()) { - core.thrust.targetThrottle = 0; - return new CourseCorrection(core); + Core.thrust.targetThrottle = 0; + return new CourseCorrection(Core); } - //We aim for a trajectory that + //We aim for a trajectory that // a) has the same vertical speed as our current trajectory // b) has a horizontal speed that will give it a periapsis of -10% of the body's radius // c) has a heading that points toward where the target will be at the end of free-fall, accounting for planetary rotation Vector3d horizontalDV = - OrbitalManeuverCalculator.DeltaVToChangePeriapsis(orbit, vesselState.time, - 0.9 * mainBody + OrbitalManeuverCalculator.DeltaVToChangePeriapsis(Orbit, VesselState.time, + 0.9 * MainBody .Radius); //Imagine we are going to deorbit now. Find the burn that would lower our periapsis to -10% of the planet's radius - Orbit forwardDeorbitTrajectory = orbit.PerturbedOrbit(vesselState.time, horizontalDV); //Compute the orbit that would put us on + Orbit forwardDeorbitTrajectory = Orbit.PerturbedOrbit(VesselState.time, horizontalDV); //Compute the orbit that would put us on double freefallTime = - forwardDeorbitTrajectory.NextTimeOfRadius(vesselState.time, mainBody.Radius) - - vesselState.time; //Find how long that orbit would take to impact the ground + forwardDeorbitTrajectory.NextTimeOfRadius(VesselState.time, MainBody.Radius) - + VesselState.time; //Find how long that orbit would take to impact the ground double planetRotationDuringFreefall = - 360 * freefallTime / mainBody.rotationPeriod; //Find how many degrees the planet will rotate during that time - Vector3d currentTargetRadialVector = mainBody.GetWorldSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0) - - mainBody.position; //Find the current vector from the planet center to the target landing site + 360 * freefallTime / MainBody.rotationPeriod; //Find how many degrees the planet will rotate during that time + Vector3d currentTargetRadialVector = MainBody.GetWorldSurfacePosition(Core.target.targetLatitude, Core.target.targetLongitude, 0) - + MainBody.position; //Find the current vector from the planet center to the target landing site var freefallPlanetRotation = Quaternion.AngleAxis((float)planetRotationDuringFreefall, - mainBody.angularVelocity); //Construct a quaternion representing the rotation of the planet found above + MainBody.angularVelocity); //Construct a quaternion representing the rotation of the planet found above Vector3d freefallEndTargetRadialVector = freefallPlanetRotation * currentTargetRadialVector; //Use this quaternion to find what the vector from the planet center to the target will be when we hit the ground Vector3d freefallEndTargetPosition = - mainBody.position + freefallEndTargetRadialVector; //Then find the actual position of the target at that time + MainBody.position + freefallEndTargetRadialVector; //Then find the actual position of the target at that time Vector3d freefallEndHorizontalToTarget = - Vector3d.Exclude(vesselState.up, freefallEndTargetPosition - vesselState.CoM) + Vector3d.Exclude(VesselState.up, freefallEndTargetPosition - VesselState.CoM) .normalized; //Find a horizontal unit vector that points toward where the target will be when we hit the ground - var currentHorizontalVelocity = Vector3d.Exclude(vesselState.up, vesselState.orbitalVelocity); //Find our current horizontal velocity + var currentHorizontalVelocity = Vector3d.Exclude(VesselState.up, VesselState.orbitalVelocity); //Find our current horizontal velocity double finalHorizontalSpeed = (currentHorizontalVelocity + horizontalDV).magnitude; //Find the desired horizontal speed after the deorbit burn Vector3d @@ -73,8 +73,8 @@ public override AutopilotStep OnFixedUpdate() freefallEndHorizontalToTarget; //Combine the desired speed and direction to get the desired velocity after the deorbi burn //Compute the angle between the location of the target at the end of freefall and the normal to our orbit: - Vector3d currentRadialVector = vesselState.CoM - mainBody.position; - double targetAngleToOrbitNormal = Vector3d.Angle(orbit.OrbitNormal(), freefallEndTargetRadialVector); + Vector3d currentRadialVector = VesselState.CoM - MainBody.position; + double targetAngleToOrbitNormal = Vector3d.Angle(Orbit.OrbitNormal(), freefallEndTargetRadialVector); targetAngleToOrbitNormal = Math.Min(targetAngleToOrbitNormal, 180 - targetAngleToOrbitNormal); double targetAheadAngle = @@ -88,29 +88,29 @@ public override AutopilotStep OnFixedUpdate() if (targetAngleToOrbitNormal < 10 || (targetAheadAngle < 90 && targetAheadAngle > 60 && planeChangeAngle < 90)) { - deorbitBurnTriggered = true; + _deorbitBurnTriggered = true; } - if (deorbitBurnTriggered) + if (_deorbitBurnTriggered) { - if (!MuUtils.PhysicsRunning()) { core.warp.MinimumWarp(); } //get out of warp + if (!MuUtils.PhysicsRunning()) { Core.warp.MinimumWarp(); } //get out of warp Vector3d deltaV = finalHorizontalVelocity - currentHorizontalVelocity; - core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, core.landing); + Core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, Core.landing); if (deltaV.magnitude < 2.0) { - return new CourseCorrection(core); + return new CourseCorrection(Core); } - status = Localizer.Format("#MechJeb_LandingGuidance_Status7"); //"Doing high deorbit burn" + Status = Localizer.Format("#MechJeb_LandingGuidance_Status7"); //"Doing high deorbit burn" } else { - core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT, core.landing); - if (core.node.autowarp) core.warp.WarpRegularAtRate((float)(orbit.period / 10)); + Core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT, Core.landing); + if (Core.node.autowarp) Core.warp.WarpRegularAtRate((float)(Orbit.period / 10)); - status = Localizer.Format("#MechJeb_LandingGuidance_Status8"); //"Moving to high deorbit burn point" + Status = Localizer.Format("#MechJeb_LandingGuidance_Status8"); //"Moving to high deorbit burn point" } return this; diff --git a/MechJeb2/LandingAutopilot/FinalDescent.cs b/MechJeb2/LandingAutopilot/FinalDescent.cs index df0aaf44..102546b8 100644 --- a/MechJeb2/LandingAutopilot/FinalDescent.cs +++ b/MechJeb2/LandingAutopilot/FinalDescent.cs @@ -8,7 +8,7 @@ namespace Landing { public class FinalDescent : AutopilotStep { - private IDescentSpeedPolicy aggressivePolicy; + private IDescentSpeedPolicy _aggressivePolicy; public FinalDescent(MechJebCore core) : base(core) { @@ -16,110 +16,109 @@ public FinalDescent(MechJebCore core) : base(core) public override AutopilotStep OnFixedUpdate() { - double minalt = Math.Min(vesselState.altitudeBottom, Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue)); + double minalt = Math.Min(VesselState.altitudeBottom, Math.Min(VesselState.altitudeASL, VesselState.altitudeTrue)); - if (core.node.autowarp && aggressivePolicy != null) - { - double maxVel = 1.02 * aggressivePolicy.MaxAllowedSpeed(vesselState.CoM - mainBody.position, vesselState.surfaceVelocity); + if (!Core.node.autowarp || _aggressivePolicy == null) return this; - double diffPercent = (maxVel / vesselState.speedSurface - 1) * 100; + double maxVel = 1.02 * _aggressivePolicy.MaxAllowedSpeed(VesselState.CoM - MainBody.position, VesselState.surfaceVelocity); - if (minalt > 200 && diffPercent > 0 && Vector3d.Angle(vesselState.forward, -vesselState.surfaceVelocity) < 45) - core.warp.WarpRegularAtRate((float)(diffPercent * diffPercent * diffPercent)); - else - core.warp.MinimumWarp(true); - } + double diffPercent = (maxVel / VesselState.speedSurface - 1) * 100; + + if (minalt > 200 && diffPercent > 0 && Vector3d.Angle(VesselState.forward, -VesselState.surfaceVelocity) < 45) + Core.warp.WarpRegularAtRate((float)(diffPercent * diffPercent * diffPercent)); + else + Core.warp.MinimumWarp(true); return this; } public override AutopilotStep Drive(FlightCtrlState s) { - if (vessel.LandedOrSplashed) + if (Vessel.LandedOrSplashed) { - core.landing.StopLanding(); + Core.landing.StopLanding(); return null; } // TODO perhaps we should pop the parachutes at this point, or at least consider it depending on the altitude. - double minalt = Math.Min(vesselState.altitudeBottom, Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue)); + double minalt = Math.Min(VesselState.altitudeBottom, Math.Min(VesselState.altitudeASL, VesselState.altitudeTrue)); - if (vesselState.limitedMaxThrustAccel < vesselState.gravityForce.magnitude) + if (VesselState.limitedMaxThrustAccel < VesselState.gravityForce.magnitude) { // if we have TWR < 1, just try as hard as we can to decelerate: // (we need this special case because otherwise the calculations spit out NaN's) - core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL; - core.thrust.trans_kill_h = true; - core.thrust.trans_spd_act = 0; + Core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL; + Core.thrust.trans_kill_h = true; + Core.thrust.trans_spd_act = 0; } else if (minalt > 200) { - if (vesselState.surfaceVelocity.magnitude > 5 && Vector3d.Angle(vesselState.surfaceVelocity, vesselState.up) < 80) + if (VesselState.surfaceVelocity.magnitude > 5 && Vector3d.Angle(VesselState.surfaceVelocity, VesselState.up) < 80) { // if we have positive vertical velocity, point up and don't thrust: - core.attitude.attitudeTo(Vector3d.up, AttitudeReference.SURFACE_NORTH, null); - core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT; - core.thrust.trans_spd_act = 0; + Core.attitude.attitudeTo(Vector3d.up, AttitudeReference.SURFACE_NORTH, null); + Core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT; + Core.thrust.trans_spd_act = 0; } - else if (vesselState.surfaceVelocity.magnitude > 5 && Vector3d.Angle(vesselState.forward, -vesselState.surfaceVelocity) > 45) + else if (VesselState.surfaceVelocity.magnitude > 5 && Vector3d.Angle(VesselState.forward, -VesselState.surfaceVelocity) > 45) { // if we're not facing approximately retrograde, turn to point retrograde and don't thrust: - core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null); - core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT; - core.thrust.trans_spd_act = 0; + Core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null); + Core.thrust.tmode = MechJebModuleThrustController.TMode.DIRECT; + Core.thrust.trans_spd_act = 0; } else { //if we're above 200m, point retrograde and control surface velocity: - core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null); + Core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null); - core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE; + Core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE; //core.thrust.trans_spd_act = (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * minalt) * 0.90F; - Vector3d estimatedLandingPosition = vesselState.CoM + vesselState.surfaceVelocity.sqrMagnitude / - (2 * vesselState.limitedMaxThrustAccel) * vesselState.surfaceVelocity.normalized; - double terrainRadius = mainBody.Radius + mainBody.TerrainAltitude(estimatedLandingPosition); - aggressivePolicy = - new GravityTurnDescentSpeedPolicy(terrainRadius, mainBody.GeeASL * 9.81, - vesselState.limitedMaxThrustAccel); // this constant policy creation is wastefull... - core.thrust.trans_spd_act = - (float)aggressivePolicy.MaxAllowedSpeed(vesselState.CoM - mainBody.position, vesselState.surfaceVelocity); + Vector3d estimatedLandingPosition = VesselState.CoM + VesselState.surfaceVelocity.sqrMagnitude / + (2 * VesselState.limitedMaxThrustAccel) * VesselState.surfaceVelocity.normalized; + double terrainRadius = MainBody.Radius + MainBody.TerrainAltitude(estimatedLandingPosition); + _aggressivePolicy = + new GravityTurnDescentSpeedPolicy(terrainRadius, MainBody.GeeASL * 9.81, + VesselState.limitedMaxThrustAccel); // this constant policy creation is wastefull... + Core.thrust.trans_spd_act = + (float)_aggressivePolicy.MaxAllowedSpeed(VesselState.CoM - MainBody.position, VesselState.surfaceVelocity); } } else { // last 200 meters: - core.thrust.trans_spd_act = -Mathf.Lerp(0, - (float)Math.Sqrt((vesselState.limitedMaxThrustAccel - vesselState.localg) * 2 * 200) * 0.90F, (float)minalt / 200); + Core.thrust.trans_spd_act = -Mathf.Lerp(0, + (float)Math.Sqrt((VesselState.limitedMaxThrustAccel - VesselState.localg) * 2 * 200) * 0.90F, (float)minalt / 200); // take into account desired landing speed: - core.thrust.trans_spd_act = (float)Math.Min(-core.landing.touchdownSpeed, core.thrust.trans_spd_act); + Core.thrust.trans_spd_act = (float)Math.Min(-Core.landing.TouchdownSpeed, Core.thrust.trans_spd_act); // core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL; // core.thrust.trans_kill_h = true; // if (Math.Abs(Vector3d.Angle(-vessel.surfaceVelocity, vesselState.up)) < 10) - if (vesselState.speedSurfaceHorizontal < 5) + if (VesselState.speedSurfaceHorizontal < 5) { - // if we're falling more or less straight down, control vertical speed and + // if we're falling more or less straight down, control vertical speed and // kill horizontal velocity - core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL; - core.thrust.trans_kill_h = true; + Core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_VERTICAL; + Core.thrust.trans_kill_h = true; } else { // if we're falling at a significant angle from vertical, our vertical speed might be // quite small but we might still need to decelerate. Control the total speed instead // by thrusting directly retrograde - core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null); - core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE; - core.thrust.trans_spd_act *= -1; + Core.attitude.attitudeTo(Vector3d.back, AttitudeReference.SURFACE_VELOCITY, null); + Core.thrust.tmode = MechJebModuleThrustController.TMode.KEEP_SURFACE; + Core.thrust.trans_spd_act *= -1; } } - status = Localizer.Format("#MechJeb_LandingGuidance_Status9", - vesselState.altitudeBottom.ToString("F0")); //"Final descent: " + + "m above terrain" + Status = Localizer.Format("#MechJeb_LandingGuidance_Status9", + VesselState.altitudeBottom.ToString("F0")); //"Final descent: " + + "m above terrain" // ComputeCourseCorrection doesn't work close to the ground /* if (core.landing.landAtTarget) diff --git a/MechJeb2/LandingAutopilot/KillHorizontalVelocity.cs b/MechJeb2/LandingAutopilot/KillHorizontalVelocity.cs index e56df459..a47ed2c3 100644 --- a/MechJeb2/LandingAutopilot/KillHorizontalVelocity.cs +++ b/MechJeb2/LandingAutopilot/KillHorizontalVelocity.cs @@ -14,40 +14,40 @@ public KillHorizontalVelocity(MechJebCore core) : base(core) // TODO I think that this function could be better rewritten to much more agressively kill the horizontal velocity. At present on low gravity bodies such as Bop, the craft will hover and slowly drift sideways, loosing the prescion of the landing. public override AutopilotStep Drive(FlightCtrlState s) { - if (!core.landing.PredictionReady) + if (!Core.landing.PredictionReady) return this; - Vector3d horizontalPointingDirection = Vector3d.Exclude(vesselState.up, vesselState.forward).normalized; - if (Vector3d.Dot(horizontalPointingDirection, vesselState.surfaceVelocity) > 0) + Vector3d horizontalPointingDirection = Vector3d.Exclude(VesselState.up, VesselState.forward).normalized; + if (Vector3d.Dot(horizontalPointingDirection, VesselState.surfaceVelocity) > 0) { - core.thrust.targetThrottle = 0; - core.attitude.attitudeTo(Vector3.up, AttitudeReference.SURFACE_NORTH, core.landing); - return new FinalDescent(core); + Core.thrust.targetThrottle = 0; + Core.attitude.attitudeTo(Vector3.up, AttitudeReference.SURFACE_NORTH, Core.landing); + return new FinalDescent(Core); } //control thrust to control vertical speed: - const double desiredSpeed = 0; //hover until horizontal velocity is killed - double controlledSpeed = Vector3d.Dot(vesselState.surfaceVelocity, vesselState.up); - double speedError = desiredSpeed - controlledSpeed; - const double speedCorrectionTimeConstant = 1.0; - double desiredAccel = speedError / speedCorrectionTimeConstant; - double minAccel = -vesselState.localg; - double maxAccel = -vesselState.localg + Vector3d.Dot(vesselState.forward, vesselState.up) * vesselState.maxThrustAccel; + const double DESIRED_SPEED = 0; //hover until horizontal velocity is killed + double controlledSpeed = Vector3d.Dot(VesselState.surfaceVelocity, VesselState.up); + double speedError = DESIRED_SPEED - controlledSpeed; + const double SPEED_CORRECTION_TIME_CONSTANT = 1.0; + double desiredAccel = speedError / SPEED_CORRECTION_TIME_CONSTANT; + double minAccel = -VesselState.localg; + double maxAccel = -VesselState.localg + Vector3d.Dot(VesselState.forward, VesselState.up) * VesselState.maxThrustAccel; if (maxAccel - minAccel > 0) { - core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F); + Core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F); } else { - core.thrust.targetThrottle = 0; + Core.thrust.targetThrottle = 0; } //angle up and slightly away from vertical: - Vector3d desiredThrustVector = (vesselState.up + 0.2 * horizontalPointingDirection).normalized; + Vector3d desiredThrustVector = (VesselState.up + 0.2 * horizontalPointingDirection).normalized; - core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, core.landing); + Core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, Core.landing); - status = Localizer.Format("#MechJeb_LandingGuidance_Status10"); //"Killing horizontal velocity before final descent" + Status = Localizer.Format("#MechJeb_LandingGuidance_Status10"); //"Killing horizontal velocity before final descent" return this; } diff --git a/MechJeb2/LandingAutopilot/LowDeorbitBurn.cs b/MechJeb2/LandingAutopilot/LowDeorbitBurn.cs index 35933001..7ead0c87 100644 --- a/MechJeb2/LandingAutopilot/LowDeorbitBurn.cs +++ b/MechJeb2/LandingAutopilot/LowDeorbitBurn.cs @@ -10,13 +10,13 @@ namespace Landing { public class LowDeorbitBurn : AutopilotStep { - private bool deorbitBurnTriggered; - private double lowDeorbitBurnMaxThrottle; + private bool _deorbitBurnTriggered; + private double _lowDeorbitBurnMaxThrottle; - private bool lowDeorbitEndConditionSet; - private bool lowDeorbitEndOnLandingSiteNearer; + private bool _lowDeorbitEndConditionSet; + private bool _lowDeorbitEndOnLandingSiteNearer; - private const double lowDeorbitBurnTriggerFactor = 2; + private const double LOW_DEORBIT_BURN_TRIGGER_FACTOR = 2; public LowDeorbitBurn(MechJebCore core) : base(core) { @@ -24,13 +24,13 @@ public LowDeorbitBurn(MechJebCore core) : base(core) public override AutopilotStep Drive(FlightCtrlState s) { - if (deorbitBurnTriggered && core.attitude.attitudeAngleFromTarget() < 5) + if (_deorbitBurnTriggered && Core.attitude.attitudeAngleFromTarget() < 5) { - core.thrust.targetThrottle = Mathf.Clamp01((float)lowDeorbitBurnMaxThrottle); + Core.thrust.targetThrottle = Mathf.Clamp01((float)_lowDeorbitBurnMaxThrottle); } else { - core.thrust.targetThrottle = 0; + Core.thrust.targetThrottle = 0; } return this; @@ -39,98 +39,98 @@ public override AutopilotStep Drive(FlightCtrlState s) public override AutopilotStep OnFixedUpdate() { //Decide when we will start the deorbit burn: - double stoppingDistance = Math.Pow(vesselState.speedSurfaceHorizontal, 2) / (2 * vesselState.limitedMaxThrustAccel); - double triggerDistance = lowDeorbitBurnTriggerFactor * stoppingDistance; - double heightAboveTarget = vesselState.altitudeASL - core.landing.DecelerationEndAltitude(); + double stoppingDistance = Math.Pow(VesselState.speedSurfaceHorizontal, 2) / (2 * VesselState.limitedMaxThrustAccel); + double triggerDistance = LOW_DEORBIT_BURN_TRIGGER_FACTOR * stoppingDistance; + double heightAboveTarget = VesselState.altitudeASL - Core.landing.DecelerationEndAltitude(); if (triggerDistance < heightAboveTarget) { triggerDistance = heightAboveTarget; } //See if it's time to start the deorbit burn: - double rangeToTarget = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).magnitude; + double rangeToTarget = Vector3d.Exclude(VesselState.up, Core.target.GetPositionTargetPosition() - VesselState.CoM).magnitude; - if (!deorbitBurnTriggered && rangeToTarget < triggerDistance) + if (!_deorbitBurnTriggered && rangeToTarget < triggerDistance) { - if (!MuUtils.PhysicsRunning()) core.warp.MinimumWarp(true); - deorbitBurnTriggered = true; + if (!MuUtils.PhysicsRunning()) Core.warp.MinimumWarp(true); + _deorbitBurnTriggered = true; } - if (deorbitBurnTriggered) status = Localizer.Format("#MechJeb_LandingGuidance_Status11"); //"Executing low deorbit burn" - else status = Localizer.Format("#MechJeb_LandingGuidance_Status12"); //"Moving to low deorbit burn point" + Status = Localizer.Format(_deorbitBurnTriggered ? "#MechJeb_LandingGuidance_Status11" : //"Executing low deorbit burn" + "#MechJeb_LandingGuidance_Status12"); //"Moving to low deorbit burn point" //Warp toward deorbit burn if it hasn't been triggerd yet: - if (!deorbitBurnTriggered && core.node.autowarp && rangeToTarget > 2 * triggerDistance) - core.warp.WarpRegularAtRate((float)(orbit.period / 6)); - if (rangeToTarget < triggerDistance && !MuUtils.PhysicsRunning()) core.warp.MinimumWarp(); + if (!_deorbitBurnTriggered && Core.node.autowarp && rangeToTarget > 2 * triggerDistance) + Core.warp.WarpRegularAtRate((float)(Orbit.period / 6)); + if (rangeToTarget < triggerDistance && !MuUtils.PhysicsRunning()) Core.warp.MinimumWarp(); //By default, thrust straight back at max throttle - Vector3d thrustDirection = -vesselState.surfaceVelocity.normalized; - lowDeorbitBurnMaxThrottle = 1; + Vector3d thrustDirection = -VesselState.surfaceVelocity.normalized; + _lowDeorbitBurnMaxThrottle = 1; //If we are burning, we watch the predicted landing site and switch to the braking //burn when the predicted landing site crosses the target. We also use the predictions //to steer the predicted landing site toward the target - if (deorbitBurnTriggered && core.landing.PredictionReady) + if (_deorbitBurnTriggered && Core.landing.PredictionReady) { //angle slightly left or right to fix any cross-range error in the predicted landing site: - Vector3d horizontalToLandingSite = Vector3d.Exclude(vesselState.up, core.landing.LandingSite - vesselState.CoM).normalized; + Vector3d horizontalToLandingSite = Vector3d.Exclude(VesselState.up, Core.landing.LandingSite - VesselState.CoM).normalized; Vector3d horizontalToTarget = - Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).normalized; - const double angleGain = 4; - Vector3d angleCorrection = angleGain * (horizontalToTarget - horizontalToLandingSite); + Vector3d.Exclude(VesselState.up, Core.target.GetPositionTargetPosition() - VesselState.CoM).normalized; + const double ANGLE_GAIN = 4; + Vector3d angleCorrection = ANGLE_GAIN * (horizontalToTarget - horizontalToLandingSite); if (angleCorrection.magnitude > 0.1) angleCorrection *= 0.1 / angleCorrection.magnitude; thrustDirection = (thrustDirection + angleCorrection).normalized; - double rangeToLandingSite = Vector3d.Exclude(vesselState.up, core.landing.LandingSite - vesselState.CoM).magnitude; - double maxAllowedSpeed = core.landing.MaxAllowedSpeed(); + double rangeToLandingSite = Vector3d.Exclude(VesselState.up, Core.landing.LandingSite - VesselState.CoM).magnitude; + double maxAllowedSpeed = Core.landing.MaxAllowedSpeed(); - if (!lowDeorbitEndConditionSet && - Vector3d.Distance(core.landing.LandingSite, vesselState.CoM) < mainBody.Radius + vesselState.altitudeASL) + if (!_lowDeorbitEndConditionSet && + Vector3d.Distance(Core.landing.LandingSite, VesselState.CoM) < MainBody.Radius + VesselState.altitudeASL) { - lowDeorbitEndOnLandingSiteNearer = rangeToLandingSite > rangeToTarget; - lowDeorbitEndConditionSet = true; + _lowDeorbitEndOnLandingSiteNearer = rangeToLandingSite > rangeToTarget; + _lowDeorbitEndConditionSet = true; } - lowDeorbitBurnMaxThrottle = 1; + _lowDeorbitBurnMaxThrottle = 1; - if (orbit.PeA < 0) + if (Orbit.PeA < 0) { if (rangeToLandingSite > rangeToTarget) { - if (lowDeorbitEndConditionSet && !lowDeorbitEndOnLandingSiteNearer) + if (_lowDeorbitEndConditionSet && !_lowDeorbitEndOnLandingSiteNearer) { - core.thrust.targetThrottle = 0; - return new DecelerationBurn(core); + Core.thrust.targetThrottle = 0; + return new DecelerationBurn(Core); } - double maxAllowedSpeedAfterDt = core.landing.MaxAllowedSpeedAfterDt(vesselState.deltaT); - double speedAfterDt = vesselState.speedSurface + - vesselState.deltaT * Vector3d.Dot(vesselState.gravityForce, vesselState.surfaceVelocity.normalized); + double maxAllowedSpeedAfterDt = Core.landing.MaxAllowedSpeedAfterDt(VesselState.deltaT); + double speedAfterDt = VesselState.speedSurface + + VesselState.deltaT * Vector3d.Dot(VesselState.gravityForce, VesselState.surfaceVelocity.normalized); double throttleToMaintainLandingSite; - if (vesselState.speedSurface < maxAllowedSpeed) throttleToMaintainLandingSite = 0; + if (VesselState.speedSurface < maxAllowedSpeed) throttleToMaintainLandingSite = 0; else throttleToMaintainLandingSite = - (speedAfterDt - maxAllowedSpeedAfterDt) / (vesselState.deltaT * vesselState.maxThrustAccel); + (speedAfterDt - maxAllowedSpeedAfterDt) / (VesselState.deltaT * VesselState.maxThrustAccel); - lowDeorbitBurnMaxThrottle = throttleToMaintainLandingSite + 1 * (rangeToLandingSite / rangeToTarget - 1) + 0.2; + _lowDeorbitBurnMaxThrottle = throttleToMaintainLandingSite + 1 * (rangeToLandingSite / rangeToTarget - 1) + 0.2; } else { - if (lowDeorbitEndConditionSet && lowDeorbitEndOnLandingSiteNearer) + if (_lowDeorbitEndConditionSet && _lowDeorbitEndOnLandingSiteNearer) { - core.thrust.targetThrottle = 0; - return new DecelerationBurn(core); + Core.thrust.targetThrottle = 0; + return new DecelerationBurn(Core); } - lowDeorbitBurnMaxThrottle = 0; - status = Localizer.Format( + _lowDeorbitBurnMaxThrottle = 0; + Status = Localizer.Format( "#MechJeb_LandingGuidance_Status13"); //"Deorbit burn complete: waiting for the right moment to start braking" } } } - core.attitude.attitudeTo(thrustDirection, AttitudeReference.INERTIAL, core.landing); + Core.attitude.attitudeTo(thrustDirection, AttitudeReference.INERTIAL, Core.landing); return this; } diff --git a/MechJeb2/LandingAutopilot/PlaneChange.cs b/MechJeb2/LandingAutopilot/PlaneChange.cs index b6639ef3..73450792 100644 --- a/MechJeb2/LandingAutopilot/PlaneChange.cs +++ b/MechJeb2/LandingAutopilot/PlaneChange.cs @@ -9,8 +9,8 @@ namespace Landing { public class PlaneChange : AutopilotStep { - private bool planeChangeTriggered; - private double planeChangeDVLeft; + private bool _planeChangeTriggered; + private double _planeChangeDVLeft; public PlaneChange(MechJebCore core) : base(core) { @@ -20,28 +20,28 @@ public PlaneChange(MechJebCore core) : base(core) private Vector3d ComputePlaneChange() { Vector3d targetRadialVector = - core.vessel.mainBody.GetWorldSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0) - mainBody.position; - Vector3d currentRadialVector = core.vesselState.CoM - core.vessel.mainBody.position; + Core.vessel.mainBody.GetWorldSurfacePosition(Core.target.targetLatitude, Core.target.targetLongitude, 0) - MainBody.position; + Vector3d currentRadialVector = Core.vesselState.CoM - Core.vessel.mainBody.position; double angleToTarget = Vector3d.Angle(targetRadialVector, currentRadialVector); //this calculation seems like it might be be working right: - double timeToTarget = orbit.TimeOfTrueAnomaly(core.vessel.orbit.trueAnomaly * UtilMath.Rad2Deg + angleToTarget, vesselState.time) - - vesselState.time; - double planetRotationAngle = 360 * timeToTarget / mainBody.rotationPeriod; - var planetRotation = Quaternion.AngleAxis((float)planetRotationAngle, mainBody.angularVelocity); + double timeToTarget = Orbit.TimeOfTrueAnomaly(Core.vessel.orbit.trueAnomaly * UtilMath.Rad2Deg + angleToTarget, VesselState.time) - + VesselState.time; + double planetRotationAngle = 360 * timeToTarget / MainBody.rotationPeriod; + var planetRotation = Quaternion.AngleAxis((float)planetRotationAngle, MainBody.angularVelocity); Vector3d targetRadialVectorOnFlyover = planetRotation * targetRadialVector; - Vector3d horizontalToTarget = Vector3d.Exclude(vesselState.up, targetRadialVectorOnFlyover - currentRadialVector).normalized; + Vector3d horizontalToTarget = Vector3d.Exclude(VesselState.up, targetRadialVectorOnFlyover - currentRadialVector).normalized; return horizontalToTarget; } public override AutopilotStep Drive(FlightCtrlState s) { - if (planeChangeTriggered && core.attitude.attitudeAngleFromTarget() < 2) + if (_planeChangeTriggered && Core.attitude.attitudeAngleFromTarget() < 2) { - core.thrust.targetThrottle = Mathf.Clamp01((float)(planeChangeDVLeft / (2 * core.vesselState.maxThrustAccel))); + Core.thrust.targetThrottle = Mathf.Clamp01((float)(_planeChangeDVLeft / (2 * Core.vesselState.maxThrustAccel))); } else { - core.thrust.targetThrottle = 0; + Core.thrust.targetThrottle = 0; } return this; @@ -49,41 +49,41 @@ public override AutopilotStep Drive(FlightCtrlState s) public override AutopilotStep OnFixedUpdate() { - Vector3d targetRadialVector = mainBody.GetWorldSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0) - - mainBody.position; - Vector3d currentRadialVector = vesselState.CoM - mainBody.position; + Vector3d targetRadialVector = MainBody.GetWorldSurfacePosition(Core.target.targetLatitude, Core.target.targetLongitude, 0) - + MainBody.position; + Vector3d currentRadialVector = VesselState.CoM - MainBody.position; double angleToTarget = Vector3d.Angle(targetRadialVector, currentRadialVector); - bool approaching = Vector3d.Dot(targetRadialVector - currentRadialVector, vesselState.orbitalVelocity) > 0; + bool approaching = Vector3d.Dot(targetRadialVector - currentRadialVector, VesselState.orbitalVelocity) > 0; - if (!planeChangeTriggered && approaching && angleToTarget > 80 && angleToTarget < 90) + if (!_planeChangeTriggered && approaching && angleToTarget > 80 && angleToTarget < 90) { - if (!MuUtils.PhysicsRunning()) core.warp.MinimumWarp(true); - planeChangeTriggered = true; + if (!MuUtils.PhysicsRunning()) Core.warp.MinimumWarp(true); + _planeChangeTriggered = true; } - if (planeChangeTriggered) + if (_planeChangeTriggered) { Vector3d horizontalToTarget = ComputePlaneChange(); - Vector3d finalVelocity = Quaternion.FromToRotation(vesselState.horizontalOrbit, horizontalToTarget) * vesselState.orbitalVelocity; + Vector3d finalVelocity = Quaternion.FromToRotation(VesselState.horizontalOrbit, horizontalToTarget) * VesselState.orbitalVelocity; - Vector3d deltaV = finalVelocity - vesselState.orbitalVelocity; + Vector3d deltaV = finalVelocity - VesselState.orbitalVelocity; //burn normal+ or normal- to avoid dropping the Pe: - var burnDir = Vector3d.Exclude(vesselState.up, Vector3d.Exclude(vesselState.orbitalVelocity, deltaV)); - planeChangeDVLeft = UtilMath.Deg2Rad * Vector3d.Angle(finalVelocity, vesselState.orbitalVelocity) * - vesselState.speedOrbitHorizontal; - core.attitude.attitudeTo(burnDir, AttitudeReference.INERTIAL, core.landing); - status = Localizer.Format("#MechJeb_LandingGuidance_Status14", - planeChangeDVLeft.ToString("F0")); //"Executing low orbit plane change of about " + + " m/s" + var burnDir = Vector3d.Exclude(VesselState.up, Vector3d.Exclude(VesselState.orbitalVelocity, deltaV)); + _planeChangeDVLeft = UtilMath.Deg2Rad * Vector3d.Angle(finalVelocity, VesselState.orbitalVelocity) * + VesselState.speedOrbitHorizontal; + Core.attitude.attitudeTo(burnDir, AttitudeReference.INERTIAL, Core.landing); + Status = Localizer.Format("#MechJeb_LandingGuidance_Status14", + _planeChangeDVLeft.ToString("F0")); //"Executing low orbit plane change of about " + + " m/s" - if (planeChangeDVLeft < 0.1F) + if (_planeChangeDVLeft < 0.1F) { - return new LowDeorbitBurn(core); + return new LowDeorbitBurn(Core); } } else { - if (core.node.autowarp) core.warp.WarpRegularAtRate((float)(orbit.period / 6)); - status = Localizer.Format("#MechJeb_LandingGuidance_Status15"); //"Moving to low orbit plane change burn point" + if (Core.node.autowarp) Core.warp.WarpRegularAtRate((float)(Orbit.period / 6)); + Status = Localizer.Format("#MechJeb_LandingGuidance_Status15"); //"Moving to low orbit plane change burn point" } return this; diff --git a/MechJeb2/LandingAutopilot/UntargetedDeorbit.cs b/MechJeb2/LandingAutopilot/UntargetedDeorbit.cs index 6aa56c35..52989b03 100644 --- a/MechJeb2/LandingAutopilot/UntargetedDeorbit.cs +++ b/MechJeb2/LandingAutopilot/UntargetedDeorbit.cs @@ -12,19 +12,16 @@ public UntargetedDeorbit(MechJebCore core) : base(core) public override AutopilotStep Drive(FlightCtrlState s) { - if (orbit.PeA < -0.1 * mainBody.Radius) + if (Orbit.PeA < -0.1 * MainBody.Radius) { - core.thrust.targetThrottle = 0; - return new FinalDescent(core); + Core.thrust.targetThrottle = 0; + return new FinalDescent(Core); } - core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT_HORIZONTAL, core.landing); - if (core.attitude.attitudeAngleFromTarget() < 5) - core.thrust.targetThrottle = 1; - else - core.thrust.targetThrottle = 0; + Core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT_HORIZONTAL, Core.landing); + Core.thrust.targetThrottle = Core.attitude.attitudeAngleFromTarget() < 5 ? 1 : 0; - status = Localizer.Format("#MechJeb_LandingGuidance_Status16"); //"Doing deorbit burn." + Status = Localizer.Format("#MechJeb_LandingGuidance_Status16"); //"Doing deorbit burn." return this; } diff --git a/MechJeb2/MechJebModuleLandingAutopilot.cs b/MechJeb2/MechJebModuleLandingAutopilot.cs index 46863bfb..1c83a45a 100644 --- a/MechJeb2/MechJebModuleLandingAutopilot.cs +++ b/MechJeb2/MechJebModuleLandingAutopilot.cs @@ -11,41 +11,41 @@ namespace MuMech // - Add a parameter to define how steep the descent will be ( fraction of Orbit ? ) // - Fix the auto wrap stop start dance // - Replace the openGL code with a LineRenderer - // - + // - [UsedImplicitly] public class MechJebModuleLandingAutopilot : AutopilotModule { - private bool deployedGears; - public bool landAtTarget; + private bool _deployedGears; + public bool LandAtTarget; [Persistent(pass = (int)(Pass.Local | Pass.Type | Pass.Global))] - public EditableDouble touchdownSpeed = 0.5; + public readonly EditableDouble TouchdownSpeed = 0.5; [Persistent(pass = (int)(Pass.Local | Pass.Type | Pass.Global))] - public bool deployGears = true; + public bool DeployGears = true; [Persistent(pass = (int)(Pass.Local | Pass.Type | Pass.Global))] - public EditableInt limitGearsStage = 0; + public readonly EditableInt LimitGearsStage = 0; [Persistent(pass = (int)(Pass.Local | Pass.Type | Pass.Global))] - public bool deployChutes = true; + public bool DeployChutes = true; [Persistent(pass = (int)(Pass.Local | Pass.Type | Pass.Global))] - public EditableInt limitChutesStage = 0; + public readonly EditableInt LimitChutesStage = 0; [Persistent(pass = (int)(Pass.Local | Pass.Type | Pass.Global))] - public bool rcsAdjustment = true; + public bool RCSAdjustment = true; // This is used to adjust the height at which the parachutes semi deploy as a means of // targeting the landing in an atmosphere where it is not possible to control atitude // to perform course correction burns. - private ParachutePlan parachutePlan; + private ParachutePlan _parachutePlan; //Landing prediction data: - private MechJebModuleLandingPredictions predictor; - public ReentrySimulation.Result Prediction => predictor.Result; + private MechJebModuleLandingPredictions _predictor; + public ReentrySimulation.Result Prediction => _predictor.Result; - private ReentrySimulation.Result ErrorPrediction => predictor.GetErrorResult(); + private ReentrySimulation.Result _errorPrediction => _predictor.GetErrorResult(); public bool PredictionReady //We shouldn't do any autopilot stuff until this is true { @@ -57,59 +57,52 @@ public class MechJebModuleLandingAutopilot : AutopilotModule return false; } - if (Prediction.Outcome != ReentrySimulation.Outcome.LANDED) - { - return false; - } - - return true; + return Prediction.Outcome == ReentrySimulation.Outcome.LANDED; } } - private bool ErrorPredictionReady => - ErrorPrediction != null && - ErrorPrediction.Outcome == ReentrySimulation.Outcome.LANDED; // We shouldn't try to use an ErrorPrediction until this is true. + // We shouldn't try to use an ErrorPrediction until this is true. + private bool _errorPredictionReady => _errorPrediction is { Outcome: ReentrySimulation.Outcome.LANDED }; - public double LandingAltitude // The altitude above sea level of the terrain at the landing site + private double _landingAltitude // The altitude above sea level of the terrain at the landing site { get { - if (PredictionReady) + if (!PredictionReady) return 0; + + // Although we know the landingASL as it is in the prediction, we suspect that + // it might sometimes be incorrect. So to check we will calculate it again here, + // and if the two differ log an error. It seems that this terrain ASL calls when + // made from the simulatiuon thread are regularly incorrect, but are OK when made + // from this thread. At the time of writting (KSP0.23) there seem to be several + // other things going wrong with he terrain system, such as visual glitches as + // we as the occasional exceptions being thrown when calls to the CelestialBody + // object are made. I suspect a bug or some sort - for now this hack improves + // the landing results. { - // Although we know the landingASL as it is in the prediction, we suspect that - // it might sometimes be incorrect. So to check we will calculate it again here, - // and if the two differ log an error. It seems that this terrain ASL calls when - // made from the simulatiuon thread are regularly incorrect, but are OK when made - // from this thread. At the time of writting (KSP0.23) there seem to be several - // other things going wrong with he terrain system, such as visual glitches as - // we as the occasional exceptions being thrown when calls to the CelestialBody - // object are made. I suspect a bug or some sort - for now this hack improves - // the landing results. + double checkASL = Prediction.Body.TerrainAltitude(Prediction.EndPosition.Latitude, Prediction.EndPosition.Longitude); + // ReSharper disable once CompareOfFloatsByEqualityOperator + // ReSharper disable once RedundantCheckBeforeAssignment + if (checkASL != Prediction.EndASL) { - double checkASL = Prediction.Body.TerrainAltitude(Prediction.EndPosition.Latitude, Prediction.EndPosition.Longitude); - if (checkASL != Prediction.EndASL) - { - // I know that this check is not required as we might as well always make - // the asignment. However this allows for some debug monitoring of how often this is occuring. - Prediction.EndASL = checkASL; - } + // I know that this check is not required as we might as well always make + // the asignment. However this allows for some debug monitoring of how often this is occuring. + Prediction.EndASL = checkASL; } - - return Prediction.EndASL; } - return 0; + return Prediction.EndASL; } } public Vector3d LandingSite => mainBody.GetWorldSurfacePosition(Prediction.EndPosition.Latitude, - Prediction.EndPosition.Longitude, LandingAltitude) - mainBody.position; // The current position of the landing site + Prediction.EndPosition.Longitude, _landingAltitude) - mainBody.position; // The current position of the landing site - private Vector3d RotatedLandingSite => Prediction.WorldEndPosition(); // The position where the landing site will be when we land at it + private Vector3d _rotatedLandingSite => Prediction.WorldEndPosition(); // The position where the landing site will be when we land at it - public IDescentSpeedPolicy descentSpeedPolicy; - public double vesselAverageDrag; + public IDescentSpeedPolicy DescentSpeedPolicy; + private double _vesselAverageDrag; public MechJebModuleLandingAutopilot(MechJebCore core) : base(core) @@ -118,44 +111,44 @@ public MechJebModuleLandingAutopilot(MechJebCore core) public override void OnStart(PartModule.StartState state) { - predictor = core.GetComputerModule(); + _predictor = core.GetComputerModule(); } //public interface: public void LandAtPositionTarget(object controller) { - landAtTarget = true; + LandAtTarget = true; users.Add(controller); - predictor.users.Add(this); + _predictor.users.Add(this); vessel.RemoveAllManeuverNodes(); // For the benefit of the landing predictions module - deployedGears = false; + _deployedGears = false; // Create a new parachute plan - parachutePlan = new ParachutePlan(this); - parachutePlan.StartPlanning(); + _parachutePlan = new ParachutePlan(this); + _parachutePlan.StartPlanning(); if (orbit.PeA < 0) - setStep(new CourseCorrection(core)); + SetStep(new CourseCorrection(core)); else if (UseLowDeorbitStrategy()) - setStep(new PlaneChange(core)); + SetStep(new PlaneChange(core)); else - setStep(new DeorbitBurn(core)); + SetStep(new DeorbitBurn(core)); } public void LandUntargeted(object controller) { - landAtTarget = false; + LandAtTarget = false; users.Add(controller); - deployedGears = false; + _deployedGears = false; // Create a new parachute plan - parachutePlan = new ParachutePlan(this); - parachutePlan.StartPlanning(); + _parachutePlan = new ParachutePlan(this); + _parachutePlan.StartPlanning(); - setStep(new UntargetedDeorbit(core)); + SetStep(new UntargetedDeorbit(core)); } public void StopLanding() @@ -163,26 +156,26 @@ public void StopLanding() users.Clear(); core.thrust.ThrustOff(); core.thrust.users.Remove(this); - if (core.landing.rcsAdjustment) + if (core.landing.RCSAdjustment) core.rcs.enabled = false; - setStep(null); + SetStep(null); } public override void Drive(FlightCtrlState s) { - if (!active) + if (!Active) return; - descentSpeedPolicy = PickDescentSpeedPolicy(); + DescentSpeedPolicy = PickDescentSpeedPolicy(); - predictor.descentSpeedPolicy = PickDescentSpeedPolicy(); //create a separate IDescentSpeedPolicy object for the simulation - predictor.decelEndAltitudeASL = DecelerationEndAltitude(); - predictor.parachuteSemiDeployMultiplier = parachutePlan.Multiplier; + _predictor.descentSpeedPolicy = PickDescentSpeedPolicy(); //create a separate IDescentSpeedPolicy object for the simulation + _predictor.decelEndAltitudeASL = DecelerationEndAltitude(); + _predictor.parachuteSemiDeployMultiplier = _parachutePlan.Multiplier; // Consider lowering the langing gear { double minalt = Math.Min(vesselState.altitudeBottom, Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue)); - if (deployGears && !deployedGears && minalt < 1000) + if (DeployGears && !_deployedGears && minalt < 1000) DeployLandingGears(); } @@ -191,7 +184,7 @@ public override void Drive(FlightCtrlState s) public override void OnFixedUpdate() { - vesselAverageDrag = VesselAverageDrag(); + _vesselAverageDrag = VesselAverageDrag(); base.OnFixedUpdate(); DeployParachutes(); } @@ -205,13 +198,13 @@ public override void OnModuleEnabled() public override void OnModuleDisabled() { core.attitude.attitudeDeactivate(); - predictor.users.Remove(this); - predictor.descentSpeedPolicy = null; + _predictor.users.Remove(this); + _predictor.descentSpeedPolicy = null; core.thrust.ThrustOff(); core.thrust.users.Remove(this); - if (core.landing.rcsAdjustment) + if (core.landing.RCSAdjustment) core.rcs.enabled = false; - setStep(null); + SetStep(null); } // Estimate the delta-V of the correction burn that would be required to put us on @@ -219,7 +212,7 @@ public override void OnModuleDisabled() public Vector3d ComputeCourseCorrection(bool allowPrograde) { // actualLandingPosition is the predicted actual landing position - Vector3d actualLandingPosition = RotatedLandingSite - mainBody.position; + Vector3d actualLandingPosition = _rotatedLandingSite - mainBody.position; // orbitLandingPosition is the point where our current orbit intersects the planet double endRadius = mainBody.Radius + DecelerationEndAltitude() - 100; @@ -228,11 +221,9 @@ public Vector3d ComputeCourseCorrection(bool allowPrograde) if (endRadius > orbit.ApR || vessel.LandedOrSplashed) StopLanding(); - Vector3d orbitLandingPosition; - if (orbit.PeR < endRadius) - orbitLandingPosition = orbit.WorldBCIPositionAtUT(orbit.NextTimeOfRadius(vesselState.time, endRadius)); - else - orbitLandingPosition = orbit.WorldBCIPositionAtUT(orbit.NextPeriapsisTime(vesselState.time)); + Vector3d orbitLandingPosition = orbit.WorldBCIPositionAtUT( + orbit.PeR < endRadius ? orbit.NextTimeOfRadius(vesselState.time, endRadius) : orbit.NextPeriapsisTime(vesselState.time) + ); // convertOrbitToActual is a rotation that rotates orbitLandingPosition on actualLandingPosition var convertOrbitToActual = Quaternion.FromToRotation(orbitLandingPosition, actualLandingPosition); @@ -249,24 +240,29 @@ public Vector3d ComputeCourseCorrection(bool allowPrograde) var deltas = new Vector3d[3]; for (int i = 0; i < 3; i++) { - const double - perturbationDeltaV = - 1; //warning: hard experience shows that setting this too low leads to bewildering bugs due to finite precision of Orbit functions + //warning: hard experience shows that setting this too low leads to bewildering bugs due to finite precision of Orbit functions + const double PERTURBATION_DELTA_V = 1; + Orbit perturbedOrbit = - orbit.PerturbedOrbit(vesselState.time, perturbationDeltaV * perturbationDirections[i]); //compute the perturbed orbit - double perturbedLandingTime; - if (perturbedOrbit.PeR < endRadius) perturbedLandingTime = perturbedOrbit.NextTimeOfRadius(vesselState.time, endRadius); - else perturbedLandingTime = perturbedOrbit.NextPeriapsisTime(vesselState.time); + orbit.PerturbedOrbit(vesselState.time, PERTURBATION_DELTA_V * perturbationDirections[i]); //compute the perturbed orbit + + double perturbedLandingTime = perturbedOrbit.PeR < endRadius + ? perturbedOrbit.NextTimeOfRadius(vesselState.time, endRadius) + : perturbedOrbit.NextPeriapsisTime(vesselState.time); + Vector3d perturbedLandingPosition = perturbedOrbit.WorldBCIPositionAtUT(perturbedLandingTime); //find where it hits the planet - Vector3d - landingDelta = perturbedLandingPosition - - orbitLandingPosition; //find the difference between that and the original orbit's intersection point - landingDelta = convertOrbitToActual * - landingDelta; //rotate that difference vector so that we can now think of it as starting at the actual landing position - landingDelta = Vector3d.Exclude(actualLandingPosition, - landingDelta); //project the difference vector onto the plane tangent to the actual landing position - deltas[i] = landingDelta / - perturbationDeltaV; //normalize by the delta-V considered, so that deltas now has units of meters per (meter/second) [i.e., seconds] + + //find the difference between that and the original orbit's intersection point + Vector3d landingDelta = perturbedLandingPosition - orbitLandingPosition; + + //rotate that difference vector so that we can now think of it as starting at the actual landing position + landingDelta = convertOrbitToActual * landingDelta; + + //project the difference vector onto the plane tangent to the actual landing position + landingDelta = Vector3d.Exclude(actualLandingPosition, landingDelta); + + //normalize by the delta-V considered, so that deltas now has units of meters per (meter/second) [i.e., seconds] + deltas[i] = landingDelta / PERTURBATION_DELTA_V; } // Now deltas stores the predicted offsets in landing position produced by each of the three perturbations. @@ -312,14 +308,14 @@ const double // Now solve a 2x2 system of linear equations to determine the linear combination // of perturbationDirection01 and normal+ that will give the desired offset in the // predicted landing position. - var A = new Matrix2x2( + var a = new Matrix2x2( downrangeDelta.sqrMagnitude, Vector3d.Dot(downrangeDelta, deltas[2]), Vector3d.Dot(downrangeDelta, deltas[2]), deltas[2].sqrMagnitude ); var b = new Vector2d(Vector3d.Dot(desiredDelta, downrangeDelta), Vector3d.Dot(desiredDelta, deltas[2])); - Vector2d coeffs = A.inverse() * b; + Vector2d coeffs = a.inverse() * b; Vector3d courseCorrection = coeffs.x * downrangeDirection + coeffs.y * perturbationDirections[2]; @@ -329,53 +325,49 @@ const double public void ControlParachutes() { // Firstly - do we have a parachute plan? If not then we had better get one quick! - if (null == parachutePlan) - { - parachutePlan = new ParachutePlan(this); - } + _parachutePlan ??= new ParachutePlan(this); // Are there any deployable parachute? If not then there is no point in us being here. Let's switch to cruising to the deceleration burn instead. if (!ParachutesDeployable()) { - predictor.runErrorSimulations = false; - parachutePlan.ClearData(); + _predictor.runErrorSimulations = false; + _parachutePlan.ClearData(); return; } - predictor.runErrorSimulations = true; + _predictor.runErrorSimulations = true; // Is there an error prediction available? If so add that into the mix - if (ErrorPredictionReady && !double.IsNaN(ErrorPrediction.ParachuteMultiplier)) + if (_errorPredictionReady && !double.IsNaN(_errorPrediction.ParachuteMultiplier)) { - parachutePlan.AddResult(ErrorPrediction); + _parachutePlan.AddResult(_errorPrediction); } // Has the Landing prediction been updated? If so then we can use the result to refine our parachute plan. if (PredictionReady && !double.IsNaN(Prediction.ParachuteMultiplier)) { - parachutePlan.AddResult(Prediction); + _parachutePlan.AddResult(Prediction); } } private void DeployParachutes() { - if (vesselState.mainBody.atmosphere && deployChutes) + if (!(vesselState.mainBody.atmosphere && DeployChutes)) return; + + for (int i = 0; i < vesselState.parachutes.Count; i++) { - for (int i = 0; i < vesselState.parachutes.Count; i++) - { - ModuleParachute p = vesselState.parachutes[i]; - // what is the ASL at which we should deploy this parachute? It is the actual deployment height above the surface + the ASL of the predicted landing point. - double LandingSiteASL = LandingAltitude; - double ParachuteDeployAboveGroundAtLandingSite = p.deployAltitude * parachutePlan.Multiplier; + ModuleParachute p = vesselState.parachutes[i]; + // what is the ASL at which we should deploy this parachute? It is the actual deployment height above the surface + the ASL of the predicted landing point. + double landingSiteASL = _landingAltitude; + double parachuteDeployAboveGroundAtLandingSite = p.deployAltitude * _parachutePlan.Multiplier; - double ASLDeployAltitude = ParachuteDeployAboveGroundAtLandingSite + LandingSiteASL; + double aslDeployAltitude = parachuteDeployAboveGroundAtLandingSite + landingSiteASL; - if (p.part.inverseStage >= limitChutesStage && p.deploymentState == ModuleParachute.deploymentStates.STOWED && - ASLDeployAltitude > vesselState.altitudeASL && p.deploymentSafeState == ModuleParachute.deploymentSafeStates.SAFE) - { - p.Deploy(); - //Debug.Log("Deploying parachute " + p.name + " at " + ASLDeployAltitude + ". (" + LandingSiteASL + " + " + ParachuteDeployAboveGroundAtLandingSite +")"); - } + if (p.part.inverseStage >= LimitChutesStage && p.deploymentState == ModuleParachute.deploymentStates.STOWED && + aslDeployAltitude > vesselState.altitudeASL && p.deploymentSafeState == ModuleParachute.deploymentSafeStates.SAFE) + { + p.Deploy(); + //Debug.Log("Deploying parachute " + p.name + " at " + ASLDeployAltitude + ". (" + LandingSiteASL + " + " + ParachuteDeployAboveGroundAtLandingSite +")"); } } } @@ -384,12 +376,12 @@ private void DeployParachutes() public bool ParachutesDeployable() { if (!vesselState.mainBody.atmosphere) return false; - if (!deployChutes) return false; + if (!DeployChutes) return false; for (int i = 0; i < vesselState.parachutes.Count; i++) { ModuleParachute p = vesselState.parachutes[i]; - if (Math.Max(p.part.inverseStage, 0) >= limitChutesStage && p.deploymentState == ModuleParachute.deploymentStates.STOWED) + if (Math.Max(p.part.inverseStage, 0) >= LimitChutesStage && p.deploymentState == ModuleParachute.deploymentStates.STOWED) { return true; } @@ -412,7 +404,7 @@ private void DeployLandingGears() if (p.HasModule()) { // p.inverseStage is -1 for some configuration ?!? - if (Math.Max(p.inverseStage, 0) >= limitGearsStage) + if (Math.Max(p.inverseStage, 0) >= LimitGearsStage) { foreach (ModuleWheelDeployment wd in p.FindModulesImplementing()) { @@ -423,7 +415,7 @@ private void DeployLandingGears() } } - deployedGears = true; + _deployedGears = true; } private IDescentSpeedPolicy PickDescentSpeedPolicy() @@ -439,21 +431,18 @@ private IDescentSpeedPolicy PickDescentSpeedPolicy() public double DecelerationEndAltitude() { - if (UseAtmosphereToBrake()) - { - // if the atmosphere is thick, deceleration (meaning freefall through the atmosphere) - // should end a safe height above the landing site in order to allow braking from terminal velocity - // FIXME: Drag Length is quite large now without parachutes, check this better - double landingSiteDragLength = mainBody.DragLength(LandingAltitude, vesselAverageDrag + ParachuteAddedDragCoef(), vesselState.mass); - - //MechJebCore.print("DecelerationEndAltitude Atmo " + (2 * landingSiteDragLength + LandingAltitude).ToString("F2")); - return 1.1 * landingSiteDragLength + LandingAltitude; - } - //if the atmosphere is thin, the deceleration burn should end //500 meters above the landing site to allow for a controlled final descent //MechJebCore.print("DecelerationEndAltitude Vacum " + (500 + LandingAltitude).ToString("F2")); - return 500 + LandingAltitude; + if (!UseAtmosphereToBrake()) return 500 + _landingAltitude; + + // if the atmosphere is thick, deceleration (meaning freefall through the atmosphere) + // should end a safe height above the landing site in order to allow braking from terminal velocity + // FIXME: Drag Length is quite large now without parachutes, check this better + double landingSiteDragLength = mainBody.DragLength(_landingAltitude, _vesselAverageDrag + ParachuteAddedDragCoef(), vesselState.mass); + + //MechJebCore.print("DecelerationEndAltitude Atmo " + (2 * landingSiteDragLength + LandingAltitude).ToString("F2")); + return 1.1 * landingSiteDragLength + _landingAltitude; } //On planets with thick enough atmospheres, we shouldn't do a deceleration burn. Rather, @@ -464,26 +453,21 @@ public double DecelerationEndAltitude() //expect to get slowed to near terminal velocity before impacting the ground. public bool UseAtmosphereToBrake() { - double landingSiteDragLength = mainBody.DragLength(LandingAltitude, vesselAverageDrag + ParachuteAddedDragCoef(), vesselState.mass); + double landingSiteDragLength = mainBody.DragLength(_landingAltitude, _vesselAverageDrag + ParachuteAddedDragCoef(), vesselState.mass); //if (mainBody.RealMaxAtmosphereAltitude() > 0 && (ParachutesDeployable() || ParachutesDeployed())) - if (mainBody.RealMaxAtmosphereAltitude() > 0 && - landingSiteDragLength < 0.7 * mainBody.RealMaxAtmosphereAltitude()) // the ratio is totally arbitrary until I get something better - { - return true; - } - - return false; + return mainBody.RealMaxAtmosphereAltitude() > 0 && + landingSiteDragLength < 0.7 * mainBody.RealMaxAtmosphereAltitude(); // the ratio is totally arbitrary until I get something better } // Get an average drag for the whole vessel. Far from precise but fast. - public double VesselAverageDrag() + private double VesselAverageDrag() { float dragCoef = 0; for (int i = 0; i < vessel.parts.Count; i++) { - Part part = vessel.parts[i]; - if (part.DragCubes.None || part.ShieldedFromAirstream) + Part p = vessel.parts[i]; + if (p.DragCubes.None || p.ShieldedFromAirstream) { continue; } @@ -492,8 +476,8 @@ public double VesselAverageDrag() float partAreaDrag = 0; for (int f = 0; f < 6; f++) { - partAreaDrag = part.DragCubes.WeightedDrag[f] * - part.DragCubes.AreaOccluded[f]; // * PhysicsGlobals.DragCurveValue(0.5, machNumber) but I ll assume it is 1 for now + partAreaDrag = p.DragCubes.WeightedDrag[f] * + p.DragCubes.AreaOccluded[f]; // * PhysicsGlobals.DragCurveValue(0.5, machNumber) but I ll assume it is 1 for now } dragCoef += partAreaDrag / 6; @@ -503,35 +487,33 @@ public double VesselAverageDrag() } // This is not the exact number, but it's good enough for our use - public double ParachuteAddedDragCoef() + private double ParachuteAddedDragCoef() { double addedDragCoef = 0; - if (vesselState.mainBody.atmosphere && deployChutes) + if (!vesselState.mainBody.atmosphere || !DeployChutes) return addedDragCoef * PhysicsGlobals.DragCubeMultiplier; + + for (int i = 0; i < vesselState.parachutes.Count; i++) { - for (int i = 0; i < vesselState.parachutes.Count; i++) - { - ModuleParachute p = vesselState.parachutes[i]; - if (p.part.inverseStage >= limitChutesStage) - { - //addedDragMass += p.part.DragCubes.Cubes.Where(c => c.Name == "DEPLOYED").m + ModuleParachute p = vesselState.parachutes[i]; + if (p.part.inverseStage < LimitChutesStage) continue; - float maxCoef = 0; - for (int c = 0; c < p.part.DragCubes.Cubes.Count; c++) - { - DragCube dragCube = p.part.DragCubes.Cubes[c]; - if (dragCube.Name != "DEPLOYED") - continue; - - for (int f = 0; f < 6; f++) - { - // we only want the additional coef from going fully deployed - maxCoef = Mathf.Max(maxCoef, p.part.DragCubes.WeightedDrag[f] - dragCube.Weight * dragCube.Drag[f]); - } - } + //addedDragMass += p.part.DragCubes.Cubes.Where(c => c.Name == "DEPLOYED").m - addedDragCoef += maxCoef; + float maxCoef = 0; + for (int c = 0; c < p.part.DragCubes.Cubes.Count; c++) + { + DragCube dragCube = p.part.DragCubes.Cubes[c]; + if (dragCube.Name != "DEPLOYED") + continue; + + for (int f = 0; f < 6; f++) + { + // we only want the additional coef from going fully deployed + maxCoef = Mathf.Max(maxCoef, p.part.DragCubes.WeightedDrag[f] - dragCube.Weight * dragCube.Drag[f]); } } + + addedDragCoef += maxCoef; } return addedDragCoef * PhysicsGlobals.DragCubeMultiplier; @@ -549,103 +531,100 @@ private bool UseLowDeorbitStrategy() public double MaxAllowedSpeed() { - return descentSpeedPolicy.MaxAllowedSpeed(vesselState.CoM - mainBody.position, vesselState.surfaceVelocity); + return DescentSpeedPolicy.MaxAllowedSpeed(vesselState.CoM - mainBody.position, vesselState.surfaceVelocity); } public double MaxAllowedSpeedAfterDt(double dt) { - return descentSpeedPolicy.MaxAllowedSpeed(vesselState.CoM + vesselState.orbitalVelocity * dt - mainBody.position, + return DescentSpeedPolicy.MaxAllowedSpeed(vesselState.CoM + vesselState.orbitalVelocity * dt - mainBody.position, vesselState.surfaceVelocity + dt * vesselState.gravityForce); } [ValueInfoItem("#MechJeb_ParachuteControlInfo", InfoItem.Category.Misc, showInEditor = false)] //ParachuteControlInfo public string ParachuteControlInfo() { - if (ParachutesDeployable()) - { - string retVal = Localizer.Format("#MechJeb_ChuteMultiplier", parachutePlan.Multiplier.ToString("F7")); //"'Chute Multiplier: " + - retVal += Localizer.Format("#MechJeb_MultiplierQuality", - parachutePlan.MultiplierQuality.ToString("F1")); //"\nMultiplier Quality: " + + "%" - retVal += Localizer.Format("#MechJeb_Usingpredictions", parachutePlan.MultiplierDataAmount); //"\nUsing " + + " predictions" + if (!ParachutesDeployable()) return "N/A"; - return retVal; - } + string retVal = Localizer.Format("#MechJeb_ChuteMultiplier", _parachutePlan.Multiplier.ToString("F7")); //"'Chute Multiplier: " + + retVal += Localizer.Format("#MechJeb_MultiplierQuality", + _parachutePlan.MultiplierQuality.ToString("F1")); //"\nMultiplier Quality: " + + "%" + retVal += Localizer.Format("#MechJeb_Usingpredictions", _parachutePlan.MultiplierDataAmount); //"\nUsing " + + " predictions" - return "N/A"; + return retVal; } public void SetTargetKSC(MechJebCore controller) { users.Add(controller); - core.target.SetPositionTarget(mainBody, MechJebModuleLandingGuidance.landingSites[0].latitude, - MechJebModuleLandingGuidance.landingSites[0].longitude); + core.target.SetPositionTarget(mainBody, MechJebModuleLandingGuidance.LandingSites[0].Latitude, + MechJebModuleLandingGuidance.LandingSites[0].Longitude); } } //A descent speed policy that gives the max safe speed if our entire velocity were straight down internal class SafeDescentSpeedPolicy : IDescentSpeedPolicy { - private readonly double terrainRadius; - private readonly double g; - private readonly double thrust; + private readonly double _terrainRadius; + private readonly double _g; + private readonly double _thrust; public SafeDescentSpeedPolicy(double terrainRadius, double g, double thrust) { - this.terrainRadius = terrainRadius; - this.g = g; - this.thrust = thrust; + _terrainRadius = terrainRadius; + _g = g; + _thrust = thrust; } public double MaxAllowedSpeed(Vector3d pos, Vector3d vel) { - double altitude = pos.magnitude - terrainRadius; - return 0.9 * Math.Sqrt(2 * (thrust - g) * altitude); + double altitude = pos.magnitude - _terrainRadius; + return 0.9 * Math.Sqrt(2 * (_thrust - _g) * altitude); } } internal class PoweredCoastDescentSpeedPolicy : IDescentSpeedPolicy { - private readonly float terrainRadius; - private readonly float g; - private readonly float thrust; + private readonly float _terrainRadius; + private readonly float _g; + private readonly float _thrust; public PoweredCoastDescentSpeedPolicy(double terrainRadius, double g, double thrust) { - this.terrainRadius = (float)terrainRadius; - this.g = (float)g; - this.thrust = (float)thrust; + _terrainRadius = (float)terrainRadius; + _g = (float)g; + _thrust = (float)thrust; } public double MaxAllowedSpeed(Vector3d pos, Vector3d vel) { - if (terrainRadius < pos.magnitude) + if (_terrainRadius < pos.magnitude) return double.MaxValue; double vSpeed = Vector3d.Dot(vel, pos.normalized); - double ToF = (vSpeed + Math.Sqrt(vSpeed * vSpeed + 2 * g * (pos.magnitude - terrainRadius))) / g; + double toF = (vSpeed + Math.Sqrt(vSpeed * vSpeed + 2 * _g * (pos.magnitude - _terrainRadius))) / _g; //MechJebCore.print("ToF = " + ToF.ToString("F2")); - return 0.8 * (thrust - g) * ToF; + return 0.8 * (_thrust - _g) * toF; } } internal class GravityTurnDescentSpeedPolicy : IDescentSpeedPolicy { - private readonly double terrainRadius; - private readonly double g; - private readonly double thrust; + private readonly double _terrainRadius; + private readonly double _g; + private readonly double _thrust; public GravityTurnDescentSpeedPolicy(double terrainRadius, double g, double thrust) { - this.terrainRadius = terrainRadius; - this.g = g; - this.thrust = thrust; + _terrainRadius = terrainRadius; + _g = g; + _thrust = thrust; } public double MaxAllowedSpeed(Vector3d pos, Vector3d vel) { //do a binary search for the max speed that avoids death - double maxFallDistance = pos.magnitude - terrainRadius; + double maxFallDistance = pos.magnitude - _terrainRadius; double lowerBound = 0; double upperBound = 1.1 * vel.magnitude; @@ -660,16 +639,16 @@ public double MaxAllowedSpeed(Vector3d pos, Vector3d vel) return 0.95 * ((upperBound + lowerBound) / 2); } - public double GravityTurnFallDistance(Vector3d x, Vector3d v) + private double GravityTurnFallDistance(Vector3d x, Vector3d v) { double startRadius = x.magnitude; - const int steps = 10; - for (int i = 0; i < steps; i++) + const int STEPS = 10; + for (int i = 0; i < STEPS; i++) { - Vector3d gVec = -g * x.normalized; - Vector3d thrustVec = -thrust * v.normalized; - double dt = 1.0 / (steps - i) * (v.magnitude / thrust); + Vector3d gVec = -_g * x.normalized; + Vector3d thrustVec = -_thrust * v.normalized; + double dt = 1.0 / (STEPS - i) * (v.magnitude / _thrust); Vector3d newV = v + dt * (thrustVec + gVec); x += dt * (v + newV) / 2; v = newV; @@ -677,7 +656,7 @@ public double GravityTurnFallDistance(Vector3d x, Vector3d v) double endRadius = x.magnitude; - endRadius -= v.sqrMagnitude / (2 * (thrust - g)); + endRadius -= v.sqrMagnitude / (2 * (_thrust - _g)); return startRadius - endRadius; } @@ -687,77 +666,80 @@ public double GravityTurnFallDistance(Vector3d x, Vector3d v) internal class ParachutePlan { // We use a linear regression to calculate the best place to deploy based on previous predictions - private LinearRegression regression; + private LinearRegression _regression; - private ReentrySimulation.Result - lastResult; // store the last result so that we can check if any new result is actually a new one, or the same one again. + // store the last result so that we can check if any new result is actually a new one, or the same one again. + private ReentrySimulation.Result _lastResult; - private ReentrySimulation.Result - lastErrorResult; // store the last error result so that we can check if any new error result is actually a new one, or the same one again. + // store the last error result so that we can check if any new error result is actually a new one, or the same one again. + private ReentrySimulation.Result _lastErrorResult; - private readonly CelestialBody body; - private readonly MechJebModuleLandingAutopilot autoPilot; - private bool parachutePresent; - private double maxSemiDeployHeight; - private double minSemiDeployHeight; - private double maxMultiplier; + private readonly CelestialBody _body; + private readonly MechJebModuleLandingAutopilot _autoPilot; + private bool _parachutePresent; + private double _maxSemiDeployHeight; + private double _minSemiDeployHeight; + private double _maxMultiplier; - private double - correlation; // This is the correlation coefficient of the dataset, and is used to tell if the data set is providing helpful information or not. It is exposed outside the class as a "quality percentage" where -1 -> 100% and 0 or more -> 0% + // This is the correlation coefficient of the dataset, and is used to tell if the data set is providing helpful information or not. + // It is exposed outside the class as a "quality percentage" where -1 -> 100% and 0 or more -> 0% + private double _correlation; - private const int dataSetSize = 40; + private const int DATA_SET_SIZE = 40; public double Multiplier { get; private set; } - public int MultiplierDataAmount => regression.dataSetSize; + public int MultiplierDataAmount => _regression.DataSetSize; public double MultiplierQuality { get { - double corr = correlation; + double corr = _correlation; if (double.IsNaN(corr)) return 0; return Math.Max(0, corr * -100); } } - // Incorporates a new simulation result into the simulation data set and calculate a new semi deployment multiplier. If the data set has a poor correlation, then it might just leave the mutiplier. If the correlation becomes positive then it will clear the dataset and start again. + // Incorporates a new simulation result into the simulation data set and calculate a new semi deployment multiplier. + // If the data set has a poor correlation, then it might just leave the mutiplier. + // If the correlation becomes positive then it will clear the dataset and start again. public void AddResult(ReentrySimulation.Result newResult) { // if this result is the same as the old result, then it is not new! if (newResult.MultiplierHasError) { - if (lastErrorResult != null) + if (_lastErrorResult != null) { - if (newResult.ID == lastErrorResult.ID) { return; } + if (newResult.ID == _lastErrorResult.ID) { return; } } - lastErrorResult = newResult; + _lastErrorResult = newResult; } else { - if (lastResult != null) + if (_lastResult != null) { - if (newResult.ID == lastResult.ID) { return; } + if (newResult.ID == _lastResult.ID) { return; } } - lastResult = newResult; + _lastResult = newResult; } // What was the overshoot for this new result? - double overshoot = newResult.GetOvershoot(autoPilot.core.target.targetLatitude, autoPilot.core.target.targetLongitude); + double overshoot = newResult.GetOvershoot(_autoPilot.core.target.targetLatitude, _autoPilot.core.target.targetLongitude); //Debug.Log("overshoot: " + overshoot.ToString("F2") + " multiplier: " + newResult.parachuteMultiplier.ToString("F4") + " hasError:" + newResult.multiplierHasError); // Add the new result to the linear regression - regression.Add(overshoot, newResult.ParachuteMultiplier); + _regression.Add(overshoot, newResult.ParachuteMultiplier); // What is the correlation coefficent of the data. If it is weak a correlation then we will dismiss the dataset and use it to change the current multiplier - correlation = regression.CorrelationCoefficient; - if (correlation > -0.2) // TODO this is the best value to test for non-correlation? + _correlation = _regression.CorrelationCoefficient; + if (_correlation > -0.2) // TODO this is the best value to test for non-correlation? { // If the correlation is less that 0 then we will give up controlling the parachutes and throw away the dataset. Also check that we have got several bits of data, just in case we get two datapoints that are badly correlated. - if (correlation > 0 && regression.dataSetSize > 5) + if (_correlation > 0 && _regression.DataSetSize > 5) { ClearData(); // Debug.Log("Giving up control of the parachutes as the data does not correlate: " + correlation); @@ -767,53 +749,54 @@ public void AddResult(ReentrySimulation.Result newResult) else { // How much data is there? If just one datapoint then we need to slightly vary the multiplier to avoid doing exactly the same multiplier again and getting a divide by zero!. If there is just two then we will not update the multiplier as we can't conclude much from two points of data! - int dataSetSize = regression.dataSetSize; - if (dataSetSize == 1) - { - Multiplier *= 0.99999; - } - else if (dataSetSize == 2) + int dataSetSize = _regression.DataSetSize; + switch (dataSetSize) { - // Doing nothing - } - else - { - // Use the linear regression to give us a new prediction for when to open the parachutes - try - { - Multiplier = regression.yIntercept; - } - catch (Exception) - { - // If there is not enough data then we expect an exception. However we need to vary the multiplier everso slightly so that we get different data in order to start generating data. This should never happen as we have already checked the size of the dataset. + case 1: Multiplier *= 0.99999; - } + break; + case 2: + // Doing nothing + break; + default: + // Use the linear regression to give us a new prediction for when to open the parachutes + try + { + Multiplier = _regression.YIntercept; + } + catch (Exception) + { + // If there is not enough data then we expect an exception. However we need to vary the multiplier everso slightly so that we get different data in order to start generating data. This should never happen as we have already checked the size of the dataset. + Multiplier *= 0.99999; + } + + break; } // Impose sensible limits on the multiplier if (Multiplier < 1 || double.IsNaN(Multiplier)) { Multiplier = 1; } - if (Multiplier > maxMultiplier) { Multiplier = maxMultiplier; } + if (Multiplier > _maxMultiplier) { Multiplier = _maxMultiplier; } } } - public ParachutePlan(MechJebModuleLandingAutopilot _autopliot) + public ParachutePlan(MechJebModuleLandingAutopilot autopliot) { // Create the linear regression for storing previous prediction results - regression = new LinearRegression(dataSetSize); // Store the previous however many predictions + _regression = new LinearRegression(DATA_SET_SIZE); // Store the previous however many predictions // Take a reference to the landing autopilot module that we are working for. - autoPilot = _autopliot; + _autoPilot = autopliot; // Take a note of which body this parachute plan is for. If we go to a different body, we will need a new plan! - body = _autopliot.vessel.orbit.referenceBody; + _body = autopliot.vessel.orbit.referenceBody; } // Throw away any old data, and create a new empty dataset public void ClearData() { // Create the linear regression for storing previous prediction results - regression = new LinearRegression(dataSetSize); // Stored the previous 20 predictions + _regression = new LinearRegression(DATA_SET_SIZE); // Stored the previous 20 predictions } public void StartPlanning() @@ -821,12 +804,12 @@ public void StartPlanning() // what is the highest point at which we could semi deploy? - look at all the parachutes in the craft, and consider the lowest semi deployment pressure. float minSemiDeployPressure = 0; float maxFullDeployHeight = 0; - parachutePresent = false; // First assume that there are no parachutes. + _parachutePresent = false; // First assume that there are no parachutes. // TODO should we check if each of these parachutes is withing the staging limit? - for (int i = 0; i < autoPilot.vesselState.parachutes.Count; i++) + for (int i = 0; i < _autoPilot.vesselState.parachutes.Count; i++) { - ModuleParachute p = autoPilot.vesselState.parachutes[i]; + ModuleParachute p = _autoPilot.vesselState.parachutes[i]; if (p.minAirPressureToOpen > minSemiDeployPressure) // Although this is called "minSemiDeployPressure" we want to find the largest value for each of our parachutes. This can be used to calculate the corresponding height, and hence a height at which we can be guarenteed that all our parachutes will deploy if asked to. { @@ -838,24 +821,24 @@ public void StartPlanning() maxFullDeployHeight = p.deployAltitude; } - parachutePresent = true; + _parachutePresent = true; } // If parachutes are present on the craft then work out the max / min semideployment heights and the starting value. - if (parachutePresent) + if (_parachutePresent) { // TODO is there benefit in running an initial simulation to calculate the height at which the ratio between vertical and horizontal velocity would be the best for being able to deply the chutes to control the landing site? // At what ASL height does the reference body have this pressure? - maxSemiDeployHeight = body.AltitudeForPressure(minSemiDeployPressure); + _maxSemiDeployHeight = _body.AltitudeForPressure(minSemiDeployPressure); // We have to have semi deployed by the time we fully deploy. - minSemiDeployHeight = maxFullDeployHeight; + _minSemiDeployHeight = maxFullDeployHeight; - maxMultiplier = maxSemiDeployHeight / minSemiDeployHeight; + _maxMultiplier = _maxSemiDeployHeight / _minSemiDeployHeight; // Set the inital multiplier to be the mid point. - Multiplier = maxMultiplier / 2; + Multiplier = _maxMultiplier / 2; } } } @@ -863,103 +846,103 @@ public void StartPlanning() // A class to hold a set of x,y data and perform linear regression analysis on it internal class LinearRegression { - private readonly double[] x; - private readonly double[] y; - private readonly int maxDataPoints; - private int currentDataPoint; - private double sumX; - private double sumY; - private double sumXX; - private double sumXY; - private double sumYY; + private readonly double[] _x; + private readonly double[] _y; + private readonly int _maxDataPoints; + private int _currentDataPoint; + private double _sumX; + private double _sumY; + private double _sumXx; + private double _sumXY; + private double _sumYy; - public LinearRegression(int _maxDataPoints) + public LinearRegression(int maxDataPoints) { - maxDataPoints = _maxDataPoints; - dataSetSize = 0; - currentDataPoint = -1; + _maxDataPoints = maxDataPoints; + DataSetSize = 0; + _currentDataPoint = -1; - x = new double[_maxDataPoints]; - y = new double[_maxDataPoints]; + _x = new double[maxDataPoints]; + _y = new double[maxDataPoints]; } // Add a new data point. It might be that this will replace an old data point, or it might be that this is the first or second datapoints - public void Add(double _x, double _y) + public void Add(double x, double y) { - currentDataPoint++; - dataSetSize++; + _currentDataPoint++; + DataSetSize++; // wrap back to the beginning if we have got the end of the array - if (currentDataPoint >= maxDataPoints) + if (_currentDataPoint >= _maxDataPoints) { - currentDataPoint = 0; + _currentDataPoint = 0; } // If we have maxed out the number of data points then we need to remove the old values from the running totals - if (dataSetSize > maxDataPoints) + if (DataSetSize > _maxDataPoints) { - dataSetSize = maxDataPoints; + DataSetSize = _maxDataPoints; } - x[currentDataPoint] = _x; - y[currentDataPoint] = _y; + _x[_currentDataPoint] = x; + _y[_currentDataPoint] = y; // Calculate the new totals - sumX = 0; - sumY = 0; - sumXX = 0; - sumXY = 0; - sumYY = 0; + _sumX = 0; + _sumY = 0; + _sumXx = 0; + _sumXY = 0; + _sumYy = 0; - for (int i = 0; i < dataSetSize; i++) + for (int i = 0; i < DataSetSize; i++) { - double thisx = x[i]; - double thisy = y[i]; - - sumX += thisx; - sumXX += thisx * thisx; - sumY += thisy; - sumYY += thisy * thisy; - sumXY += thisx * thisy; + double thisx = _x[i]; + double thisy = _y[i]; + + _sumX += thisx; + _sumXx += thisx * thisx; + _sumY += thisy; + _sumYy += thisy * thisy; + _sumXY += thisx * thisy; } } - public double slope + private double _slope { get { // Require at least half the datapoints in the dataset - if (dataSetSize < 2) + if (DataSetSize < 2) { throw new Exception("Not enough data to calculate trend line"); } - double result = (sumXY - sumX * sumY / dataSetSize) / (sumXX - sumX * sumX / dataSetSize); + double result = (_sumXY - _sumX * _sumY / DataSetSize) / (_sumXx - _sumX * _sumX / DataSetSize); return result; } } - public double yIntercept + public double YIntercept { get { - double result = sumY / dataSetSize - slope * (sumX / dataSetSize); + double result = _sumY / DataSetSize - _slope * (_sumX / DataSetSize); return result; } } - public int dataSetSize { get; private set; } + public int DataSetSize { get; private set; } // Calculation the Pearson product-moment correlation coefficient public double CorrelationCoefficient { get { - double stdX = Math.Sqrt(sumXX / dataSetSize - sumX * sumX / dataSetSize / dataSetSize); - double stdY = Math.Sqrt(sumYY / dataSetSize - sumY * sumY / dataSetSize / dataSetSize); - double covariance = sumXY / dataSetSize - sumX * sumY / dataSetSize / dataSetSize; + double stdX = Math.Sqrt(_sumXx / DataSetSize - _sumX * _sumX / DataSetSize / DataSetSize); + double stdY = Math.Sqrt(_sumYy / DataSetSize - _sumY * _sumY / DataSetSize / DataSetSize); + double covariance = _sumXY / DataSetSize - _sumX * _sumY / DataSetSize / DataSetSize; return covariance / stdX / stdY; } diff --git a/MechJeb2/MechJebModuleLandingGuidance.cs b/MechJeb2/MechJebModuleLandingGuidance.cs index 8b584bb5..aef06ad1 100644 --- a/MechJeb2/MechJebModuleLandingGuidance.cs +++ b/MechJeb2/MechJebModuleLandingGuidance.cs @@ -11,25 +11,25 @@ namespace MuMech [UsedImplicitly] public class MechJebModuleLandingGuidance : DisplayModule { - public MechJebModuleLandingPredictions predictor; - public static List landingSites; + private MechJebModuleLandingPredictions _predictor; + public static List LandingSites; [Persistent(pass = (int)(Pass.Global | Pass.Local))] - public int landingSiteIdx; + private int _landingSiteIdx; public struct LandingSite { - public string name; - public CelestialBody body; - public double latitude; - public double longitude; + public string Name; + public CelestialBody Body; + public double Latitude; + public double Longitude; } public override void OnStart(PartModule.StartState state) { - predictor = core.GetComputerModule(); + _predictor = core.GetComputerModule(); - if (landingSites == null && HighLogic.LoadedSceneIsFlight) + if (LandingSites == null && HighLogic.LoadedSceneIsFlight) InitLandingSitesList(); } @@ -38,9 +38,9 @@ public override GUILayoutOption[] WindowOptions() return new[] { GUILayout.Width(200), GUILayout.Height(150) }; } - private void moveByMeter(ref EditableAngle angle, double distance, double Alt) + private void MoveByMeter(ref EditableAngle angle, double distance, double alt) { - double angularDelta = distance * UtilMath.Rad2Deg / (Alt + mainBody.Radius); + double angularDelta = distance * UtilMath.Rad2Deg / (alt + mainBody.Radius); angle += angularDelta; } @@ -50,20 +50,20 @@ protected override void WindowGUI(int windowID) if (core.target.PositionTargetExists) { - double ASL = core.vessel.mainBody.TerrainAltitude(core.target.targetLatitude, core.target.targetLongitude); + double asl = core.vessel.mainBody.TerrainAltitude(core.target.targetLatitude, core.target.targetLongitude); GUILayout.Label(Localizer.Format("#MechJeb_LandingGuidance_label1")); //Target coordinates: GUILayout.BeginHorizontal(); core.target.targetLatitude.DrawEditGUI(EditableAngle.Direction.NS); if (GUILayout.Button("▲")) { - moveByMeter(ref core.target.targetLatitude, 10, ASL); + MoveByMeter(ref core.target.targetLatitude, 10, asl); } GUILayout.Label("10m"); if (GUILayout.Button("▼")) { - moveByMeter(ref core.target.targetLatitude, -10, ASL); + MoveByMeter(ref core.target.targetLatitude, -10, asl); } GUILayout.EndHorizontal(); @@ -71,19 +71,19 @@ protected override void WindowGUI(int windowID) core.target.targetLongitude.DrawEditGUI(EditableAngle.Direction.EW); if (GUILayout.Button("◄")) { - moveByMeter(ref core.target.targetLongitude, -10, ASL); + MoveByMeter(ref core.target.targetLongitude, -10, asl); } GUILayout.Label("10m"); if (GUILayout.Button("►")) { - moveByMeter(ref core.target.targetLongitude, 10, ASL); + MoveByMeter(ref core.target.targetLongitude, 10, asl); } GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); - GUILayout.Label("ASL: " + ASL.ToSI() + "m"); + GUILayout.Label("ASL: " + asl.ToSI() + "m"); GUILayout.Label(core.target.targetBody.GetExperimentBiomeSafe(core.target.targetLatitude, core.target.targetLongitude)); GUILayout.EndHorizontal(); } @@ -97,15 +97,15 @@ protected override void WindowGUI(int windowID) if (GUILayout.Button(Localizer.Format("#MechJeb_LandingGuidance_button2"))) core.target.PickPositionTargetOnMap(); //Pick target on map - var availableLandingSites = landingSites.Where(p => p.body == mainBody).ToList(); + var availableLandingSites = LandingSites.Where(p => p.Body == mainBody).ToList(); if (availableLandingSites.Any()) { GUILayout.BeginHorizontal(); - landingSiteIdx = GuiUtils.ComboBox.Box(landingSiteIdx, availableLandingSites.Select(p => p.name).ToArray(), this); + _landingSiteIdx = GuiUtils.ComboBox.Box(_landingSiteIdx, availableLandingSites.Select(p => p.Name).ToArray(), this); if (GUILayout.Button("Set", GUILayout.ExpandWidth(false))) { - core.target.SetPositionTarget(mainBody, availableLandingSites[landingSiteIdx].latitude, - availableLandingSites[landingSiteIdx].longitude); + core.target.SetPositionTarget(mainBody, availableLandingSites[_landingSiteIdx].Latitude, + availableLandingSites[_landingSiteIdx].Longitude); } GUILayout.EndHorizontal(); @@ -117,8 +117,8 @@ protected override void WindowGUI(int windowID) { GUILayout.Label(Localizer.Format("#MechJeb_LandingGuidance_label2")); //Autopilot: - predictor.maxOrbits = core.landing.enabled ? 0.5 : 4; - predictor.noSkipToFreefall = !core.landing.enabled; + _predictor.maxOrbits = core.landing.enabled ? 0.5 : 4; + _predictor.noSkipToFreefall = !core.landing.enabled; if (core.landing.enabled) { @@ -136,31 +136,31 @@ protected override void WindowGUI(int windowID) GUILayout.EndHorizontal(); } - GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_LandingGuidance_label3"), core.landing.touchdownSpeed, "m/s", + GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_LandingGuidance_label3"), core.landing.TouchdownSpeed, "m/s", 35); //Touchdown speed: if (core.landing != null) core.node.autowarp = GUILayout.Toggle(core.node.autowarp, Localizer.Format("#MechJeb_LandingGuidance_checkbox1")); //Auto-warp - core.landing.deployGears = - GUILayout.Toggle(core.landing.deployGears, Localizer.Format("#MechJeb_LandingGuidance_checkbox2")); //Deploy Landing Gear - GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_LandingGuidance_label4"), core.landing.limitGearsStage, "", 35); //"Stage Limit:" - core.landing.deployChutes = - GUILayout.Toggle(core.landing.deployChutes, Localizer.Format("#MechJeb_LandingGuidance_checkbox3")); //Deploy Parachutes - predictor.deployChutes = core.landing.deployChutes; - GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_LandingGuidance_label5"), core.landing.limitChutesStage, "", 35); //Stage Limit: - predictor.limitChutesStage = core.landing.limitChutesStage; - core.landing.rcsAdjustment = - GUILayout.Toggle(core.landing.rcsAdjustment, + core.landing.DeployGears = + GUILayout.Toggle(core.landing.DeployGears, Localizer.Format("#MechJeb_LandingGuidance_checkbox2")); //Deploy Landing Gear + GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_LandingGuidance_label4"), core.landing.LimitGearsStage, "", 35); //"Stage Limit:" + core.landing.DeployChutes = + GUILayout.Toggle(core.landing.DeployChutes, Localizer.Format("#MechJeb_LandingGuidance_checkbox3")); //Deploy Parachutes + _predictor.deployChutes = core.landing.DeployChutes; + GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_LandingGuidance_label5"), core.landing.LimitChutesStage, "", 35); //Stage Limit: + _predictor.limitChutesStage = core.landing.LimitChutesStage; + core.landing.RCSAdjustment = + GUILayout.Toggle(core.landing.RCSAdjustment, Localizer.Format("#MechJeb_LandingGuidance_checkbox4")); //Use RCS for small adjustment if (core.landing.enabled) { - GUILayout.Label(Localizer.Format("#MechJeb_LandingGuidance_label6") + core.landing.status); //Status: + GUILayout.Label(Localizer.Format("#MechJeb_LandingGuidance_label6") + core.landing.Status); //Status: GUILayout.Label(Localizer.Format("#MechJeb_LandingGuidance_label7") + (core.landing.CurrentStep != null ? core.landing.CurrentStep.GetType().Name : "N/A")); //Step: GUILayout.Label(Localizer.Format("#MechJeb_LandingGuidance_label8") + - (core.landing.descentSpeedPolicy != null ? core.landing.descentSpeedPolicy.GetType().Name : "N/A") + " (" + + (core.landing.DescentSpeedPolicy != null ? core.landing.DescentSpeedPolicy.GetType().Name : "N/A") + " (" + core.landing.UseAtmosphereToBrake() + ")"); //Mode //GUILayout.Label("DecEndAlt: " + core.landing.DecelerationEndAltitude().ToString("F2")); //var dragLength = mainBody.DragLength(core.landing.LandingAltitude, core.landing.vesselAverageDrag, vesselState.mass); @@ -181,8 +181,8 @@ protected override void WindowGUI(int windowID) public void SetAndLandTargetKSC() { - LandingSite ksc = landingSites.First(x => x.name == "KSC Pad"); - core.target.SetPositionTarget(mainBody, ksc.latitude, ksc.longitude); + LandingSite ksc = LandingSites.First(x => x.Name == "KSC Pad"); + core.target.SetPositionTarget(mainBody, ksc.Latitude, ksc.Longitude); core.landing.LandAtPositionTarget(this); } @@ -197,29 +197,29 @@ private void DrawGUITogglePredictions() { GUILayout.BeginVertical(); - bool active = GUILayout.Toggle(predictor.enabled, Localizer.Format("#MechJeb_LandingGuidance_checkbox5")); //Show landing predictions - if (predictor.enabled != active) + bool active = GUILayout.Toggle(_predictor.enabled, Localizer.Format("#MechJeb_LandingGuidance_checkbox5")); //Show landing predictions + if (_predictor.enabled != active) { if (active) { - predictor.users.Add(this); + _predictor.users.Add(this); } else { - predictor.users.Remove(this); + _predictor.users.Remove(this); } } - if (predictor.enabled) + if (_predictor.enabled) { - predictor.makeAerobrakeNodes = - GUILayout.Toggle(predictor.makeAerobrakeNodes, Localizer.Format("#MechJeb_LandingGuidance_checkbox6")); //"Show aerobrake nodes" - predictor.showTrajectory = - GUILayout.Toggle(predictor.showTrajectory, Localizer.Format("#MechJeb_LandingGuidance_checkbox7")); //Show trajectory - predictor.worldTrajectory = - GUILayout.Toggle(predictor.worldTrajectory, Localizer.Format("#MechJeb_LandingGuidance_checkbox8")); //World trajectory - predictor.camTrajectory = - GUILayout.Toggle(predictor.camTrajectory, Localizer.Format("#MechJeb_LandingGuidance_checkbox9")); //Camera trajectory (WIP) + _predictor.makeAerobrakeNodes = + GUILayout.Toggle(_predictor.makeAerobrakeNodes, Localizer.Format("#MechJeb_LandingGuidance_checkbox6")); //"Show aerobrake nodes" + _predictor.showTrajectory = + GUILayout.Toggle(_predictor.showTrajectory, Localizer.Format("#MechJeb_LandingGuidance_checkbox7")); //Show trajectory + _predictor.worldTrajectory = + GUILayout.Toggle(_predictor.worldTrajectory, Localizer.Format("#MechJeb_LandingGuidance_checkbox8")); //World trajectory + _predictor.camTrajectory = + GUILayout.Toggle(_predictor.camTrajectory, Localizer.Format("#MechJeb_LandingGuidance_checkbox9")); //Camera trajectory (WIP) DrawGUIPrediction(); } @@ -228,7 +228,7 @@ private void DrawGUITogglePredictions() private void DrawGUIPrediction() { - ReentrySimulation.Result result = predictor.Result; + ReentrySimulation.Result result = _predictor.Result; if (result != null) { switch (result.Outcome) @@ -280,7 +280,7 @@ private void DrawGUIPrediction() private void InitLandingSitesList() { - landingSites = new List(); + LandingSites = new List(); // Import landing sites from users createded .cfg foreach (UrlDir.UrlConfig mjConf in GameDatabase.Instance.GetConfigs("MechJeb2Landing")) @@ -304,10 +304,10 @@ private void InitLandingSitesList() string bodyName = site.GetValue("body"); CelestialBody body = bodyName != null ? FlightGlobals.Bodies.Find(b => b.bodyName == bodyName) : Planetarium.fetch.Home; - if (landingSites.All(p => p.name != launchSiteName)) + if (LandingSites.All(p => p.Name != launchSiteName)) { print("Adding " + launchSiteName); - landingSites.Add(new LandingSite { name = launchSiteName, latitude = latitude, longitude = longitude, body = body }); + LandingSites.Add(new LandingSite { Name = launchSiteName, Latitude = latitude, Longitude = longitude, Body = body }); } } } @@ -318,9 +318,9 @@ private void InitLandingSitesList() if (site.spawnPoints.Length > 0) { LaunchSite.SpawnPoint point = site.spawnPoints[0]; - landingSites.Add(new LandingSite + LandingSites.Add(new LandingSite { - name = point.name.Replace("_", " "), latitude = point.latitude, longitude = point.longitude, body = site.Body + Name = point.name.Replace("_", " "), Latitude = point.latitude, Longitude = point.longitude, Body = site.Body }); } } @@ -347,14 +347,14 @@ private void InitLandingSitesList() double latitude = Math.Asin(pos.y) * UtilMath.Rad2Deg; double longitude = Math.Atan2(pos.z, pos.x) * UtilMath.Rad2Deg; - if (body != null && landingSites.All(p => p.name != launchSiteName)) + if (body != null && LandingSites.All(p => p.Name != launchSiteName)) { - landingSites.Add(new LandingSite + LandingSites.Add(new LandingSite { - name = launchSiteName, - latitude = !double.IsNaN(latitude) ? latitude : 0, - longitude = !double.IsNaN(longitude) ? longitude : 0, - body = body + Name = launchSiteName, + Latitude = !double.IsNaN(latitude) ? latitude : 0, + Longitude = !double.IsNaN(longitude) ? longitude : 0, + Body = body }); } } @@ -384,24 +384,23 @@ private void InitLandingSitesList() continue; } - double latitude, longitude; - double.TryParse(lat, out latitude); - double.TryParse(lon, out longitude); + double.TryParse(lat, out double latitude); + double.TryParse(lon, out double longitude); - if (landingSites.All(p => p.name != launchSiteName)) + if (LandingSites.All(p => p.Name != launchSiteName)) { - landingSites.Add(new LandingSite + LandingSites.Add(new LandingSite { - name = launchSiteName, latitude = latitude, longitude = longitude, body = Planetarium.fetch.Home + Name = launchSiteName, Latitude = latitude, Longitude = longitude, Body = Planetarium.fetch.Home }); } } } } - if (landingSiteIdx > landingSites.Count) + if (_landingSiteIdx > LandingSites.Count) { - landingSiteIdx = 0; + _landingSiteIdx = 0; } }