static void SetFinalTarget(Vector3d rf, Vector3d vf, int rfaxes, int vfaxes, ref List <SolveTarget> targets)
{
// Add final target if set
if ((rfaxes != 0) || (vfaxes != 0))
{
SolveTarget final = new SolveTarget();
final.r = rf;
final.raxes = rfaxes;
final.v = vf;
final.vaxes = vfaxes;
final.t = -1; // unset
targets.Add(final);
}
}
static bool SetTargets(string key, string value, ref Vector3d r0, ref Vector3d v0, ref Vector3d rf, ref Vector3d vf, ref int rfaxes, ref int vfaxes, ref List <SolveTarget> targets, ref bool correct)
{
float t = -1;
if (key == "--correct")
{
correct = true;
}
else if (key == "r0")
{
r0 = HGUtils.ToVector3d(value);
}
else if (key == "v0")
{
v0 = HGUtils.ToVector3d(value);
}
else if (key == "rf")
{
rf = HGUtils.ToVector3d(value);
rfaxes = SolveTarget.X | SolveTarget.Y | SolveTarget.Z;
}
else if (key == "vf")
{
vf = HGUtils.ToVector3d(value);
vfaxes = SolveTarget.X | SolveTarget.Y | SolveTarget.Z;
}
// TODO: Handle : to specify time
else if (key == "target")
{
SolveTarget tgt = new SolveTarget();
tgt.r = HGUtils.ToVector3d(value);
tgt.raxes = SolveTarget.X | SolveTarget.Y | SolveTarget.Z;
tgt.vaxes = 0;
tgt.t = t;
targets.Add(tgt);
}
else
{
return(false);
}
return(true);
}
public void EnableLandAtTarget()
{
if (_tgts.Count == 0)
{
ScreenMessages.PostScreenMessage("No targets", 3.0f, ScreenMessageStyle.UPPER_CENTER);
autoMode = AutoMode.Off;
return;
}
checkingLanded = false; // stop trajectory being cancelled while on ground
lowestY = FindLowestPointOnVessel();
Vector3d r0 = vessel.GetWorldPos3D();
Vector3d v0 = vessel.GetSrfVelocity();
Vector3d g = FlightGlobals.getGeeForceAtPosition(r0);
Vector3d vf = new Vector3d(0, -touchDownSpeed, 0);
ComputeMinMaxThrust(out _minThrust, out _maxThrust);
if (_maxThrust == 0)
{
ScreenMessages.PostScreenMessage("No engine thrust (activate engines)", 3.0f, ScreenMessageStyle.UPPER_CENTER);
autoMode = AutoMode.Off;
return;
}
_startTime = Time.time;
double amin = _minThrust / vessel.totalMass;
double amax = _maxThrust / vessel.totalMass;
if (amin > g.magnitude)
{
ScreenMessages.PostScreenMessage("Difficult to land as thrust of engine is greater than gravity", 3.0f, ScreenMessageStyle.UPPER_CENTER);
}
if (amin > amax * 0.95)
{
ScreenMessages.PostScreenMessage("Engine doesn't appear to be throttleable. This makes precision guidance impossible", 3.0f, ScreenMessageStyle.UPPER_CENTER);
autoMode = AutoMode.Off;
return;
}
solver = new Solve();
solver.Tmin = 1;
solver.tol = 0.5;
solver.vmax = maxV;
solver.amin = amin * (1 + errMargin);
solver.amax = amax * (1 - errMargin);
solver.minDurationPerThrust = 4;
solver.maxThrustsBetweenTargets = 3;
solver.g = g.magnitude;
solver.minDescentAngle = minDescentAngle;
solver.maxThrustAngle = maxThrustAngle * (1 - 2 * errMargin);
solver.maxLandingThrustAngle = 0.1f * maxThrustAngle; // 10% of max thrust angle
// hack for large craft to allow extra slowdown time at target to prepare for next target
// where thrust is just over gravity give 5 seconds extra time
// where thrust is double gravity than use 0.5 secs extra time
solver.extraTime = (float)(2.5 - 2 * Math.Min(0.5 * (amax / g.magnitude), 1));
// Shut-off throttle
FlightCtrlState ctrl = new FlightCtrlState();
vessel.GetControlState(ctrl);
ctrl.mainThrottle = (_keepIgnited)?0.01f:0;
// Compute trajectory to landing spot
List <SolveTarget> targets = new List <SolveTarget>();
Vector3d tr0 = _transform.InverseTransformPoint(r0);
tr0.y += 0.1f; // move up slightly to ensure above ground plane
Vector3d tv0 = _transform.InverseTransformVector(v0);
// Create list of solve targets
double d = 0;
Vector3 cr = tr0;
Vector3d wrf = Vector3d.zero;
Vector3d wvf = Vector3d.zero;
for (int i = 0; i < _tgts.Count; i++)
{
SolveTarget tgt = new SolveTarget();
Vector3d pos = vessel.mainBody.GetWorldSurfacePosition(_tgts[i].lat, _tgts[i].lon, _tgts[i].alt + _tgts[i].height);
tgt.r = _transform.InverseTransformPoint(pos); // convert to local (for orientation)
tgt.raxes = SolveTarget.X | SolveTarget.Y | SolveTarget.Z;
if (i == _tgts.Count - 1) // final target
{
tgt.r.y += -lowestY;
wrf = _transform.TransformPoint(tgt.r);
tgt.vaxes = SolveTarget.X | SolveTarget.Y | SolveTarget.Z;
tgt.v = vf;
wvf = _transform.TransformVector(tgt.v); // to correct final later
}
tgt.t = -1;
d = d + (cr - tgt.r).magnitude;
cr = tgt.r;
targets.Add(tgt);
}
solver.Tmax = -1; // Forces estimation given initial position, velocity and targets
solver.apex = targets[targets.Count - 1].r;
_traj = new Trajectory();
SolveResult result = MainProg.MultiPartSolve(ref solver, ref _traj, tr0, tv0, ref targets, (float)g.magnitude, extendTime);
Log(solver.DumpString() + " " + result.DumpString());
if (result.isSolved()) // solved for complete path?
{
string msg = String.Format("Found solution T={0:F1} Fuel={1:F1}", result.T, result.fuel);
Log(msg, true);
// Enable autopilot
_pid3d.Init(corrFactor, ki1, 0, corrFactor * kP2Scale, ki2, 0, maxV, (float)amax, yMult);
// TODO - Testing out using in solution co-ordinates
DrawTargets(_tgts, _transform, targetcol, tgtSize);
vessel.Autopilot.Enable(VesselAutopilot.AutopilotMode.StabilityAssist);
vessel.OnFlyByWire += new FlightInputCallback(Fly);
// Write solution
if (_logging)
{
List <string> comments = new List <string>();
comments.Add(result.DumpString());
// Thrusts
List <float> thrust_times = new List <float>();
for (int i = 0; i < result.thrusts.Length; i++)
{
thrust_times.Add(result.thrusts[i].t);
}
comments.Add("thrust_times=" + String.Join(",", thrust_times));
if (result.checktimes != null)
{
comments.Add("check_times=" + String.Join(",", result.checktimes));
}
_traj.Write(_solutionLogFilename, comments);
}
autoMode = AutoMode.LandAtTarget;
Events["ToggleGuidance"].guiName = "Cancel guidance";
}
else
{
DisableLand();
Events["ToggleGuidance"].guiName = "Failed! - Cancel guidance";
string msg = "Failure to find solution as ";
// Do some checks
if (v0.magnitude > maxV)
{
msg = msg + " velocity over " + maxV + " m/s";
}
else if (amax < g.magnitude)
{
msg = msg + "engine has insufficient thrust, no engines active or no fuel";
}
else if (amin > g.magnitude)
{
msg = msg + "can't throttle engine low enough to descent";
}
else
{
msg = msg + "impossible to reach target within constraints";
}
Log(msg, true);
autoMode = AutoMode.Failed;
}
if (result.isSolved()) // solved for complete path? - show partial?
{
DrawTrack(_traj, _transform);
}
}
