public void SetPhase(BLControllerPhase phase)
{
if (controller != null)
{
controller.SetPhase(phase);
}
}
public void SetPhase(BLControllerPhase a_phase)
{
minError = double.MaxValue; // reset so boostback doesn't give up
lowestY = KSPUtils.FindLowestPointOnVessel(vessel); // in case its changed
// Current phase unset and specified phase unset then find out suitable phase
// otherwise use already set phase
if ((phase == BLControllerPhase.Unset) && (a_phase == BLControllerPhase.Unset))
{
if (vessel.altitude > reentryBurnAlt)
{
phase = BLControllerPhase.BoostBack;
}
else
{
phase = BLControllerPhase.AeroDescent;
}
}
else
{
phase = a_phase;
}
if (fp != null)
{
LogSimulation();
}
}
void EnableGuidance(BLControllerPhase phase)
{
BoosterGuidanceCore core = BoosterGuidanceCore.GetBoosterGuidanceCore(FlightGlobals.ActiveVessel);
KSPActionParam param = new KSPActionParam(KSPActionGroup.None, KSPActionType.Activate);
core.useFAR = hasFAR;
Debug.Log("[BoosterGuidance] Vessel=" + FlightGlobals.ActiveVessel.name + " useFAR=" + core.useFAR);
core.EnableGuidance(param);
core.SetPhase(phase);
}
public BLController(BLController v) : base()
{
phase = v.phase;
vessel = v.vessel;
tgtLatitude = v.tgtLatitude;
tgtLongitude = v.tgtLongitude;
tgtAlt = v.tgtAlt;
lowestY = v.lowestY;
reentryBurnAlt = v.reentryBurnAlt;
reentryBurnMaxAoA = v.reentryBurnMaxAoA;
reentryBurnSteerKp = v.reentryBurnSteerKp;
reentryBurnTargetSpeed = v.reentryBurnTargetSpeed;
aeroModel = v.aeroModel;
aeroDescentSteerKp = v.aeroDescentSteerKp;
landingBurnHeight = v.landingBurnHeight;
landingBurnAMax = v.landingBurnAMax;
landingBurnSteerKp = v.landingBurnSteerKp;
landingBurnEngines = v.landingBurnEngines;
suicideFactor = v.suicideFactor;
targetError = v.targetError;
igniteDelay = v.igniteDelay;
noSteerHeight = v.noSteerHeight;
setLandingEnginesDone = false;
}
static public Vector3d ToGround(double tgtAlt, Vessel vessel, Trajectories.VesselAerodynamicModel aeroModel, CelestialBody body, BLController controller,
Vector3d tgt_r, out double T, string logFilename = "", Transform logTransform = null, double timeOffset = 0, double maxT = 600)
// Changes step size, dt, based on the amount of deacceleration forces, aero or thrust and winds back to choose smaller timesteps
{
float ang;
Quaternion bodyRotation;
System.IO.StreamWriter f = null;
if (logFilename != "")
{
f = new System.IO.StreamWriter(logFilename);
f.WriteLine("time x y z vx vy vz ax ay az att_err target_error total_mass");
f.WriteLine("# tgtAlt=" + tgtAlt);
}
T = 0;
Vector3d r = vessel.GetWorldPos3D() - body.position;
Vector3d v = vessel.GetObtVelocity();
Vector3d a = Vector3d.zero;
Vector3d last_r = r;
Vector3d last_v = v;
BLControllerPhase last_phase = controller.phase;
double minThrust, maxThrust;
double totalMass = vessel.totalMass;
// Initially thrust is for all operational engines
KSPUtils.ComputeMinMaxThrust(vessel, out minThrust, out maxThrust);
double y = r.magnitude - body.Radius;
// TODO: att should be supplied as vessel transform will be wrong in simulation
Vector3d att = new Vector3d(vessel.transform.up.x, vessel.transform.up.y, vessel.transform.up.z);
double targetError = 0;
if (controller != null)
{
// Take target error from previously calculated trajectory
// We would know this at the end but can't wait until then
targetError = controller.targetError;
}
// Use small dt all the way when below 5000m
double dt_max = (y > 5000) ? dt_space : 1;
double dt = dt_max;
double last_T = T;
while ((y > tgtAlt) && (T < maxT))
{
y = r.magnitude - body.Radius;
double dy = (r + v * dt).magnitude - body.Radius - y;
if (y + dy < controller.reentryBurnAlt)
{
dt = dt_reentry;
}
if ((controller.phase == BLControllerPhase.AeroDescent) || (controller.phase == BLControllerPhase.LandingBurn))
{
dt = Math.Min(dt_aero, dt_max);
}
if (f != null)
{
// NOTE: Cancel out rotation of planet
ang = (float)((-T) * body.angularVelocity.magnitude / Math.PI * 180.0);
// Rotation 1 second earlier
float prevang = (float)((-(T - 1)) * body.angularVelocity.magnitude / Math.PI * 180.0);
// Consider body rotation at this time
bodyRotation = Quaternion.AngleAxis(ang, body.angularVelocity.normalized);
Quaternion prevbodyRotation = Quaternion.AngleAxis(prevang, body.angularVelocity.normalized);
Vector3d tr = bodyRotation * r;
Vector3d tr1 = prevbodyRotation * r;
Vector3d tr2 = bodyRotation * (r + v);
Vector3d ta = bodyRotation * a;
tr = logTransform.InverseTransformPoint(tr + body.position);
Vector3d tv = logTransform.InverseTransformVector(tr2 - tr1);
ta = logTransform.InverseTransformVector(ta);
f.WriteLine(string.Format("{0} {1:F5} {2:F5} {3:F5} {4:F5} {5:F5} {6:F5} {7:F1} {8:F1} {9:F1} 0 {10:F2} {11:F2}", T + timeOffset, tr.x, tr.y, tr.z, tv.x, tv.y, tv.z, ta.x, ta.y, ta.z, targetError, totalMass));
}
if ((y < body.atmosphereDepth) || (y < controller.reentryBurnAlt + 1500 * dt))
{
dt = Math.Min(dt, 2);
}
Vector3d vel_air;
Vector3d steer;
double throttle;
double aeroFudgeFactor = 1.05; // Assume aero forces 5% higher which causes overshoot of target and more vertical final descent
Vector3d out_r;
Vector3d out_v;
// Compute time step change in r and v
EulerStep(dt, vessel, r, v, att, totalMass, minThrust, maxThrust, aeroModel, body, T, controller, tgt_r, aeroFudgeFactor, out steer, out vel_air, out throttle, out out_r, out out_v);
y = r.magnitude - body.Radius;
att = steer; // assume can turn immediately
last_phase = controller.phase;
last_r = r;
last_v = v;
last_T = T;
r = out_r;
v = out_v;
T = T + dt;
}
if (T > maxT)
{
Debug.Log("[BoosterGuidance] Simulation time exceeds maxT=" + maxT);
}
// Correct to point of intersect on surface
double vy = Vector3d.Dot(last_v, Vector3d.Normalize(r));
double p = 0;
if (vy < -0.1)
{
p = (tgtAlt - y) / -vy; // Backup proportion
r = r - last_v * p;
T = T - p;
}
if (f != null)
{
f.Close();
}
// Compensate for body rotation giving world position in the surface point now
// that would be hit in the future
ang = (float)((-T) * body.angularVelocity.magnitude / Math.PI * 180.0);
bodyRotation = Quaternion.AngleAxis(ang, body.angularVelocity.normalized);
r = bodyRotation * r;
return(r);
}
bool MainTab(int windowID)
{
bool targetChanged = false;
BoosterGuidanceCore core = CheckCore(FlightGlobals.ActiveVessel);
BLControllerPhase phase = core.Phase();
// Target:
// Draw any Controls inside the window here
GUILayout.Label(Localizer.Format("#BoosterGuidance_Target"));//Target coordinates:
GUILayout.BeginHorizontal();
double step = 1.0 / (60 * 60); // move by 1 arc second
tgtLatitude.DrawEditGUI(EditableAngle.Direction.NS);
if (GUILayout.Button("▲"))
{
tgtLatitude += step;
targetChanged = true;
}
if (GUILayout.Button("▼"))
{
tgtLatitude -= step;
targetChanged = true;
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
tgtLongitude.DrawEditGUI(EditableAngle.Direction.EW);
if (GUILayout.Button("◄"))
{
tgtLongitude -= step;
targetChanged = true;
}
if (GUILayout.Button("►"))
{
tgtLongitude += step;
targetChanged = true;
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button(Localizer.Format("#BoosterGuidance_PickTarget")))
{
PickTarget();
}
if (GUILayout.Button("Set Here"))
{
SetTargetHere();
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
showTargets = GUILayout.Toggle(showTargets, Localizer.Format("#BoosterGuidance_ShowTargets"));
bool prevLogging = core.logging;
// TODO
string filename = FlightGlobals.ActiveVessel.name;
filename = filename.Replace(" ", "_");
filename = filename.Replace("(", "");
filename = filename.Replace(")", "");
core.logFilename = filename;
core.logging = GUILayout.Toggle(core.logging, Localizer.Format("#BoosterGuidance_Logging"));
if (core.Enabled())
{
if ((!prevLogging) && (core.logging)) // logging switched on
{
core.StartLogging();
}
if ((prevLogging) && (!core.logging)) // logging switched off
{
core.StopLogging();
}
}
GUILayout.EndHorizontal();
// Info box
GUILayout.BeginHorizontal();
GUILayout.Label(core.Info());
GUILayout.EndHorizontal();
// Boostback
SetEnabledColors((phase == BLControllerPhase.BoostBack) || (phase == BLControllerPhase.Unset));
GUILayout.BeginHorizontal();
if (GUILayout.Button(new GUIContent(Localizer.Format("#BoosterGuidance_Boostback"), "Enable thrust towards target when out of atmosphere")))
{
EnableGuidance(BLControllerPhase.BoostBack);
}
GUILayout.EndHorizontal();
// Coasting
SetEnabledColors((phase == BLControllerPhase.Coasting) || (phase == BLControllerPhase.Unset));
GUILayout.BeginHorizontal();
if (GUILayout.Button(new GUIContent(Localizer.Format("#BoosterGuidance_Coasting"), "Turn to retrograde attitude and wait for Aero Descent phase")))
{
EnableGuidance(BLControllerPhase.Coasting);
}
GUILayout.EndHorizontal();
// Re-Entry Burn
SetEnabledColors((phase == BLControllerPhase.ReentryBurn) || (phase == BLControllerPhase.Unset));
GUILayout.BeginHorizontal();
if (GUILayout.Button(new GUIContent(Localizer.Format("#BoosterGuidance_ReentryBurn"), "Ignite engine on re-entry to reduce overheating")))
{
EnableGuidance(BLControllerPhase.ReentryBurn);
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GuiUtils.SimpleTextBox(Localizer.Format("#BoosterGuidance_EnableAltitude"), reentryBurnAlt, "m", 65);
core.reentryBurnAlt = reentryBurnAlt;
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GuiUtils.SimpleTextBox(Localizer.Format("#BoosterGuidance_TargetSpeed"), reentryBurnTargetSpeed, "m/s", 40);
core.reentryBurnTargetSpeed = reentryBurnTargetSpeed;
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Steer", GUILayout.Width(40));
core.reentryBurnSteerKp = Mathf.Clamp(core.reentryBurnSteerKp, 0, maxReentryGain);
core.reentryBurnSteerKp = GUILayout.HorizontalSlider(core.reentryBurnSteerKp, 0, maxReentryGain);
GUILayout.Label(((int)(core.reentryBurnMaxAoA)).ToString() + "°(max)", GUILayout.Width(60));
GUILayout.EndHorizontal();
// Aero Descent
SetEnabledColors((phase == BLControllerPhase.AeroDescent) || (phase == BLControllerPhase.Unset));
GUILayout.BeginHorizontal();
if (GUILayout.Button(new GUIContent(Localizer.Format("#BoosterGuidance_AeroDescent"), "No thrust aerodynamic descent, steering with gridfins within atmosphere")))
{
EnableGuidance(BLControllerPhase.AeroDescent);
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Steer", GUILayout.Width(40));
core.aeroDescentSteerKp = Mathf.Clamp(core.aeroDescentSteerKp, 0, maxAeroDescentGain);
core.aeroDescentSteerKp = GUILayout.HorizontalSlider(core.aeroDescentSteerKp, 0, maxAeroDescentGain); // max turn 2 degrees for 100m error
GUILayout.Label(((int)core.aeroDescentMaxAoA).ToString() + "°(max)", GUILayout.Width(60));
GUILayout.EndHorizontal();
// Landing Burn
SetEnabledColors((phase == BLControllerPhase.LandingBurn) || (phase == BLControllerPhase.Unset));
GUILayout.BeginHorizontal();
if (GUILayout.Button(Localizer.Format("#BoosterGuidance_LandingBurn")))
{
EnableGuidance(BLControllerPhase.LandingBurn);
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#BoosterGuidance_EnableAltitude"));
String text = "n/a";
if (core.Enabled())
{
if (core.LandingBurnHeight() > 0)
{
text = ((int)(core.LandingBurnHeight() + tgtAlt)).ToString() + "m";
}
else
{
if (core.LandingBurnHeight() < 0)
{
text = Localizer.Format("#BoosterGuidance_TooHeavy");
}
}
}
GUILayout.Label(text, GUILayout.Width(60));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(Localizer.Format("#BoosterGuidance_Engines"));
if (numLandingBurnEngines == Localizer.Format("#BoosterGuidance_Current"))
{
GUILayout.Label(numLandingBurnEngines);
}
else
{
GUILayout.Label(numLandingBurnEngines);
}
if (numLandingBurnEngines == "current") // Save active engines
{
if (GUILayout.Button(Localizer.Format("#BoosterGuidance_Set"))) // Set to currently active engines
{
numLandingBurnEngines = core.SetLandingBurnEngines();
}
}
else
{
if (GUILayout.Button(Localizer.Format("#BoosterGuidance_Unset"))) // Set to currently active engines
{
numLandingBurnEngines = core.UnsetLandingBurnEngines();
}
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Steer", GUILayout.Width(40));
core.landingBurnSteerKp = Mathf.Clamp(core.landingBurnSteerKp, 0, maxLandingBurnGain);
core.landingBurnSteerKp = GUILayout.HorizontalSlider(core.landingBurnSteerKp, 0, maxLandingBurnGain);
string max = Localizer.Format("#BoosterGuidance_Max");
GUILayout.Label(((int)(core.landingBurnMaxAoA)).ToString() + "°(" + max + ")", GUILayout.Width(60));
GUILayout.EndHorizontal();
// Activate guidance
SetEnabledColors(true); // back to normal
GUILayout.BeginHorizontal();
if (!core.Enabled())
{
if (GUILayout.Button(Localizer.Format("#BoosterGuidance_EnableGuidance")))
{
core.EnableGuidance();
}
}
else
{
if (GUILayout.Button(Localizer.Format("#BoosterGuidance_DisableGuidance")))
{
core.DisableGuidance();
}
}
GUILayout.EndHorizontal();
GUI.DragWindow();
return((GUI.changed) || targetChanged);
}
public override string GetControlOutputs(
Vessel vessel,
double totalMass,
Vector3d r, // world pos relative to body
Vector3d v, // world velocity
Vector3d att, // attitude
double minThrust, double maxThrust,
double t,
CelestialBody body,
bool simulate, // if true just go retrograde (no corrections)
out double throttle, out Vector3d steer,
out bool landingGear, // true if landing gear requested (stays true)
bool bailOutLandingBurn = false, // if set too true on RO set throttle=0 if thrust > gravity at landing
bool showCpuTime = false)
{
// height of lowest point with additional margin
double y = r.magnitude - body.Radius - tgtAlt - touchdownMargin + lowestY;
Vector3d up = Vector3d.Normalize(r);
Vector3d vel_air = v - body.getRFrmVel(r + body.position);
double vy = Vector3d.Dot(vel_air, up);
double amin = minThrust / totalMass;
double amax = maxThrust / totalMass;
float minThrottle = 0.01f;
BLControllerPhase lastPhase = phase;
Vector3d tgt_r = body.GetWorldSurfacePosition(tgtLatitude, tgtLongitude, tgtAlt) - body.position;
System.Diagnostics.Stopwatch timer = new System.Diagnostics.Stopwatch();
timer.Start();
string msg = ""; // return message about the status
landingGear = (y < deployLandingGearHeight) && (deployLandingGear);
double g = FlightGlobals.getGeeForceAtPosition(r + body.position).magnitude;
double dt = t - lastt;
// No thrust - retrograde relative to surface (default and Coasting phase
throttle = 0;
steer = -Vector3d.Normalize(vel_air);
Vector3d error = Vector3d.zero;
attitudeError = 0;
if ((!simulate) && (y > noSteerHeight))
{
BLController tc = new BLController(this);
// Only simulate phases beyond boostback so boostback minimizes error and simulate includes just
// the remaining phases and doesn't try to redo reentry burn for instance
if (phase == BLControllerPhase.BoostBack)
{
tc.phase = BLControllerPhase.Coasting;
}
predBodyRelPos = Simulate.ToGround(tgtAlt, vessel, aeroModel, body, tc, tgt_r, out targetT);
landingBurnAMax = tc.landingBurnAMax;
landingBurnHeight = tc.landingBurnHeight; // Update from simulation
error = predBodyRelPos - tgt_r;
error = Vector3d.Exclude(up, error); // make sure error is horizontal
targetError = error.magnitude;
}
// BOOSTBACK
if (phase == BLControllerPhase.BoostBack)
{
// Aim to close max of 20% of error in 1 second
steer = -Vector3d.Normalize(error);
// Safety checks in inverse cosine
attitudeError = HGUtils.angle_between(att, steer);
double dv = error.magnitude / targetT; // estimated delta V needed
double ba = 0;
if (attitudeError < 10 + dv * 0.5) // more accuracy needed when close to target
{
ba = Math.Max(0.3 * dv, 10 / targetT);
}
throttle = Mathf.Clamp((float)((ba - amin) / (0.01 + amax - amin)), minThrottle, 1);
// Stop if error has grown significantly
if ((targetError > minError * 1.5) || (targetError < 10))
{
if (targetError < 5000) // check if error changes dramatically but still far from target
{
phase = BLControllerPhase.Coasting;
msg = Localizer.Format("#BoosterGuidance_SwitchedToCoasting");
}
}
minError = Math.Min(targetError, minError);
if ((y < reentryBurnAlt) && (vy < 0)) // falling
{
phase = BLControllerPhase.ReentryBurn;
msg = Localizer.Format("#BoosterGuidance_SwitchedToReentryBurn");
}
// TODO - Check for steer in 180 degrees as interpolation wont work
steer = Vector3d.Normalize(att * 0.75 + steer * 0.25); // simple interpolation to damp rapid oscillations
}
// COASTING
if (phase == BLControllerPhase.Coasting)
{
if ((y < reentryBurnAlt) && (vy < 0))
{
phase = BLControllerPhase.ReentryBurn;
msg = Localizer.Format("#BoosterGuidance_SwitchedToReentryBurn");
}
}
// Set default gains for steering
steerGain = 0;
// RE-ENTRY BURN
if (phase == BLControllerPhase.ReentryBurn)
{
double errv = vel_air.magnitude - reentryBurnTargetSpeed;
if (errv > 0)
{
double smooth = HGUtils.LinearMap((double)y, (double)reentryBurnAlt, (double)reentryBurnAlt - 4000, 0, 1);
// Limit maximum de-acceleration to make the simulation accuracy when dt=2 or 4 secs
double da = g + Math.Min(Math.Max(errv * 0.3, 10), 50); // attempt to cancel 30% of extra velocity in 1 sec and min of 10m/s/s
// Use of dt prevents too high throttle when simulating re-entry burn with dt=2 or 4 secs.
double newThrottle = smooth * (da - amin) / (0.01 + amax - amin);
throttle = HGUtils.Clamp(newThrottle, minThrottle, 1);
}
else
{
phase = BLControllerPhase.AeroDescent;
msg = Localizer.Format("#BoosterGuidance_SwitchedToAeroDescent");
}
if (!simulate)
{
pid_reentry.kp = reentryBurnSteerKp * CalculateSteerGain(throttle, vel_air, r, y, totalMass, false);
steerGain = pid_reentry.kp;
double ang = pid_reentry.Update(error.magnitude, Time.deltaTime);
steer = -Vector3d.Normalize(vel_air) + GetSteerAdjust(error, ang, reentryBurnMaxAoA);
}
}
// desired velocity - used in AERO DESCENT and LANDING BURN
double dvy = -touchdownSpeed;
double av = Math.Max(0.1, landingBurnAMax - g);
// AERO DESCENT
if (phase == BLControllerPhase.AeroDescent)
{
if (!simulate)
{
pid_aero.kp = aeroDescentSteerKp * CalculateSteerGain(0, vel_air, r, y, totalMass, false);
steerGain = pid_aero.kp;
double ang = pid_aero.Update(error.magnitude, dt);
steer = -Vector3d.Normalize(vel_air) + GetSteerAdjust(error, ang, aeroDescentMaxAoA);
}
double landingMinThrust, landingMaxThrust;
KSPUtils.ComputeMinMaxThrust(vessel, out landingMinThrust, out landingMaxThrust, false, landingBurnEngines);
double newLandingBurnAMax = landingMaxThrust / totalMass;
if (Math.Abs(landingBurnAMax - newLandingBurnAMax) > 0.5)
{
landingBurnAMax = landingMaxThrust / totalMass; // update so we don't continually recalc
landingBurnHeight = Simulate.CalculateLandingBurnHeight(tgtAlt, r, v, vessel, totalMass, landingMinThrust, landingMaxThrust, aeroModel, vessel.mainBody, this, 100, "", suicideFactor);
}
if (y - vel_air.magnitude * igniteDelay <= landingBurnHeight) // Switch to landing burn N secs earlier to allow RO engine start up time
{
lowestY = KSPUtils.FindLowestPointOnVessel(vessel);
phase = BLControllerPhase.LandingBurn;
msg = Localizer.Format("#BoosterGuidance_SwitchedToLandingBurn");
}
// Interpolate to avoid rapid swings
steer = Vector3d.Normalize(att * 0.75 + steer * 0.25); // simple interpolation to damp rapid oscillations
}
// LANDING BURN (suicide burn)
if (phase == BLControllerPhase.LandingBurn)
{
if ((landingBurnEngines != null) && (!setLandingEnginesDone) && (!simulate))
{
KSPUtils.SetActiveEngines(vessel, landingBurnEngines);
msg = string.Format(Localizer.Format("#BoosterGuidance_SetXEnginesForLanding", landingBurnEngines.Count.ToString()));
setLandingEnginesDone = true;
}
av = Math.Max(0.1, amax - g); // wrong on first iteration
if (y > 0)
{
dvy = -Math.Sqrt((1 + suicideFactor) * av * y) - touchdownSpeed; // Factor is 2 for perfect suicide burn, lower for margin and hor vel
}
if (amax > 0)
{
double err_dv = vy - dvy; // +ve is velocity too high
double da = g - 0.3 * (err_dv / dt); // required accel to change vy in about the next three timesteps, cancel out g (only works if vertical)
throttle = HGUtils.Clamp((da - amin) / (0.01 + amax - amin), minThrottle, 1);
// compensate if not vertical as need more vertical component of thrust
throttle = HGUtils.Clamp(throttle / Math.Max(0.1, Vector3.Dot(att, up)), minThrottle, 1);
}
if ((!simulate) && (y > noSteerHeight))
{
double ang;
pid_landing.kp = landingBurnSteerKp * CalculateSteerGain(throttle, vel_air, r, y, totalMass, false);
steerGain = pid_landing.kp;
ang = pid_landing.Update(error.magnitude, Time.deltaTime);
// Steer retrograde with added up component to damp oscillations at slow speed near ground
steer = -Vector3d.Normalize(vel_air - 20 * up) + GetSteerAdjust(error, ang, landingBurnMaxAoA);
}
else
{
// Just cancel velocity with significant upwards component to stay upright
steer = -Vector3d.Normalize(vel_air - 20 * up);
}
if ((y < noSteerHeight) && (!noSteerReported))
{
msg = string.Format(Localizer.Format("#BoosterGuidance_NoSteerHeightReached"));
noSteerReported = true;
}
// Decide to shutdown engines for final touch down? (within 3 secs)
// Criteria should be if
// height
double minHeight = KSPUtils.MinHeightAtMinThrust(y, vy, amin, g);
// Criteria for shutting down engines
// - we could not reach ground at minimum thrust (would ascend)
// - falling less than touchdown speed (otherwise can decide to shutdown engines when still high and travelling fast)
// This is particulary done to stop the simulation never hitting the ground and making pretty circles through the sky
// until the maximum time is exceeded. The predicted impact position will vary widely and this was incur a lot of time to calculate
// - this is very tricky to get right for the actual vessel since in RO engines take time to throttle down, so it needs to be done
// early, allowing for the fact the residual engine thrust will slow the rocket more for the next 2-3 secs
// - the engine will restart again if landing doesn't happen with 2-3 secs
bool cant_reach_ground = (minHeight > 0) && (vy > -50);
if ((cant_reach_ground) && (bailOutLandingBurn))
{
throttle = 0;
}
// Interpolate to avoid rapid swings
steer = Vector3d.Normalize(att * 0.75 + steer * 0.25); // simple interpolation to damp rapid oscillations
}
// Logging
if (fp != null)
{
Vector3d a = att * (amin + throttle * (amax - amin)); // this assumes engine is ignited though
Vector3d tr = logTransform.InverseTransformPoint(r + body.position);
Vector3d tv = logTransform.InverseTransformVector(vel_air);
Vector3d ta = logTransform.InverseTransformVector(a);
fp.WriteLine("{0:F1} {1} {2:F1} {3:F1} {4:F1} {5:F1} {6:F1} {7:F1} {8:F1} {9:F1} {10:F1} {11:F1} {12:F1} {13:F1} {14:F3} {15:F1} {16:F2}", t - logStartTime, phase, tr.x, tr.y, tr.z, tv.x, tv.y, tv.z, a.x, a.y, a.z, attitudeError, amin, amax, steerGain, targetError, totalMass);
logLastTime = t;
}
lastt = t;
steer = Vector3d.Normalize(steer);
attitudeError = HGUtils.angle_between(att, steer);
throttle = HGUtils.Clamp(throttle, 0, 1);
// Log simulate to ground when phase changes
// So the logging is done at the start of the new phase
if ((lastPhase != phase) && (fp != null))
{
LogSimulation();
}
elapsed_secs = timer.ElapsedMilliseconds * 0.001;
// Set info message
string tgtErrStr;
if (targetError > 1000)
{
if (targetError > 100000)
{
tgtErrStr = string.Format("{0:F0}km", targetError * 0.001);
}
else
{
if (targetError > 10000)
{
tgtErrStr = string.Format("{0:F1}km", targetError * 0.001);
}
else
{
tgtErrStr = string.Format("{0:F2}km", targetError * 0.001);
}
}
}
else
{
tgtErrStr = string.Format("{0:F0}m", targetError);
}
if (vessel.checkLanded())
{
info = string.Format(Localizer.Format("#BoosterGuidance_LandedXFromTarget", tgtErrStr));
}
else
{
string s1 = tgtErrStr;
string s2 = string.Format("{0:F0}", attitudeError);
string s3 = string.Format("{0:F0}", targetT);
string s4 = string.Format("{0:F0}", elapsed_secs * 1000);
if (showCpuTime)
{
info = string.Format(Localizer.Format("#BoosterGuidance_ErrorXTimeXCPUX", s1, s2, s3, s4));
}
else
{
info = string.Format(Localizer.Format("#BoosterGuidance_ErrorXTimeX", s1, s2, s3));
}
}
if (msg != "")
{
info = msg;
}
return(msg);
}
