public void SequenceWaypoint(George george, Vessel vessel, VesselData vdata)
{
Deb.Log("SequenceWaypoint: go");
if (next == null)
{
Deb.Log("SequenceWaypoint: no next waypoint.");
return;
}
next.SetFlag(WPFlag.Flown);
var nextNext = next.next;
if (nextNext == null)
return;
// do not carry over values from last course which are invalid at this point
courseStatus = new CourseStatus();
next.ClearFlag(WPFlag.Active);
// do not just reference the waypoint because we want to modify
// it in case we have made an early sequence and altitude is messed up
prev = next.Clone();
next = nextNext;
prev.next = next;
// check prev altitude to make sure we do not dive down or up to
// altitude
if (prev.HasFlag(WPFlag.Vertical))
{
if (next.HasFlag(WPFlag.RW)) // are we on final approach?
{
Deb.Log("SequenceWaypoint: final approach, do not touch altitude");
}
else
{
if (prev.alt < vessel.altitude)
{
Deb.Log("SequenceWaypoint: adjust alt of prev from {0} to {1}",
prev.alt, vessel.altitude);
prev.alt = vessel.altitude;
}
else
{
Deb.Log("SequenceWaypoint: current alt below prev alt, keep it");
}
}
}
else
{
Deb.Log("SequenceWaypoint: prev has no vertical info, so fill in our alt");
prev.SetFlag(WPFlag.Vertical);
prev.alt = vessel.altitude;
}
next.SetFlag(WPFlag.Active);
UpdateData(george, vessel, vdata);
george.WayPointSequenced(next);
CheckForSequence(george, vessel, vdata);
}
private void drawWayPoint(WayPoint wp)
{
if (wp.HasFlag(WPFlag.Current))
return;
GUILayout.BeginHorizontal();
var i = 0;
if (wp.HasFlag(WPFlag.Active))
{
drawField("Active", i++);
}
else
{
if (drawButtonField("D➔", i++)) // ->
{
directToWaypoint(wp);
}
}
if (!String.IsNullOrEmpty(wp.name))
{
drawField(wp.name, i++, true);
}
else
{
var lat = FormatLat(wp.lat, "{0:F2}");
var lon = FormatLon(wp.lon, "{0:F2}");
drawField(String.Format("{0} {1}", lat, lon), i++, true);
}
if (wp.HasFlag(WPFlag.Vertical))
drawField(String.Format("{0:F0}", wp.alt), i++);
else
drawField("", i++);
if (wp.HasFlag(WPFlag.Flown))
{
drawField("", i++);
drawField("", i++);
}
else
{
drawField(String.Format("{0:F0}°", wp.bearing), i++);
drawField(String.Format("{0:F1}", wp.distance / 1000.0), i++);
}
String notes = "";
if (wp.HasFlag(WPFlag.IAF))
notes = "IAF";
else if (wp.HasFlag(WPFlag.FAF))
notes = "FAF";
else if (wp.HasFlag(WPFlag.RW))
notes = "Final";
else if (wp.HasFlag(WPFlag.Stop))
notes = "RW";
drawField(notes, i++);
GUILayout.EndHorizontal();
}
// logic: find waypoint and mark all previous waypoints flown
// insert current pos as first waypoint
// EXCEPTION; if it would by-pass final approach or IAF to FAF we want to go to the leg
public void DirectToWaypoint(George george, WayPoint awp, Vessel vessel, VesselData vdata)
{
Deb.Log("DirectToWaypoint: direct to {0}", next);
if (next != null)
next.ClearFlag(WPFlag.Active);
bool activateLeg = false;
if (awp.HasFlag(WPFlag.RW) || awp.HasFlag(WPFlag.FAF))
{
activateLeg = true;
}
// should not happen, but just in case
if (course.Count == 0)
{
Deb.Err("DirectToWaypoint: flight plan has no waypoint");
return;
}
// need to copy all waypoints and prune out any "current" ones that are in the middle
var newCourse = new List<WayPoint>();
int ndx = -1;
int j = 0; // in case we skip any
WayPoint prev_wp = null;
WayPoint prevToDirect = null;
for (int i = 0; i < course.Count; i++)
{
var wp = course[i];
if (System.Object.ReferenceEquals(wp, awp))
{
// may have skipped so use index of destination
prevToDirect = prev_wp;
ndx = j;
}
if (i > 0 && wp.HasFlag(WPFlag.Current))
continue;
var newWp = wp.Clone();
if (prev_wp != null)
{
prev_wp.next= newWp;
}
prev_wp = newWp;
newCourse.Add(newWp);
j += 1;
}
if (ndx < 0)
{
Deb.Err("DirectToWaypoint: wp {0} not found.", awp);
return;
}
// everything before active wp should be marked flown and inactive
for (int i = 0; i < ndx; i++)
{
var wp = newCourse[i];
wp.SetFlag(WPFlag.Flown);
wp.ClearFlag(WPFlag.Active);
}
// everything after active wp should be marked inactive and NOT flown
for (int i = ndx; i < newCourse.Count; i++)
{
var wp = newCourse[i];
wp.ClearFlag(WPFlag.Flown);
wp.ClearFlag(WPFlag.Active);
}
if (activateLeg)
{
// ignore our position
if (prevToDirect == null)
{
// should not happen, but just in case
Deb.Err("DirectToWaypoint: no previous waypoint for leg!");
return;
}
course = newCourse;
next = course[ndx];
prev = prevToDirect.Clone();
prev.next = next;
next.SetFlag(WPFlag.Active);
}
else
{
// insert a fake waypoint for our current position
course = newCourse;
var current = position.Clone();
current.SetFlag(WPFlag.Vertical);
current.SetFlag(WPFlag.Flown);
current.SetFlag(WPFlag.Current);
next = course[ndx];
course.Insert(ndx, current);
// don't set old_prev to link to current: old_prev.next = current;
prev = current;
prev.next = next;
next.SetFlag(WPFlag.Active);
}
// need fresh status
courseStatus = new CourseStatus();
UpdateWayPointValues(planet);
UpdateData(george, vessel, vdata);
george.WayPointSequenced(next);
}
public void UpdateLandingMode(WayPoint wp)
{
if (wp == null)
{
Deb.Log("UpdateLandingMode: no waypoint");
return;
}
Deb.Log("UpdateLandingMode: {0}", wp);
if (wp.HasFlag(WPFlag.Stop))
{
Deb.Log("UpdateLandingMode: autoland--touchdown sequence started.");
landingModeChanged(LandingMode.Touchdown);
}
else if (wp.HasFlag(WPFlag.IAF))
{
Deb.Log("UpdateLandingMode: autoland--flying to IAF.");
landingModeChanged(LandingMode.ToIAF);
}
else if (wp.HasFlag(WPFlag.FAF))
{
Deb.Log("UpdateLandingMode: autoland--flying to FAF.");
landingModeChanged(LandingMode.ToFAF);
}
else if (wp.HasFlag(WPFlag.RW))
{
Deb.Log("UpdateLandingMode: autoland--on final.");
landingModeChanged(LandingMode.Final);
}
else
{
Deb.Log("WayPointSequenced: autoland--not on approach yet");
landingModeChanged(LandingMode.None);
}
}
