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