static private Vector3d GetForces(Vessel vessel, Vector3d r, Vector3d v, Vector3d att, double totalMass, double minThrust, double maxThrust,
Trajectories.VesselAerodynamicModel aeroModel, CelestialBody body, double t, double dt,
BLController controller, Vector3d tgt_r, double aeroFudgeFactor,
out Vector3d steer, out Vector3d vel_air, out double throttle)
{
Vector3d F = Vector3d.zero;
double y = r.magnitude - body.Radius;
steer = -Vector3d.Normalize(v);
throttle = 0;
// gravity
double R = r.magnitude;
Vector3d g = r * (-body.gravParameter / (R * R * R));
float lastAng = (float)((-1) * body.angularVelocity.magnitude / Math.PI * 180.0);
Quaternion lastBodyRot = Quaternion.AngleAxis(lastAng, body.angularVelocity.normalized);
vel_air = v - body.getRFrmVel(r + body.position);
if (controller != null)
{
bool bailOutLandingBurn = true;
bool simulate = true;
bool landingGear;
controller.GetControlOutputs(vessel, totalMass, r, v, att, minThrust, maxThrust, t, body, simulate, out throttle, out steer, out landingGear, bailOutLandingBurn);
if (throttle > 0)
{
F = steer * (minThrust + throttle * (maxThrust - minThrust));
}
att = steer; // assume attitude is always correct
}
F = F + aeroModel.GetForces(body, r, vel_air, Math.PI) * aeroFudgeFactor; // retrograde
F = F + g * totalMass;
return(F);
}
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;
}
// Simulate trajectory to ground and work out point to fire landing burn assuming air resistance will help slow the vessel down
// This point will be MUCH later than thrust would be applied minus air resistance
// Height is used to mean the height above the target altitude
static public double CalculateLandingBurnHeight(double tgtAlt, Vector3d r, Vector3d v, Vessel vessel, double totalMass, double minThrust, double maxThrust, Trajectories.VesselAerodynamicModel aeroModel, CelestialBody body,
BLController controller = null, double maxT = 600, string filename = "", double suicideFactor = 0.8f)
{
double T = 0;
double y = r.magnitude - body.Radius;
double amin = minThrust / totalMass;
double amax = maxThrust / totalMass;
double LandingBurnHeight = -1;
Vector3d att = -Vector3d.Normalize(v);
double touchdownSpeed = 2;
System.IO.StreamWriter f = null;
if (filename != "")
{
f = new System.IO.StreamWriter(filename);
f.WriteLine("time y vy dvy");
}
double dt = dt_aero; // was 0.5
while ((y > tgtAlt) && (T < maxT))
{
y = r.magnitude - body.Radius;
// Get all forces, i.e. aero-dynamic and thrust
Vector3d steer, vel_air;
double throttle;
double aeroFudgeFactor = 1;
// Need to simulate reentry burn to get reduced mass and less velocity
// could probably approximation this well without much effort though
Vector3d F = GetForces(vessel, r, v, -Vector3d.Normalize(v), totalMass, minThrust, maxThrust, aeroModel, body, T, dt, null, Vector3d.zero, aeroFudgeFactor, out steer, out vel_air, out throttle);
double R = r.magnitude;
Vector3d g = r * (-body.gravParameter / (R * R * R));
// Calculate suicide burn velocity
//Debug.Log("[BoosterGuidance g=" + g);
double av = amax - g.magnitude;
if (av < 0)
{
av = 0.1; // pretend we have more thrust to look like we are doing something rather than giving up!!
}
// dvy in 2 seconds time (allowing time for engine start up)
double dvy = Math.Sqrt((1 + suicideFactor) * av * (y - tgtAlt)) + touchdownSpeed;
// Find latest point when velocity is less than desired velocity
// as it means it is too high in the next time step meaning this is the time to
// apply landing burn thrust
if (dvy > vel_air.magnitude)
{
LandingBurnHeight = y - tgtAlt;
}
if (f != null)
{
f.WriteLine(string.Format("{0} {1:F1} {2:F1} {3:F1}", T, y, vel_air.magnitude, dvy));
}
// Equations of motion
Vector3d a = (F / totalMass);
r = r + v * dt + 0.5 * a * dt * dt;
v = v + a * dt;
T = T + dt;
}
if (T > maxT)
{
Debug.Log("[BoosterGuidance] Simulation time exceeds maxT=" + maxT);
}
if (f != null)
{
f.WriteLine("# LandingBurnHeight=" + LandingBurnHeight + " amax=" + amax);
f.Close();
}
return(LandingBurnHeight);
}
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);
}
public void RemoveController(BLController controller)
{
controllers.Remove(controller);
}
public void AddController(BLController controller)
{
controllers.Add(controller);
}
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);
}
