static private void EulerStep( double dt, Vessel vessel, Vector3d r, // in world space but relative to body.position Vector3d v, Vector3d att, double totalMass, double minThrust, double maxThrust, Trajectories.VesselAerodynamicModel aeroModel, CelestialBody body, double t, BLController controller, Vector3d tgt_r, double aeroFudgeFactor, out Vector3d steer, out Vector3d vel_air, out double throttle, out Vector3d out_r, out Vector3d out_v) { 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)); // Get steer and throttle bool bailOutLandingBurn = true; if (controller != null) { bool landingGear; controller.GetControlOutputs(vessel, totalMass, r, v, att, minThrust, maxThrust, t, body, true, out throttle, out steer, out landingGear, bailOutLandingBurn); // Stop throttle so we don't take off again in timestep, dt // TODO - Fix HACK!! if (y < controller.TgtAlt + 50) { throttle = 0; } } Vector3d Ft = Vector3d.zero; if (throttle > 0) { Ft = steer * (minThrust + throttle * (maxThrust - minThrust)); } // TODO: Do repeated calls to GetForces() mess up PID controllers which updates their internal estimates? vel_air = v - body.getRFrmVel(r + body.position); if (aeroModel == null) { Debug.Log("EulerStep() - No aeroModel"); } Vector3d F = aeroModel.GetForces(body, r, vel_air, Math.PI) * aeroFudgeFactor + Ft; Vector3d a = F / totalMass + g; out_r = r + v * dt + 0.5 * a * dt * dt; out_v = v + a * dt; }
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 void Fly(FlightCtrlState state) { double throttle = last_throttle; Vector3d steer = last_steer; double minThrust; double maxThrust; KSPUtils.ComputeMinMaxThrust(vessel, out minThrust, out maxThrust); Vector3d tgt_r = vessel.mainBody.GetWorldSurfacePosition(tgtLatitude, tgtLongitude, tgtAlt); bool landingGear = false; bool bailOutLandingBurn = true; // cut thrust if near ground and have too much thrust to reach ground double dt = vessel.missionTime - last_t; string msg = ""; if ((dt > 0.1) || (vessel.altitude < tgtAlt + 500)) { msg = controller.GetControlOutputs(vessel, vessel.GetTotalMass(), vessel.GetWorldPos3D() - vessel.mainBody.position, vessel.GetObtVelocity(), vessel.transform.up, minThrust, maxThrust, controller.vessel.missionTime, vessel.mainBody, false, out throttle, out steer, out landingGear, bailOutLandingBurn, debug); last_throttle = throttle; last_steer = steer; last_t = vessel.missionTime; } if ((landingGear) && (!reportedLandingGear)) { KSPUtils.DeployLandingGear(vessel); if (vessel == FlightGlobals.ActiveVessel) { GuiUtils.ScreenMessage(Localizer.Format("#BoosterGuidance_DeployingLandingGear")); } } if ((msg != "") && (vessel == FlightGlobals.ActiveVessel)) { GuiUtils.ScreenMessage(msg); } if (vessel.checkLanded()) { DisableGuidance(); state.mainThrottle = 0; return; } // Set active engines in landing burn if (controller.phase == BLControllerPhase.LandingBurn) { if (controller.landingBurnEngines != null) { foreach (ModuleEngines engine in KSPUtils.GetAllEngines(vessel)) { if (controller.landingBurnEngines.Contains(engine)) { if (!engine.isOperational) { engine.Activate(); } } else { if (engine.isOperational) { engine.Shutdown(); } } } } } // Draw predicted position if controlling that vessel if (vessel == FlightGlobals.ActiveVessel) { double lat, lon, alt; // prediction is for position of planet at current time compensating for // planet rotation vessel.mainBody.GetLatLonAlt(controller.predBodyRelPos + controller.vessel.mainBody.position, out lat, out lon, out alt); alt = vessel.mainBody.TerrainAltitude(lat, lon); // Make on surface Targets.RedrawPrediction(vessel.mainBody, lat, lon, alt + 1); // 1m above ground Targets.DrawSteer(vessel.vesselSize.x * Vector3d.Normalize(steer), null, Color.green); } state.mainThrottle = (float)throttle; vessel.Autopilot.SAS.lockedMode = false; vessel.Autopilot.SAS.SetTargetOrientation(steer, false); }