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);
}
}
static int Main(string[] mainargs)
{
if (mainargs.Length == 0)
{
System.Console.Error.WriteLine("usage: VesselSim.exe - computes a solution given initial conditions like SolveText.exe but also simulates a vessel like running inside KSP");
System.Console.Error.WriteLine("vessel options: r0offset=<float>");
System.Console.Error.WriteLine(" v0offset=<float>");
System.Console.Error.WriteLine(" maxT=<float>");
return(1);
}
var solver = new Solve();
var result = new SolveResult();
var traj = new Trajectory();
var controller = new PDController();
bool correct = false;
Vector3d r0 = Vector3d.zero;
Vector3d v0 = Vector3d.zero;
Vector3d rf = Vector3d.zero;
Vector3d vf = Vector3d.zero;
Vector3d r0offset = Vector3d.zero; // move away from compute solution at start
Vector3d v0offset = Vector3d.zero;
int rfaxes = 0;
int vfaxes = 0;
double dt = 0.1;
double maxT = 0;
List <SolveTarget> targets = new List <SolveTarget>();
foreach (var arg in HGUtils.ToKeyValuePairs(mainargs))
{
bool used = solver.Set(arg.Item1, arg.Item2);
if (controller.Set(arg.Item1, arg.Item2))
{
used = true;
}
if (SetTargets(arg.Item1, arg.Item2, ref r0, ref v0, ref rf, ref vf, ref rfaxes, ref vfaxes, ref targets, ref correct))
{
used = true;
}
if (Set(arg.Item1, arg.Item2, ref dt, ref maxT, ref r0offset, ref v0offset))
{
used = true;
}
if (!used)
{
throw new System.ArgumentException("Unexpected argument: " + arg.Item1 + "=" + arg.Item2);
return(1);
}
}
SetFinalTarget(rf, vf, rfaxes, vfaxes, ref targets);
if (targets.Count == 0)
{
System.Console.Error.WriteLine("Error! You must specify targets with either target=[x,y,z], rf=[x,y,z] or vf=[x,y,z]");
return(1);
}
result = MainProg.MultiPartSolve(ref solver, ref traj, r0, v0, ref targets, (float)solver.g, solver.extendTime, correct);
if (result.isSolved())
{
List <string> comments = new List <string>();
System.Console.Error.WriteLine("Writing solution to: solution.dat");
traj.Write("solution.dat", comments);
if (traj.T > maxT)
{
maxT = traj.T;
}
r0 = r0 + r0offset;
v0 = v0 + v0offset;
Simulate(controller, ref traj, r0, v0, new Vector3d(0, -solver.g, 0), dt, maxT);
}
return(0);
}
public void DrawTrack(Trajectory traj, Transform a_transform, float amult = 1)
{
if (traj == null)
{
return;
}
if (_track_obj != null)
{
Destroy(_track_obj); // delete old track
_track_obj = null;
}
if (_thrusts_obj != null)
{
Destroy(_thrusts_obj); // delete old track
_thrusts_obj = null;
}
if (!showTrack)
{
return;
}
// Track
_track_obj = new GameObject("Track");
_track_obj.transform.SetParent(a_transform, false);
MeshFilter meshf = _track_obj.AddComponent <MeshFilter>();
MeshRenderer meshr = _track_obj.AddComponent <MeshRenderer>();
meshr.material = new Material(Shader.Find("KSP/Alpha/Unlit Transparent"));
meshr.material.color = trackcol;
meshr.receiveShadows = false;
meshr.shadowCastingMode = UnityEngine.Rendering.ShadowCastingMode.Off;
Mesh mesh = new Mesh();
Vector3 [] vertices = new Vector3[(traj.Length() - 1) * 4 * 2];
int [] triangles = new int[(traj.Length() - 1) * 4 * 2 * 3]; // number of vertices in tris
float lineWidth = 0.2f;
int v = 0, t = 0;
for (int i = 0; i < traj.Length() - 1; i++)
{
Vector3 p1 = traj.r[i];
Vector3 p2 = traj.r[i + 1];
AddLine(vertices, ref v, triangles, ref t, p1, p2, lineWidth, true);
}
mesh.vertices = vertices;
mesh.triangles = triangles;
meshf.mesh = mesh;
mesh.RecalculateNormals();
// Thrust vectors
_thrusts_obj = new GameObject("Thrusts");
_thrusts_obj.transform.SetParent(a_transform, false);
meshf = _thrusts_obj.AddComponent <MeshFilter>();
meshr = _thrusts_obj.AddComponent <MeshRenderer>();
mesh = new Mesh();
meshr.material = new Material(Shader.Find("KSP/Alpha/Unlit Transparent"));
meshr.material.color = thrustcol;
meshr.receiveShadows = false;
meshr.shadowCastingMode = UnityEngine.Rendering.ShadowCastingMode.Off;
vertices = new Vector3[traj.Length() * 4 * 2];
triangles = new int[traj.Length() * 4 * 2 * 3]; // number of vertices in tris
v = 0;
t = 0;
for (int i = 0; i < traj.Length(); i++)
{
Vector3 p1 = traj.r[i];
Vector3 p2 = traj.r[i] + traj.a[i] * amult;
AddLine(vertices, ref v, triangles, ref t, p1, p2, lineWidth, true);
}
mesh.vertices = vertices;
mesh.triangles = triangles;
meshf.mesh = mesh;
mesh.RecalculateNormals();
}
