void DriveCourseCorrections(FlightCtrlState s)
{
double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), LandingSite);
if (currentError < 300)
{
core.thrust.targetThrottle = 0;
landStep = LandStep.COAST_TO_DECELERATION;
return;
}
Vector3d deltaV = ComputeCourseCorrection(true);
status = "Performing course correction of about " + deltaV.magnitude.ToString("F1") + " m/s";
core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, this);
if (core.attitude.attitudeAngleFromTarget() < 5)
{
core.thrust.targetThrottle = Mathf.Clamp01((float)(deltaV.magnitude / (2.0 * vesselState.maxThrustAccel)));
}
else
{
core.thrust.targetThrottle = 0;
}
}
public void InvalidLinkerWhenRoleExit()
{
mPlayerData = null;
mForwardInfo.RoleId = 0;
mForwardInfo.MapIndexInServer = UInt16.MaxValue;
mForwardInfo.PlayerIndexInMap = UInt16.MaxValue;
mForwardInfo.Gate2PlanesConnect = null;
mLandStep = Gate.LandStep.SelectRole;
}
public override void OnModuleDisabled()
{
core.attitude.attitudeDeactivate();
predictor.users.Remove(this);
predictor.descentSpeedPolicy = null;
core.thrust.ThrustOff();
core.thrust.users.Remove(this);
landStep = LandStep.OFF;
status = "Off";
}
void FixedUpdateCoastToDeceleration()
{
core.thrust.targetThrottle = 0;
double maxAllowedSpeed = MaxAllowedSpeed();
if (vesselState.speedSurface > 0.9 * maxAllowedSpeed)
{
core.warp.MinimumWarp();
landStep = LandStep.DECELERATING;
return;
}
double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), LandingSite);
if (currentError > 600)
{
core.warp.MinimumWarp();
landStep = LandStep.COURSE_CORRECTIONS;
return;
}
if (core.attitude.attitudeAngleFromTarget() < 1)
{
warpReady = true;
} // less warp start warp stop jumping
if (core.attitude.attitudeAngleFromTarget() > 5)
{
warpReady = false;
} // hopefully
if (PredictionReady)
{
double decelerationStartTime = (prediction.trajectory.Any() ? prediction.trajectory.First().UT : vesselState.time);
Vector3d decelerationStartAttitude = -orbit.SwappedOrbitalVelocityAtUT(decelerationStartTime);
decelerationStartAttitude += mainBody.getRFrmVel(orbit.SwappedAbsolutePositionAtUT(decelerationStartTime));
decelerationStartAttitude = decelerationStartAttitude.normalized;
core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, this);
}
//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)
{
core.warp.WarpRegularAtRate((float)(vesselState.altitudeASL / (10 * Math.Abs(vesselState.speedVertical))));
}
else
{
core.warp.MinimumWarp();
}
status = "Coasting toward deceleration burn";
}
void FixedUpdatePlaneChange()
{
Vector3d targetRadialVector = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0);
Vector3d currentRadialVector = vesselState.CoM - mainBody.position;
double angleToTarget = Vector3d.Angle(targetRadialVector, currentRadialVector);
bool approaching = Vector3d.Dot(targetRadialVector - currentRadialVector, vesselState.velocityVesselOrbit) > 0;
if (!planeChangeTriggered && approaching && (angleToTarget > 80) && (angleToTarget < 90))
{
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp(true);
}
planeChangeTriggered = true;
}
if (planeChangeTriggered)
{
Vector3d horizontalToTarget = ComputePlaneChange();
Vector3d finalVelocity = Quaternion.FromToRotation(vesselState.horizontalOrbit, horizontalToTarget) * vesselState.velocityVesselOrbit;
Vector3d deltaV = finalVelocity - vesselState.velocityVesselOrbit;
//burn normal+ or normal- to avoid dropping the Pe:
Vector3d burnDir = Vector3d.Exclude(vesselState.up, Vector3d.Exclude(vesselState.velocityVesselOrbit, deltaV));
planeChangeDVLeft = Math.PI / 180 * Vector3d.Angle(finalVelocity, vesselState.velocityVesselOrbit) * vesselState.speedOrbitHorizontal;
core.attitude.attitudeTo(burnDir, AttitudeReference.INERTIAL, this);
if (planeChangeDVLeft < 0.1F)
{
landStep = LandStep.LOW_DEORBIT_BURN;
}
status = "Executing low orbit plane change of about " + planeChangeDVLeft.ToString("F0") + " m/s";
}
else
{
if (core.node.autowarp)
{
core.warp.WarpRegularAtRate((float)(orbit.period / 6));
}
status = "Moving to low orbit plane change burn point";
}
}
void DriveUntargetedDeorbit(FlightCtrlState s)
{
if (orbit.PeA < -0.1 * mainBody.Radius)
{
core.thrust.targetThrottle = 0;
landStep = LandStep.FINAL_DESCENT;
return;
}
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT_HORIZONTAL, this);
if (core.attitude.attitudeAngleFromTarget() < 5)
{
core.thrust.targetThrottle = 1;
}
else
{
core.thrust.targetThrottle = 0;
}
status = "Doing deorbit burn.";
}
public void DriveKillHorizontalVelocity(FlightCtrlState s)
{
Vector3d horizontalPointingDirection = Vector3d.Exclude(vesselState.up, vesselState.forward).normalized;
if (Vector3d.Dot(horizontalPointingDirection, vesselState.velocityVesselSurface) > 0)
{
core.thrust.targetThrottle = 0;
core.attitude.attitudeTo(Vector3.up, AttitudeReference.SURFACE_NORTH, this);
landStep = LandStep.FINAL_DESCENT;
return;
}
//control thrust to control vertical speed:
double desiredSpeed = 0; //hover until horizontal velocity is killed
double controlledSpeed = Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.up);
double speedError = desiredSpeed - controlledSpeed;
double speedCorrectionTimeConstant = 1.0;
double desiredAccel = speedError / speedCorrectionTimeConstant;
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);
}
else
{
core.thrust.targetThrottle = 0;
}
//angle up and slightly away from vertical:
Vector3d desiredThrustVector = (vesselState.up + 0.2 * horizontalPointingDirection).normalized;
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
status = "Killing horizontal velocity before final descent";
}
// 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 void DriveKillHorizontalVelocity(FlightCtrlState s)
{
Vector3d horizontalPointingDirection = Vector3d.Exclude(vesselState.up, vesselState.forward).normalized;
if (Vector3d.Dot(horizontalPointingDirection, vessel.srf_velocity) > 0)
{
core.thrust.targetThrottle = 0;
core.attitude.attitudeTo(Vector3.up, AttitudeReference.SURFACE_NORTH, this);
landStep = LandStep.FINAL_DESCENT;
return;
}
//control thrust to control vertical speed:
const double desiredSpeed = 0; //hover until horizontal velocity is killed
double controlledSpeed = Vector3d.Dot(vessel.srf_velocity, 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;
if (maxAccel - minAccel > 0)
{
core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F);
}
else
{
core.thrust.targetThrottle = 0;
}
//angle up and slightly away from vertical:
Vector3d desiredThrustVector = (vesselState.up + 0.2 * horizontalPointingDirection).normalized;
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
status = "Killing horizontal velocity before final descent";
}
void driveFinalDescent(FlightCtrlState s)
{
landStatusString = "Final descent";
//hand off to core's landing mode
if (part.vessel.Landed || part.vessel.Splashed)
{
turnOffSteering();
core.controlRelease(this);
landStep = LandStep.LANDED;
return;
}
if (!core.trans_land)
{
core.landActivate(this, touchdownSpeed);
}
}
void FixedUpdateDeorbitBurn()
{
//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 (part.vessel.orbit.ApA < part.vessel.mainBody.RealMaxAtmosphereAltitude())
{
landStep = LandStep.COURSE_CORRECTIONS;
core.thrust.targetThrottle = 0;
return;
}
//We aim for a trajectory that
// a) has the same vertical speed as our current trajectory
// b) has a horizontal speed that will give it a periapsis of -10% of the body's radius
// c) has a heading that points toward where the target will be at the end of free-fall, accounting for planetary rotation
Vector3d horizontalDV = OrbitalManeuverCalculator.DeltaVToChangePeriapsis(orbit, vesselState.time, 0.9 * mainBody.Radius); //Imagine we are going to deorbit now. Find the burn that would lower our periapsis to -10% of the planet's radius
Orbit forwardDeorbitTrajectory = orbit.PerturbedOrbit(vesselState.time, horizontalDV); //Compute the orbit that would put us on
double freefallTime = forwardDeorbitTrajectory.NextTimeOfRadius(vesselState.time, mainBody.Radius) - vesselState.time; //Find how long that orbit would take to impact the ground
double planetRotationDuringFreefall = 360 * freefallTime / mainBody.rotationPeriod; //Find how many degrees the planet will rotate during that time
Vector3d currentTargetRadialVector = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0); //Find the current vector from the planet center to the target landing site
Quaternion freefallPlanetRotation = Quaternion.AngleAxis((float)planetRotationDuringFreefall, mainBody.angularVelocity); //Construct a quaternion representing the rotation of the planet found above
Vector3d freefallEndTargetRadialVector = freefallPlanetRotation * currentTargetRadialVector; //Use this quaternion to find what the vector from the planet center to the target will be when we hit the ground
Vector3d freefallEndTargetPosition = mainBody.position + freefallEndTargetRadialVector; //Then find the actual position of the target at that time
Vector3d freefallEndHorizontalToTarget = Vector3d.Exclude(vesselState.up, freefallEndTargetPosition - vesselState.CoM).normalized; //Find a horizontal unit vector that points toward where the target will be when we hit the ground
Vector3d currentHorizontalVelocity = Vector3d.Exclude(vesselState.up, vesselState.velocityVesselOrbit); //Find our current horizontal velocity
double finalHorizontalSpeed = (currentHorizontalVelocity + horizontalDV).magnitude; //Find the desired horizontal speed after the deorbit burn
Vector3d finalHorizontalVelocity = finalHorizontalSpeed * freefallEndHorizontalToTarget; //Combine the desired speed and direction to get the desired velocity after the deorbi burn
//Compute the angle between the location of the target at the end of freefall and the normal to our orbit:
Vector3d currentRadialVector = vesselState.CoM - part.vessel.mainBody.position;
double targetAngleToOrbitNormal = Vector3d.Angle(orbit.SwappedOrbitNormal(), freefallEndTargetRadialVector);
targetAngleToOrbitNormal = Math.Min(targetAngleToOrbitNormal, 180 - targetAngleToOrbitNormal);
double targetAheadAngle = Vector3d.Angle(currentRadialVector, freefallEndTargetRadialVector); //How far ahead the target is, in degrees
double planeChangeAngle = Vector3d.Angle(currentHorizontalVelocity, freefallEndHorizontalToTarget); //The plane change required to get onto the deorbit trajectory, in degrees
//If the target is basically almost normal to our orbit, it doesn't matter when we deorbit; might as well do it now
//Otherwise, wait until the target is ahead
if (targetAngleToOrbitNormal < 10 ||
(targetAheadAngle < 90 && targetAheadAngle > 60 && planeChangeAngle < 90))
{
deorbitBurnTriggered = true;
}
if (deorbitBurnTriggered)
{
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp(true); //get out of warp
}
Vector3d deltaV = finalHorizontalVelocity - currentHorizontalVelocity;
core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, this);
if (deltaV.magnitude < 2.0)
{
landStep = LandStep.COURSE_CORRECTIONS;
}
status = "Doing high deorbit burn";
}
else
{
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT, this);
if (core.node.autowarp)
{
core.warp.WarpRegularAtRate((float)(orbit.period / 10));
}
status = "Moving to high deorbit burn point";
}
}
void FixedUpdateDecelerationBurn()
{
if (vesselState.altitudeASL < DecelerationEndAltitude() + 5)
{
if (UseAtmosphereToBrake())
{
landStep = LandStep.FINAL_DESCENT;
}
else
{
landStep = LandStep.KILLING_HORIZONTAL_VELOCITY;
}
core.warp.MinimumWarp();
return;
}
double decelerationStartTime = (prediction.trajectory.Any() ? prediction.trajectory.First().UT : vesselState.time);
if (decelerationStartTime - vesselState.time > 5)
{
core.thrust.targetThrottle = 0;
status = "Warping to start of braking burn.";
//warp to deceleration start
Vector3d decelerationStartAttitude = -orbit.SwappedOrbitalVelocityAtUT(decelerationStartTime);
decelerationStartAttitude += mainBody.getRFrmVel(orbit.SwappedAbsolutePositionAtUT(decelerationStartTime));
decelerationStartAttitude = decelerationStartAttitude.normalized;
core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, this);
bool warpReady = core.attitude.attitudeAngleFromTarget() < 5;
if (warpReady && core.node.autowarp)
{
core.warp.WarpToUT(decelerationStartTime - 5);
}
else if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp();
}
return;
}
Vector3d desiredThrustVector = -vesselState.velocityVesselSurfaceUnit;
Vector3d courseCorrection = 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.velocityVesselSurface, vesselState.up) > 0 ||
Vector3d.Dot(vesselState.forward, desiredThrustVector) < 0.75)
{
core.thrust.targetThrottle = 0;
status = "Braking";
}
else
{
double controlledSpeed = vesselState.speedSurface * Math.Sign(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.up)); //positive if we are ascending, negative if descending
double desiredSpeed = -MaxAllowedSpeed();
double desiredSpeedAfterDt = -MaxAllowedSpeedAfterDt(vesselState.deltaT);
double minAccel = -vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.velocityVesselSurfaceUnit, vesselState.up));
double maxAccel = vesselState.maxThrustAccel * Vector3d.Dot(vesselState.forward, -vesselState.velocityVesselSurfaceUnit) - vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.velocityVesselSurfaceUnit, vesselState.up));
double speedCorrectionTimeConstant = 0.3;
double speedError = desiredSpeed - controlledSpeed;
double desiredAccel = speedError / speedCorrectionTimeConstant + (desiredSpeedAfterDt - desiredSpeed) / vesselState.deltaT;
if (maxAccel - minAccel > 0)
{
core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F);
}
else
{
core.thrust.targetThrottle = 0;
}
status = "Braking: target speed = " + Math.Abs(desiredSpeed).ToString("F1") + " m/s";
}
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
}
void DriveCourseCorrections(FlightCtrlState s)
{
double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), LandingSite);
if (currentError < 150)
{
core.thrust.targetThrottle = 0;
landStep = LandStep.COAST_TO_DECELERATION;
return;
}
Vector3d deltaV = ComputeCourseCorrection(true);
status = "Performing course correction of about " + deltaV.magnitude.ToString("F1") + " m/s";
core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, this);
if (core.attitude.attitudeAngleFromTarget() < 5)
{
core.thrust.targetThrottle = Mathf.Clamp01((float)(deltaV.magnitude / (2.0 * vesselState.maxThrustAccel)));
}
else
{
core.thrust.targetThrottle = 0;
}
}
//programmatic interface to land at a specific target.
public void landAt(double latitude, double longitude)
{
this.enabled = true;
core.controlClaim(this);
autoLandAtTarget = true;
deorbitBurnFirstMessage = false;
gaveUpOnCourseCorrections = false;
targetLatitude = latitude;
targetLongitude = longitude;
initializeDecimalStrings();
initializeDMSStrings();
if (targetLatitude == BEACH_LATITUDE && targetLongitude == BEACH_LONGITUDE) dmsInput = false;
core.landDeactivate(this);
landStep = LandStep.COURSE_CORRECTIONS;
FlightInputHandler.SetNeutralControls();
//run an initial landing simulation
prediction = simulateReentryRK4(simulationTimeStep, Vector3d.zero);
//initialize the state machine
if (part.vessel.orbit.PeA < 0 || prediction.outcome == LandingPrediction.Outcome.LANDED)
{
landStep = LandStep.COURSE_CORRECTIONS;
}
else
{
landStep = LandStep.DEORBIT_BURN;
deorbiting = false;
}
}
void FixedUpdateCoastToDeceleration()
{
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 (mainBody.DragAccel(vesselState.CoM, vessel.obt_velocity, vesselState.massDrag / vesselState.mass).magnitude > 0.10)
{
if(ParachutesDeployable())
{
ControlParachutes();
}
}
double maxAllowedSpeed = MaxAllowedSpeed();
if (vesselState.speedSurface > 0.9 * maxAllowedSpeed)
{
core.warp.MinimumWarp();
landStep = LandStep.DECELERATING;
return;
}
double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), LandingSite);
if (currentError > 600)
{
if (!vesselState.parachuteDeployed) // However if there is already a parachute deployed, then do not bother trying to correct the course as we will not have any attitude control anyway.
{
core.warp.MinimumWarp();
landStep = LandStep.COURSE_CORRECTIONS;
return;
}
}
// Sometimes (on bodies with a thick atmosphere) there is no need for a decleration burn. Check for this so that it is possible to transition into the final decent step.
if ((vesselState.altitudeASL < DecelerationEndAltitude() + 5) && UseAtmosphereToBrake())
{
landStep = LandStep.FINAL_DESCENT;
core.warp.MinimumWarp();
return;
}
if (core.attitude.attitudeAngleFromTarget() < 1) { warpReady = true; } // less warp start warp stop jumping
if (core.attitude.attitudeAngleFromTarget() > 5) { warpReady = false; } // hopefully
if (PredictionReady)
{
double decelerationStartTime = (prediction.trajectory.Any() ? prediction.trajectory.First().UT : vesselState.time);
Vector3d decelerationStartAttitude = -orbit.SwappedOrbitalVelocityAtUT(decelerationStartTime);
decelerationStartAttitude += mainBody.getRFrmVel(orbit.SwappedAbsolutePositionAtUT(decelerationStartTime));
decelerationStartAttitude = decelerationStartAttitude.normalized;
core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, this);
}
//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) core.warp.WarpRegularAtRate((float)(vesselState.altitudeASL / (10 * Math.Abs(vesselState.speedVertical))));
else core.warp.MinimumWarp();
status = "Coasting toward deceleration burn";
}
//programmatic interface to land at a specific target. this should probably
//only be called if the module has been enabled for a while so that it has had
//the chance to run at least one landing simulation. I'm not sure what will happen
//if you enable the module and then immediately call landAt()
public void landAt(double latitude, double longitude)
{
core.controlClaim(this);
autoLandAtTarget = true;
deorbitBurnFirstMessage = false;
gaveUpOnCourseCorrections = false;
targetLatitude = latitude;
targetLongitude = longitude;
initializeDecimalStrings();
initializeDMSStrings();
if (targetLatitude == BEACH_LATITUDE && targetLongitude == BEACH_LONGITUDE) dmsInput = false;
core.landDeactivate(this);
landStep = LandStep.COURSE_CORRECTIONS;
FlightInputHandler.SetNeutralControls();
}
void DriveCourseCorrections(FlightCtrlState s)
{
// If the atomospheric drag is at least 100mm/s2 then start trying to target the overshoot using the parachutes
if (mainBody.DragAccel(vesselState.CoM, vessel.obt_velocity, vesselState.massDrag / vesselState.mass).magnitude > 0.1)
{
if (ParachutesDeployable())
{
ControlParachutes();
}
}
double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), LandingSite);
if (currentError < 150)
{
core.thrust.targetThrottle = 0;
landStep = LandStep.COAST_TO_DECELERATION;
return;
}
// 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)
{
core.thrust.targetThrottle = 0;
landStep = LandStep.COAST_TO_DECELERATION;
return;
}
Vector3d deltaV = ComputeCourseCorrection(true);
status = "Performing course correction of about " + deltaV.magnitude.ToString("F1") + " m/s";
core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, this);
if (core.attitude.attitudeAngleFromTarget() < 5)
{
core.thrust.targetThrottle = Mathf.Clamp01((float)(deltaV.magnitude / (2.0 * vesselState.maxThrustAccel)));
}
else
{
core.thrust.targetThrottle = 0;
}
}
void DriveUntargetedDeorbit(FlightCtrlState s)
{
if (orbit.PeA < -0.1 * mainBody.Radius)
{
core.thrust.targetThrottle = 0;
landStep = LandStep.FINAL_DESCENT;
return;
}
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT_HORIZONTAL, this);
if (core.attitude.attitudeAngleFromTarget() < 5) core.thrust.targetThrottle = 1;
else core.thrust.targetThrottle = 0;
status = "Doing deorbit burn.";
}
public override void OnModuleDisabled()
{
core.attitude.attitudeDeactivate();
predictor.users.Remove(this);
predictor.descentSpeedPolicy = null;
core.thrust.ThrustOff();
core.thrust.users.Remove(this);
landStep = LandStep.OFF;
status = "Off";
}
public void LandUntargeted(object controller)
{
users.Add(controller);
deployedGears = false;
// Create a new parachute plan
this.parachutePlan = new ParachutePlan(this);
this.parachutePlan.StartPlanning();
landStep = LandStep.UNTARGETED_DEORBIT;
}
//public interface:
public void LandAtPositionTarget(object controller)
{
users.Add(controller);
predictor.users.Add(this);
vessel.RemoveAllManeuverNodes(); //for the benefit of the landing predictions module
deployedGears = false;
deorbitBurnTriggered = false;
lowDeorbitEndConditionSet = false;
planeChangeTriggered = false;
// Create a new parachute plan
this.parachutePlan = new ParachutePlan(this);
this.parachutePlan.StartPlanning();
if (orbit.PeA < 0) landStep = LandStep.COURSE_CORRECTIONS;
else if (UseLowDeorbitStrategy()) landStep = LandStep.PLANE_CHANGE;
else landStep = LandStep.DEORBIT_BURN;
}
void driveKillHorizontalVelocity(FlightCtrlState s)
{
landStatusString = "Killing horizontal velocity";
if (Vector3d.Dot(Vector3d.Exclude(vesselState.up, vesselState.forward), vesselState.velocityVesselSurface) > 0)
{
s.mainThrottle = 0;
core.attitudeTo(Vector3.up, MechJebCore.AttitudeReference.SURFACE_NORTH, this);
landStep = LandStep.FINAL_DESCENT;
return;
}
//control thrust to control vertical speed:
double desiredSpeed = 0; //hover until horizontal velocity is killed
double controlledSpeed = Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.up);
double speedError = desiredSpeed - controlledSpeed;
double speedCorrectionTimeConstant = 1.0;
double desiredAccel = speedError / speedCorrectionTimeConstant;
double minAccel = -vesselState.localg;
double maxAccel = -vesselState.localg + Vector3d.Dot(vesselState.forward, vesselState.up) * vesselState.maxThrustAccel;
if (maxAccel - minAccel > 0)
{
s.mainThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F);
}
else
{
s.mainThrottle = 0;
}
//angle up and slightly away from vertical:
Vector3d desiredThrustVector = (vesselState.up + 0.2 * Vector3d.Exclude(vesselState.up, vesselState.forward).normalized).normalized;
core.attitudeTo(desiredThrustVector, MechJebCore.AttitudeReference.INERTIAL, this);
}
void FixedUpdateDecelerationBurn()
{
if (vesselState.altitudeASL < DecelerationEndAltitude() + 5)
{
if (UseAtmosphereToBrake()) landStep = LandStep.FINAL_DESCENT;
else landStep = LandStep.KILLING_HORIZONTAL_VELOCITY;
core.warp.MinimumWarp();
return;
}
double decelerationStartTime = (prediction.trajectory.Any() ? prediction.trajectory.First().UT : vesselState.time);
if (decelerationStartTime - vesselState.time > 5)
{
core.thrust.targetThrottle = 0;
status = "Warping to start of braking burn.";
//warp to deceleration start
Vector3d decelerationStartAttitude = -orbit.SwappedOrbitalVelocityAtUT(decelerationStartTime);
decelerationStartAttitude += mainBody.getRFrmVel(orbit.SwappedAbsolutePositionAtUT(decelerationStartTime));
decelerationStartAttitude = decelerationStartAttitude.normalized;
core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, this);
bool warpReady = core.attitude.attitudeAngleFromTarget() < 5;
if (warpReady && core.node.autowarp) core.warp.WarpToUT(decelerationStartTime - 5);
else if (!MuUtils.PhysicsRunning()) core.warp.MinimumWarp();
return;
}
Vector3d desiredThrustVector = -vessel.srf_velocity.normalized;
Vector3d courseCorrection = 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(vessel.srf_velocity, vesselState.up) > 0
|| Vector3d.Dot(vesselState.forward, desiredThrustVector) < 0.75)
{
core.thrust.targetThrottle = 0;
status = "Braking";
}
else
{
double controlledSpeed = vesselState.speedSurface * Math.Sign(Vector3d.Dot(vessel.srf_velocity, vesselState.up)); //positive if we are ascending, negative if descending
double desiredSpeed = -MaxAllowedSpeed();
double desiredSpeedAfterDt = -MaxAllowedSpeedAfterDt(vesselState.deltaT);
double minAccel = -vesselState.localg * Math.Abs(Vector3d.Dot(vessel.srf_velocity.normalized, vesselState.up));
double maxAccel = vesselState.maxThrustAccel * Vector3d.Dot(vesselState.forward, -vessel.srf_velocity.normalized) - vesselState.localg * Math.Abs(Vector3d.Dot(vessel.srf_velocity.normalized, vesselState.up));
const double speedCorrectionTimeConstant = 0.3;
double speedError = desiredSpeed - controlledSpeed;
double desiredAccel = speedError / speedCorrectionTimeConstant + (desiredSpeedAfterDt - desiredSpeed) / vesselState.deltaT;
if (maxAccel - minAccel > 0) core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F);
else core.thrust.targetThrottle = 0;
status = "Braking: target speed = " + Math.Abs(desiredSpeed).ToString("F1") + " m/s";
}
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
}
void driveOnCourse(FlightCtrlState s)
{
landStatusString = "On course for target (safe to warp)";
s.mainThrottle = 0.0F;
core.attitudeTo(Vector3d.back, MechJebCore.AttitudeReference.SURFACE_VELOCITY, this);
//if we are too low for course corrections, start the deceleration burn
if (!courseCorrectionsAllowed()) landStep = LandStep.DECELERATING;
//check if another course correction is needed:
double errorKm = part.vessel.mainBody.Radius / 1000.0 * latLonSeparation(prediction.landingLatitude, prediction.landingLongitude,
targetLatitude, targetLongitude).magnitude;
if (errorKm > 2 * courseCorrectionsAccuracyTargetKm()) landStep = LandStep.COURSE_CORRECTIONS;
}
void FixedUpdateDeorbitBurn()
{
//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 (part.vessel.orbit.ApA < part.vessel.mainBody.RealMaxAtmosphereAltitude())
{
landStep = LandStep.COURSE_CORRECTIONS;
core.thrust.targetThrottle = 0;
return;
}
//We aim for a trajectory that
// a) has the same vertical speed as our current trajectory
// b) has a horizontal speed that will give it a periapsis of -10% of the body's radius
// c) has a heading that points toward where the target will be at the end of free-fall, accounting for planetary rotation
Vector3d horizontalDV = OrbitalManeuverCalculator.DeltaVToChangePeriapsis(orbit, vesselState.time, 0.9 * mainBody.Radius); //Imagine we are going to deorbit now. Find the burn that would lower our periapsis to -10% of the planet's radius
Orbit forwardDeorbitTrajectory = orbit.PerturbedOrbit(vesselState.time, horizontalDV); //Compute the orbit that would put us on
double freefallTime = forwardDeorbitTrajectory.NextTimeOfRadius(vesselState.time, mainBody.Radius) - vesselState.time; //Find how long that orbit would take to impact the ground
double planetRotationDuringFreefall = 360 * freefallTime / mainBody.rotationPeriod; //Find how many degrees the planet will rotate during that time
Vector3d currentTargetRadialVector = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0); //Find the current vector from the planet center to the target landing site
Quaternion freefallPlanetRotation = Quaternion.AngleAxis((float)planetRotationDuringFreefall, mainBody.angularVelocity); //Construct a quaternion representing the rotation of the planet found above
Vector3d freefallEndTargetRadialVector = freefallPlanetRotation * currentTargetRadialVector; //Use this quaternion to find what the vector from the planet center to the target will be when we hit the ground
Vector3d freefallEndTargetPosition = mainBody.position + freefallEndTargetRadialVector; //Then find the actual position of the target at that time
Vector3d freefallEndHorizontalToTarget = Vector3d.Exclude(vesselState.up, freefallEndTargetPosition - vesselState.CoM).normalized; //Find a horizontal unit vector that points toward where the target will be when we hit the ground
Vector3d currentHorizontalVelocity = Vector3d.Exclude(vesselState.up, vessel.obt_velocity); //Find our current horizontal velocity
double finalHorizontalSpeed = (currentHorizontalVelocity + horizontalDV).magnitude; //Find the desired horizontal speed after the deorbit burn
Vector3d finalHorizontalVelocity = finalHorizontalSpeed * freefallEndHorizontalToTarget; //Combine the desired speed and direction to get the desired velocity after the deorbi burn
//Compute the angle between the location of the target at the end of freefall and the normal to our orbit:
Vector3d currentRadialVector = vesselState.CoM - part.vessel.mainBody.position;
double targetAngleToOrbitNormal = Vector3d.Angle(orbit.SwappedOrbitNormal(), freefallEndTargetRadialVector);
targetAngleToOrbitNormal = Math.Min(targetAngleToOrbitNormal, 180 - targetAngleToOrbitNormal);
double targetAheadAngle = Vector3d.Angle(currentRadialVector, freefallEndTargetRadialVector); //How far ahead the target is, in degrees
double planeChangeAngle = Vector3d.Angle(currentHorizontalVelocity, freefallEndHorizontalToTarget); //The plane change required to get onto the deorbit trajectory, in degrees
//If the target is basically almost normal to our orbit, it doesn't matter when we deorbit; might as well do it now
//Otherwise, wait until the target is ahead
if (targetAngleToOrbitNormal < 10
|| (targetAheadAngle < 90 && targetAheadAngle > 60 && planeChangeAngle < 90))
{
deorbitBurnTriggered = true;
}
if (deorbitBurnTriggered)
{
if (!MuUtils.PhysicsRunning()) { core.warp.MinimumWarp(); } //get out of warp
Vector3d deltaV = finalHorizontalVelocity - currentHorizontalVelocity;
core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, this);
if (deltaV.magnitude < 2.0) landStep = LandStep.COURSE_CORRECTIONS;
status = "Doing high deorbit burn";
}
else
{
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT, this);
if (core.node.autowarp) core.warp.WarpRegularAtRate((float)(orbit.period / 10));
status = "Moving to high deorbit burn point";
}
}
protected override void WindowGUI(int windowID)
{
GUILayout.BeginVertical();
GUILayout.BeginHorizontal();
double newTargetLatitude;
if (!dmsInput)
{
bool parseError = false;
if (Double.TryParse(targetLatitudeString, out newTargetLatitude)) targetLatitude = newTargetLatitude;
else parseError = true;
double newTargetLongitude;
if (Double.TryParse(targetLongitudeString, out newTargetLongitude)) targetLongitude = newTargetLongitude;
else parseError = true;
if (targetLatitudeString.ToLower().Equals("the") && targetLongitudeString.ToLower().Equals("beach"))
{
parseError = false;
targetLatitude = BEACH_LATITUDE;
targetLongitude = BEACH_LONGITUDE;
}
GUIStyle labelStyle = new GUIStyle(GUI.skin.label);
if (parseError) labelStyle.normal.textColor = Color.yellow;
GUILayout.Label("Target:", labelStyle, GUILayout.Width(40.0F));
targetLatitudeString = GUILayout.TextField(targetLatitudeString, GUILayout.Width(80));
GUILayout.Label("° N, ");
targetLongitudeString = GUILayout.TextField(targetLongitudeString, GUILayout.Width(80));
GUILayout.Label("° E");
}
else
{
bool parseError = false;
double latDeg, latMin, latSec;
if (Double.TryParse(targetLatitudeDegString, out latDeg)
&& Double.TryParse(targetLatitudeMinString, out latMin)
&& Double.TryParse(targetLatitudeSecString, out latSec))
{
targetLatitude = (dmsNorth ? 1 : -1) * (latDeg + latMin / 60.0 + latSec / 3600.0);
}
else
{
parseError = true;
}
double lonDeg, lonMin, lonSec;
if (Double.TryParse(targetLongitudeDegString, out lonDeg)
&& Double.TryParse(targetLongitudeMinString, out lonMin)
&& Double.TryParse(targetLongitudeSecString, out lonSec))
{
targetLongitude = (dmsEast ? 1 : -1) * (lonDeg + lonMin / 60.0 + lonSec / 3600.0);
}
else
{
parseError = true;
}
GUIStyle labelStyle = new GUIStyle(GUI.skin.label);
if (parseError) labelStyle.normal.textColor = Color.yellow;
GUIStyle compassToggleStyle = new GUIStyle(GUI.skin.button);
compassToggleStyle.padding.bottom = compassToggleStyle.padding.top = 3;
// compassToggleStyle.padding.left = 3;
GUILayout.Label("Target:", labelStyle, GUILayout.Width(40));
targetLatitudeDegString = GUILayout.TextField(targetLatitudeDegString, GUILayout.Width(35));
GUILayout.Label("°");
targetLatitudeMinString = GUILayout.TextField(targetLatitudeMinString, GUILayout.Width(20));
GUILayout.Label("'");
targetLatitudeSecString = GUILayout.TextField(targetLatitudeSecString, GUILayout.Width(20));
GUILayout.Label("''");
if (dmsNorth)
{
if (GUILayout.Button("N", compassToggleStyle, GUILayout.Width(25.0F))) dmsNorth = false;
}
else
{
if (GUILayout.Button("S", compassToggleStyle, GUILayout.Width(25.0F))) dmsNorth = true;
}
targetLongitudeDegString = GUILayout.TextField(targetLongitudeDegString, GUILayout.Width(35));
GUILayout.Label("°");
targetLongitudeMinString = GUILayout.TextField(targetLongitudeMinString, GUILayout.Width(20));
GUILayout.Label("'");
targetLongitudeSecString = GUILayout.TextField(targetLongitudeSecString, GUILayout.Width(20));
GUILayout.Label("''");
if (dmsEast)
{
if (GUILayout.Button("E", compassToggleStyle, GUILayout.Width(25.0F))) dmsEast = false;
}
else
{
if (GUILayout.Button("W", compassToggleStyle, GUILayout.Width(25.0F))) dmsEast = true;
}
}
GUILayout.EndHorizontal();
GUIStyle normalStyle = new GUIStyle(GUI.skin.button);
GUIStyle greenStyle = ARUtils.buttonStyle(Color.green);
GUILayout.BeginHorizontal();
if (gettingMapTarget && !MapView.MapIsEnabled) gettingMapTarget = false;
GUI.SetNextControlName("MapTargetToggle");
gettingMapTarget = GUILayout.Toggle(gettingMapTarget, "Select target on map", (gettingMapTarget ? greenStyle : normalStyle));
if (gettingMapTarget && !MapView.MapIsEnabled) MapView.EnterMapView();
if (part.vessel.mainBody.name.Equals("Kerbin") && GUILayout.Button("Target KSC"))
{
targetLatitude = KSC_LATITUDE;
targetLongitude = KSC_LONGITUDE;
initializeDecimalStrings();
initializeDMSStrings();
}
if (dmsInput)
{
if (GUILayout.Button("DEC"))
{
dmsInput = false;
initializeDecimalStrings();
}
}
else
{
if (GUILayout.Button("DMS"))
{
dmsInput = true;
initializeDMSStrings();
}
}
GUILayout.EndHorizontal();
switch (prediction.outcome)
{
case LandingPrediction.Outcome.LANDED:
if (part.vessel.Landed || part.vessel.Splashed
|| part.vessel.mainBody.maxAtmosphereAltitude > 0 || autoLandAtTarget)
{
String locationString;
if (part.vessel.Landed) locationString = "Landed at ";
else if (part.vessel.Splashed) locationString = "Splashed down at ";
else locationString = "Predicted landing site: ";
if (dmsInput) locationString += dmsLocationString(prediction.landingLatitude, prediction.landingLongitude);
else locationString += String.Format("{0:0.000}° N, {1:0.000}° E", prediction.landingLatitude, prediction.landingLongitude);
GUILayout.Label(locationString);
double eastError = eastErrorKm(prediction.landingLongitude, targetLongitude, targetLatitude);
double northError = northErrorKm(prediction.landingLatitude, targetLatitude);
GUILayout.Label(String.Format("{1:0.0} km " + (northError > 0 ? "north" : "south") +
", {0:0.0} km " + (eastError > 0 ? "east" : "west") + " of target", Math.Abs(eastError), Math.Abs(northError)));
}
//double currentLatitude = part.vessel.mainBody.GetLatitude(vesselState.CoM);
//double currentLongitude = part.vessel.mainBody.GetLongitude(vesselState.CoM);
//if (dmsInput) GUILayout.Label("Current coordinates: " + dmsLocationString(currentLatitude, currentLongitude));
//else GUILayout.Label(String.Format("Current coordinates: {0:0.000}° N, {1:0.000}° E", currentLatitude, ARUtils.clampDegrees(currentLongitude)));
break;
case LandingPrediction.Outcome.AEROBRAKED:
GUILayout.Label("Predicted orbit after aerobreaking:");
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("Apoapsis = {0:0} km", prediction.aerobrakeApoapsis / 1000.0));
GUILayout.Label(String.Format("Periapsis = {0:0} km", prediction.aerobrakePeriapsis / 1000.0));
GUILayout.EndHorizontal();
break;
case LandingPrediction.Outcome.TIMED_OUT:
GUILayout.Label("Reentry simulation timed out.");
break;
case LandingPrediction.Outcome.NO_REENTRY:
GUILayout.Label("Orbit does not re-enter:");
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("Periapsis = {0:0} km", part.vessel.orbit.PeA / 1000.0));
GUILayout.Label(String.Format("Atmosphere height = {0:0} km", part.vessel.mainBody.maxAtmosphereAltitude / 1000.0));
GUILayout.EndHorizontal();
break;
}
if ((prediction.outcome == LandingPrediction.Outcome.LANDED || prediction.outcome == LandingPrediction.Outcome.AEROBRAKED)
&& part.vessel.mainBody.maxAtmosphereAltitude > 0)
{
GUILayout.Label(String.Format("Predicted maximum of {0:0.0} gees", prediction.maxGees));
}
GUILayout.BeginHorizontal();
bool newAutoLand = GUILayout.Toggle(autoLand, "LAND", (autoLand ? greenStyle : normalStyle));
if (newAutoLand && !autoLand)
{
autoLandAtTarget = false;
core.controlClaim(this);
core.landActivate(this, touchdownSpeed);
FlightInputHandler.SetNeutralControls();
}
else if (!newAutoLand && autoLand)
{
core.landDeactivate(this);
core.controlRelease(this);
}
autoLand = newAutoLand;
bool newAutoLandAtTarget = GUILayout.Toggle(autoLandAtTarget, "LAND at target", (autoLandAtTarget ? greenStyle : normalStyle));
if (newAutoLandAtTarget && !autoLandAtTarget)
{
deorbitBurnFirstMessage = false;
gaveUpOnCourseCorrections = false;
core.controlClaim(this);
core.landDeactivate(this);
if (part.vessel.orbit.PeA > -0.1 * part.vessel.mainBody.Radius) landStep = LandStep.DEORBIT_BURN;
else landStep = LandStep.COURSE_CORRECTIONS;
FlightInputHandler.SetNeutralControls();
}
else if (!newAutoLandAtTarget && autoLandAtTarget)
{
turnOffSteering();
core.controlRelease(this);
}
autoLandAtTarget = newAutoLandAtTarget;
showHelpWindow = GUILayout.Toggle(showHelpWindow, "?", new GUIStyle(GUI.skin.button));
GUILayout.EndHorizontal();
if (autoLandAtTarget)
{
GUILayout.Label("Landing step: " + landStatusString);
}
GUIStyle yellow = ARUtils.labelStyle(Color.yellow);
if (gaveUpOnCourseCorrections) GUILayout.Label("Attempted course corrections made the trajectory no longer land. Try a bigger deorbit burn. Or is the target on the wrong side of " + part.vessel.mainBody.name + "?", yellow);
if (tooLittleThrustToLand) GUILayout.Label("Warning: Too little thrust to land.", yellow);
if (deorbitBurnFirstMessage) GUILayout.Label("You must do a deorbit burn before activating \"LAND at target\"", yellow);
GUILayout.BeginHorizontal();
touchdownSpeed = ARUtils.doGUITextInput("Touchdown speed:", 200.0F, touchdownSpeedString, 50.0F, "m/s", 50.0F, out touchdownSpeedString, touchdownSpeed);
core.trans_land_touchdown_speed = touchdownSpeed;
GUILayout.EndHorizontal();
GUILayout.EndVertical();
GUI.DragWindow();
}
void FixedUpdateCoastToDeceleration()
{
core.thrust.targetThrottle = 0;
double maxAllowedSpeed = MaxAllowedSpeed();
if (vesselState.speedSurface > 0.9 * maxAllowedSpeed)
{
core.warp.MinimumWarp();
landStep = LandStep.DECELERATING;
return;
}
double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), LandingSite);
if (currentError > 600)
{
core.warp.MinimumWarp();
landStep = LandStep.COURSE_CORRECTIONS;
return;
}
if (core.attitude.attitudeAngleFromTarget() < 1) { warpReady = true; } // less warp start warp stop jumping
if (core.attitude.attitudeAngleFromTarget() > 5) { warpReady = false; } // hopefully
if (PredictionReady)
{
double decelerationStartTime = (prediction.trajectory.Any() ? prediction.trajectory.First().UT : vesselState.time);
Vector3d decelerationStartAttitude = -orbit.SwappedOrbitalVelocityAtUT(decelerationStartTime);
decelerationStartAttitude += mainBody.getRFrmVel(orbit.SwappedAbsolutePositionAtUT(decelerationStartTime));
decelerationStartAttitude = decelerationStartAttitude.normalized;
core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, this);
}
//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) core.warp.WarpRegularAtRate((float)(vesselState.altitudeASL / (10 * Math.Abs(vesselState.speedVertical))));
else core.warp.MinimumWarp();
status = "Coasting toward deceleration burn";
}
public void LandUntargeted(object controller)
{
users.Add(controller);
deployedGears = false;
landStep = LandStep.UNTARGETED_DEORBIT;
}
void driveCourseCorrections(FlightCtrlState s)
{
landStatusString = "Executing course correction";
//if we are too low to make course corrections, start the deceleration burn
if (!courseCorrectionsAllowed())
{
landStep = LandStep.DECELERATING;
s.mainThrottle = 0;
core.attitudeTo(Vector3d.back, MechJebCore.AttitudeReference.SURFACE_VELOCITY, this);
return;
}
// double errorKm = Math.Sqrt(Math.Pow(eastErrorKm(prediction.landingLongitude, targetLongitude, targetLatitude), 2)
// + Math.Pow(northErrorKm(prediction.landingLatitude, targetLatitude), 2));
//if our predicted landing site is close enough, stop trying to correct it
double errorKm = part.vessel.mainBody.Radius / 1000.0 * latLonSeparation(prediction.landingLatitude, prediction.landingLongitude,
targetLatitude, targetLongitude).magnitude;
if (errorKm < courseCorrectionsAccuracyTargetKm())
{
//course correction complete
landStep = LandStep.ON_COURSE;
s.mainThrottle = 0.0F;
core.attitudeTo(Vector3d.back, MechJebCore.AttitudeReference.SURFACE_VELOCITY, this);
return;
}
//if we haven't calculated the desired correction, just point retrograde
if (!velocityCorrectionSet)
{
s.mainThrottle = 0;
core.attitudeTo(Vector3.back, MechJebCore.AttitudeReference.SURFACE_VELOCITY, this);
return;
}
//do course corrections to try and land at our target:
//project out of correctedSurfaceVelocity - surfaceVelocity any components normal to velA and velB
Vector3d uncorrectedDirection = Vector3d.Cross(velAUnit, velBUnit);
velocityCorrection = Vector3d.Exclude(uncorrectedDirection, velocityCorrection);
Vector3d desiredThrustVector = velocityCorrection.normalized;
core.attitudeTo(desiredThrustVector, MechJebCore.AttitudeReference.INERTIAL, this);
double speedCorrectionTimeConstant = 10.0;
if (Vector3d.Dot(vesselState.forward, desiredThrustVector) > 0.9)
{
if (vesselState.maxThrustAccel > 0) s.mainThrottle = Mathf.Clamp((float)(velocityCorrection.magnitude / (speedCorrectionTimeConstant * vesselState.maxThrustAccel)), 0.0F, 1.0F);
else s.mainThrottle = 0;
}
else
{
s.mainThrottle = 0;
}
if (velocityCorrectionSet)
{
velocityCorrection -= vesselState.deltaT * vesselState.thrustAccel(s.mainThrottle) * vesselState.forward;
}
}
void FixedUpdateLowDeorbitBurn()
{
//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 - DecelerationEndAltitude();
if (triggerDistance < heightAboveTarget)
{
triggerDistance = heightAboveTarget;
}
//See if it's time to start the deorbit burn:
double rangeToTarget = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).magnitude;
if (!deorbitBurnTriggered && rangeToTarget < triggerDistance)
{
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp(true);
}
deorbitBurnTriggered = true;
}
if (deorbitBurnTriggered)
{
status = "Executing low deorbit burn";
}
else
{
status = "Moving to low deorbit burn point";
}
//Warp toward deorbit burn if it hasn't been triggerd yet:
if (!deorbitBurnTriggered && core.node.autowarp && rangeToTarget > 2 * triggerDistance)
{
core.warp.WarpRegularAtRate((float)(orbit.period / 6));
}
if (rangeToTarget < triggerDistance && !MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp();
}
//By default, thrust straight back at max throttle
Vector3d thrustDirection = -vesselState.velocityVesselSurfaceUnit;
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 && PredictionReady)
{
//angle slightly left or right to fix any cross-range error in the predicted landing site:
Vector3d horizontalToLandingSite = Vector3d.Exclude(vesselState.up, LandingSite - vesselState.CoM).normalized;
Vector3d horizontalToTarget = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).normalized;
double angleGain = 4;
Vector3d angleCorrection = angleGain * (horizontalToTarget - horizontalToLandingSite);
if (angleCorrection.magnitude > 0.1)
{
angleCorrection *= 0.1 / angleCorrection.magnitude;
}
thrustDirection = (thrustDirection + angleCorrection).normalized;
double rangeToLandingSite = Vector3d.Exclude(vesselState.up, LandingSite - vesselState.CoM).magnitude;
double maxAllowedSpeed = MaxAllowedSpeed();
if (!lowDeorbitEndConditionSet && Vector3d.Distance(LandingSite, vesselState.CoM) < mainBody.Radius + vesselState.altitudeASL)
{
lowDeorbitEndOnLandingSiteNearer = rangeToLandingSite > rangeToTarget;
lowDeorbitEndConditionSet = true;
}
lowDeorbitBurnMaxThrottle = 1;
if (orbit.PeA < 0)
{
if (rangeToLandingSite > rangeToTarget)
{
if (lowDeorbitEndConditionSet && !lowDeorbitEndOnLandingSiteNearer)
{
landStep = LandStep.DECELERATING;
core.thrust.targetThrottle = 0;
}
double maxAllowedSpeedAfterDt = MaxAllowedSpeedAfterDt(vesselState.deltaT);
double speedAfterDt = vesselState.speedSurface + vesselState.deltaT * Vector3d.Dot(vesselState.gravityForce, vesselState.velocityVesselSurfaceUnit);
double throttleToMaintainLandingSite;
if (vesselState.speedSurface < maxAllowedSpeed)
{
throttleToMaintainLandingSite = 0;
}
else
{
throttleToMaintainLandingSite = (speedAfterDt - maxAllowedSpeedAfterDt) / (vesselState.deltaT * vesselState.maxThrustAccel);
}
lowDeorbitBurnMaxThrottle = throttleToMaintainLandingSite + 1 * (rangeToLandingSite / rangeToTarget - 1) + 0.2;
}
else
{
if (lowDeorbitEndConditionSet && lowDeorbitEndOnLandingSiteNearer)
{
landStep = LandStep.DECELERATING;
core.thrust.targetThrottle = 0;
}
else
{
lowDeorbitBurnMaxThrottle = 0;
status = "Deorbit burn complete: waiting for the right moment to start braking";
}
}
}
}
core.attitude.attitudeTo(thrustDirection, AttitudeReference.INERTIAL, this);
}
void driveDecelerationBurn(FlightCtrlState s)
{
//we aren't right next to the ground but our speed is near or above the nominal deceleration speed
//for the current altitude
if (TimeWarp.CurrentRate > TimeWarp.MaxPhysicsRate) core.warpPhysics(this);
//for atmosphere landings, let the air decelerate us
if (part.vessel.mainBody.maxAtmosphereAltitude > 0)
{
landStatusString = "Decelerating";
s.mainThrottle = 0;
core.attitudeTo(Vector3.back, MechJebCore.AttitudeReference.SURFACE_VELOCITY, this);
//skip deceleration burn and kill-horizontal-velocity for kerbin landings
if (vesselState.altitudeASL < decelerationEndAltitudeASL) landStep = LandStep.FINAL_DESCENT;
return;
}
//vacuum landings:
if (vesselState.altitudeASL < decelerationEndAltitudeASL)
{
landStep = LandStep.KILLING_HORIZONTAL_VELOCITY;
s.mainThrottle = 0;
core.attitudeTo(Vector3.up, MechJebCore.AttitudeReference.SURFACE_NORTH, this);
return;
}
Vector3d desiredThrustVector;
if (Vector3d.Dot(vesselState.velocityVesselSurfaceUnit, vesselState.up) > 0.5)
{
//velocity is close to directly upward. point up
desiredThrustVector = vesselState.up;
}
else if (Vector3d.Dot(vesselState.velocityVesselSurfaceUnit, vesselState.up) > 0)
{
//velocity is upward but not directly upward. point at 45 degrees, leaning retrograde
desiredThrustVector = (vesselState.up + Vector3d.Exclude(vesselState.up, -vesselState.velocityVesselSurfaceUnit).normalized).normalized;
}
else
{
//velocity is downward
desiredThrustVector = -vesselState.velocityVesselSurfaceUnit;
}
if (velocityCorrectionSet)
{
double velACorrection = Vector3d.Dot(velAUnit, velocityCorrection);
double velACorrectionAngle = velACorrection / (vesselState.maxThrustAccel * 5.0); //1 second time constant at max thrust
velACorrectionAngle = Mathf.Clamp((float)velACorrectionAngle, -0.10F, 0.10F);
double velBCorrection = Vector3d.Dot(velBUnit, velocityCorrection);
double velBCorrectionAngle = velBCorrection / (vesselState.maxThrustAccel * 5.0);
velBCorrectionAngle = Mathf.Clamp((float)velBCorrectionAngle, -0.10F, 0.10F);
desiredThrustVector = (desiredThrustVector + velACorrectionAngle * velAUnit + velBCorrectionAngle * velBUnit).normalized;
}
if (Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.up) > 0
|| Vector3d.Dot(vesselState.forward, desiredThrustVector) < 0.75)
{
s.mainThrottle = 0;
}
else
{
double controlledSpeed = vesselState.speedSurface * Math.Sign(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.up)); //positive if we are ascending, negative if descending
double desiredSpeed = -decelerationSpeed(vesselState.altitudeASL, vesselState.maxThrustAccel, part.vessel.mainBody);
double desiredSpeedAfterDt = -decelerationSpeed(vesselState.altitudeASL + vesselState.deltaT * Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.up), vesselState.maxThrustAccel, part.vessel.mainBody);
double minAccel = -vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.velocityVesselSurfaceUnit, vesselState.up));
double maxAccel = vesselState.maxThrustAccel * Vector3d.Dot(vesselState.forward, -vesselState.velocityVesselSurfaceUnit) - vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.velocityVesselSurfaceUnit, vesselState.up));
double speedCorrectionTimeConstant = 0.3;
double speedError = desiredSpeed - controlledSpeed;
double desiredAccel = speedError / speedCorrectionTimeConstant + (desiredSpeedAfterDt - desiredSpeed) / vesselState.deltaT;
if (maxAccel - minAccel > 0) s.mainThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), 0.0F, 1.0F);
else s.mainThrottle = 0;
}
core.attitudeTo(desiredThrustVector, MechJebCore.AttitudeReference.INERTIAL, this);
if (velocityCorrectionSet)
{
Vector3d velocityNormalThrustAccel = Vector3d.Exclude(vesselState.velocityVesselSurface, vesselState.thrustAccel(s.mainThrottle) * vesselState.forward);
velocityCorrection -= vesselState.deltaT * velocityNormalThrustAccel;
}
if (s.mainThrottle == 0.0F) landStatusString = "Preparing for braking burn";
else landStatusString = "Executing braking burn";
}
//public interface:
public void LandAtPositionTarget(object controller)
{
users.Add(controller);
predictor.users.Add(this);
vessel.RemoveAllManeuverNodes(); //for the benefit of the landing predictions module
deployedGears = false;
deorbitBurnTriggered = false;
lowDeorbitEndConditionSet = false;
planeChangeTriggered = false;
if (orbit.PeA < 0) landStep = LandStep.DECELERATING;
else if (UseLowDeorbitStrategy()) landStep = LandStep.PLANE_CHANGE;
else landStep = LandStep.DEORBIT_BURN;
}
void driveDeorbitBurn(FlightCtrlState s)
{
//compute the desired velocity after deorbiting. 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
double horizontalDV = orbitOper.deltaVToChangePeriapsis(-0.1 * part.vessel.mainBody.Radius);
horizontalDV *= Math.Sign(-0.1 * part.vessel.mainBody.Radius - part.vessel.orbit.PeA);
Vector3d currentHorizontal = Vector3d.Exclude(vesselState.up, vesselState.velocityVesselOrbit).normalized;
Vector3d forwardDeorbitVelocity = vesselState.velocityVesselOrbit + horizontalDV * currentHorizontal;
AROrbit forwardDeorbitTrajectory = new AROrbit(vesselState.CoM, forwardDeorbitVelocity, vesselState.time, part.vessel.mainBody);
double freefallTime = projectFreefallEndTime(forwardDeorbitTrajectory, vesselState.time) - vesselState.time;
double planetRotationDuringFreefall = 360 * freefallTime / part.vessel.mainBody.rotationPeriod;
Vector3d currentTargetRadialVector = part.vessel.mainBody.GetRelSurfacePosition(targetLatitude, targetLongitude, 0);
Quaternion freefallPlanetRotation = Quaternion.AngleAxis((float)planetRotationDuringFreefall, part.vessel.mainBody.angularVelocity);
Vector3d freefallEndTargetRadialVector = freefallPlanetRotation * currentTargetRadialVector;
Vector3d currentTargetPosition = part.vessel.mainBody.position + part.vessel.mainBody.Radius * part.vessel.mainBody.GetSurfaceNVector(targetLatitude, targetLongitude);
Vector3d freefallEndTargetPosition = part.vessel.mainBody.position + freefallEndTargetRadialVector;
Vector3d freefallEndHorizontalToTarget = Vector3d.Exclude(vesselState.up, freefallEndTargetPosition - vesselState.CoM).normalized;
double currentHorizontalSpeed = Vector3d.Exclude(vesselState.up, vesselState.velocityVesselOrbit).magnitude;
double finalHorizontalSpeed = currentHorizontalSpeed + horizontalDV;
double currentVerticalSpeed = Vector3d.Dot(vesselState.up, vesselState.velocityVesselOrbit);
Vector3d currentRadialVector = vesselState.CoM - part.vessel.mainBody.position;
Vector3d currentOrbitNormal = Vector3d.Cross(currentRadialVector, vesselState.velocityVesselOrbit);
double targetAngleToOrbitNormal = Math.Abs(Vector3d.Angle(currentOrbitNormal, freefallEndTargetRadialVector));
targetAngleToOrbitNormal = Math.Min(targetAngleToOrbitNormal, 180 - targetAngleToOrbitNormal);
double targetAheadAngle = Math.Abs(Vector3d.Angle(currentRadialVector, freefallEndTargetRadialVector));
double planeChangeAngle = Math.Abs(Vector3d.Angle(currentHorizontal, freefallEndHorizontalToTarget));
if (!deorbiting)
{
if (targetAngleToOrbitNormal < 10
|| (targetAheadAngle < 90 && targetAheadAngle > 60 && planeChangeAngle < 90)) deorbiting = true;
}
if (deorbiting)
{
if (TimeWarp.CurrentRate > TimeWarp.MaxPhysicsRate) core.warpMinimum(this);
Vector3d desiredVelocity = finalHorizontalSpeed * freefallEndHorizontalToTarget + currentVerticalSpeed * vesselState.up;
Vector3d velocityChange = desiredVelocity - vesselState.velocityVesselOrbit;
if (velocityChange.magnitude < 2.0) landStep = LandStep.COURSE_CORRECTIONS;
core.attitudeTo(velocityChange.normalized, MechJebCore.AttitudeReference.INERTIAL, this);
if (core.attitudeAngleFromTarget() < 5) s.mainThrottle = 1.0F;
else s.mainThrottle = 0.0F;
landStatusString = "Executing deorbit burn";
}
else
{
//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 (part.vessel.orbit.ApA < part.vessel.mainBody.maxAtmosphereAltitude)
{
landStep = LandStep.COURSE_CORRECTIONS;
}
else
{
s.mainThrottle = 0.0F;
core.attitudeTo(Vector3d.back, MechJebCore.AttitudeReference.ORBIT, this);
core.warpIncrease(this);
}
landStatusString = "Moving to deorbit burn point";
}
}
void FixedUpdateLowDeorbitBurn()
{
//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 - DecelerationEndAltitude();
if (triggerDistance < heightAboveTarget)
{
triggerDistance = heightAboveTarget;
}
//See if it's time to start the deorbit burn:
double rangeToTarget = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).magnitude;
if (!deorbitBurnTriggered && rangeToTarget < triggerDistance)
{
if (!MuUtils.PhysicsRunning()) core.warp.MinimumWarp(true);
deorbitBurnTriggered = true;
}
if (deorbitBurnTriggered) status = "Executing low deorbit burn";
else status = "Moving to low deorbit burn point";
//Warp toward deorbit burn if it hasn't been triggerd yet:
if (!deorbitBurnTriggered && core.node.autowarp && rangeToTarget > 2 * triggerDistance) core.warp.WarpRegularAtRate((float)(orbit.period / 6));
if (rangeToTarget < triggerDistance && !MuUtils.PhysicsRunning()) core.warp.MinimumWarp();
//By default, thrust straight back at max throttle
Vector3d thrustDirection = -vessel.srf_velocity.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 && PredictionReady)
{
//angle slightly left or right to fix any cross-range error in the predicted landing site:
Vector3d horizontalToLandingSite = Vector3d.Exclude(vesselState.up, LandingSite - vesselState.CoM).normalized;
Vector3d horizontalToTarget = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).normalized;
const double angleGain = 4;
Vector3d angleCorrection = angleGain * (horizontalToTarget - horizontalToLandingSite);
if (angleCorrection.magnitude > 0.1) angleCorrection *= 0.1 / angleCorrection.magnitude;
thrustDirection = (thrustDirection + angleCorrection).normalized;
double rangeToLandingSite = Vector3d.Exclude(vesselState.up, LandingSite - vesselState.CoM).magnitude;
double maxAllowedSpeed = MaxAllowedSpeed();
if (!lowDeorbitEndConditionSet && Vector3d.Distance(LandingSite, vesselState.CoM) < mainBody.Radius + vesselState.altitudeASL)
{
lowDeorbitEndOnLandingSiteNearer = rangeToLandingSite > rangeToTarget;
lowDeorbitEndConditionSet = true;
}
lowDeorbitBurnMaxThrottle = 1;
if (orbit.PeA < 0)
{
if (rangeToLandingSite > rangeToTarget)
{
if (lowDeorbitEndConditionSet && !lowDeorbitEndOnLandingSiteNearer)
{
landStep = LandStep.DECELERATING;
core.thrust.targetThrottle = 0;
}
double maxAllowedSpeedAfterDt = MaxAllowedSpeedAfterDt(vesselState.deltaT);
double speedAfterDt = vesselState.speedSurface + vesselState.deltaT * Vector3d.Dot(vesselState.gravityForce, vessel.srf_velocity.normalized);
double throttleToMaintainLandingSite;
if (vesselState.speedSurface < maxAllowedSpeed) throttleToMaintainLandingSite = 0;
else throttleToMaintainLandingSite = (speedAfterDt - maxAllowedSpeedAfterDt) / (vesselState.deltaT * vesselState.maxThrustAccel);
lowDeorbitBurnMaxThrottle = throttleToMaintainLandingSite + 1 * (rangeToLandingSite / rangeToTarget - 1) + 0.2;
}
else
{
if (lowDeorbitEndConditionSet && lowDeorbitEndOnLandingSiteNearer)
{
landStep = LandStep.DECELERATING;
core.thrust.targetThrottle = 0;
}
else
{
lowDeorbitBurnMaxThrottle = 0;
status = "Deorbit burn complete: waiting for the right moment to start braking";
}
}
}
}
core.attitude.attitudeTo(thrustDirection, AttitudeReference.INERTIAL, this);
}
void FixedUpdatePlaneChange()
{
Vector3d targetRadialVector = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0);
Vector3d currentRadialVector = vesselState.CoM - mainBody.position;
double angleToTarget = Vector3d.Angle(targetRadialVector, currentRadialVector);
bool approaching = Vector3d.Dot(targetRadialVector - currentRadialVector, vessel.obt_velocity) > 0;
if (!planeChangeTriggered && approaching && (angleToTarget > 80) && (angleToTarget < 90))
{
if (!MuUtils.PhysicsRunning()) core.warp.MinimumWarp(true);
planeChangeTriggered = true;
}
if (planeChangeTriggered)
{
Vector3d horizontalToTarget = ComputePlaneChange();
Vector3d finalVelocity = Quaternion.FromToRotation(vesselState.horizontalOrbit, horizontalToTarget) * vessel.obt_velocity;
Vector3d deltaV = finalVelocity - vessel.obt_velocity;
//burn normal+ or normal- to avoid dropping the Pe:
Vector3d burnDir = Vector3d.Exclude(vesselState.up, Vector3d.Exclude(vessel.obt_velocity, deltaV));
planeChangeDVLeft = Math.PI / 180 * Vector3d.Angle(finalVelocity, vessel.obt_velocity) * vesselState.speedOrbitHorizontal;
core.attitude.attitudeTo(burnDir, AttitudeReference.INERTIAL, this);
if (planeChangeDVLeft < 0.1F)
{
landStep = LandStep.LOW_DEORBIT_BURN;
}
status = "Executing low orbit plane change of about " + planeChangeDVLeft.ToString("F0") + " m/s";
}
else
{
if (core.node.autowarp) core.warp.WarpRegularAtRate((float)(orbit.period / 6));
status = "Moving to low orbit plane change burn point";
}
}
