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); }