private void CheckForSequence(George george, Vessel vessel, VesselData vdata)
{
if (next == null)
return;
planet = vessel.mainBody;
var nextNext = next.next;
// do not skip over courses that we have never been on
// otherwise it can auto-sequence through a whole series of things that
// the player meant to fly.
if (!courseStatus.beenValid)
{
return;
}
// have to figure out if we have passed next yet
// looking at distance not good enough, should look for
// either reversal of heading neeeded or closer to next course
// -10 to deal with any round-off error
if (courseStatus.distanceTraveled >= (courseStatus.courseDistance-10))
{
if (nextNext != null)
{
Deb.Log("FlightPlan.Update: traveled past current leg, sequence");
SequenceWaypoint(george, vessel, vdata);
return;
}
else
{
next = null;
}
}
else if (nextNext != null &&
!next.HasFlag(WPFlag.FAF) &&
!next.HasFlag(WPFlag.RW) &&
!next.HasFlag(WPFlag.Stop))
{
CourseStatus nextStatus = new CourseStatus();
CrossTrackError(planet,
next.lat, next.lon,
nextNext.lat, nextNext.lon,
position.lat, position.lon,
nextStatus);
var remaining = courseStatus.currentDistance;
var speed = vessel.srfSpeed;
if (speed > 0)
{
var timeToNext = remaining / speed;
var turnAmount = Math.Abs(nextStatus.courseBearing - courseStatus.courseBearing);
// deal with 350-10
if (turnAmount > 180)
{
turnAmount = 360 - turnAmount;
}
// estimate turn time
var turnRate = 3; // 3 degrees per second gives standard rate turn, should get from vessel
var timeToTurn = turnAmount / turnRate;
if (timeToNext < timeToTurn)
{
Deb.Log("FlightPlan.Update: time to turn (speed {5}, remain {0}, timeToNext {1}, amount {2}, rate {3}, timeToTurn {4}",
remaining, timeToNext, turnAmount, turnRate, timeToTurn, speed);
SequenceWaypoint(george, vessel, vdata);
return;
}
}
}
if (next != null)
{
next.bearing = courseStatus.currentBearing;
next.distance = courseStatus.currentDistance;
}
// ok, we know how far off we are, with dXt, what heading do we need?
// can we use the PID controller for this?
}
private void UpdateWayPoint(CelestialBody planet, WayPoint wp)
{
if (planet == null)
return;
var after = wp.next;
Deb.Log("UpdateWayPoint: {0}, next={1}", wp, after);
if (after == null)
return;
CourseUtils.BearingAndDistance(planet,
wp.lat, wp.lon, after.lat, after.lon,
out after.bearing, out after.distance);
}
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);
}
public void SetFromConfigNode(ConfigNode node)
{
planet = GetCelestialBodyForName(node.GetValue("planet"));
name = node.GetValue("name");
description = node.GetValue("description");
course.Clear();
var courseNode = node.GetNode("WayPoints");
if (courseNode != null)
{
foreach (var wp_node in courseNode.GetNodes("WayPoint"))
{
var wp = new WayPoint();
wp.SetFromConfigNode(wp_node);
course.Add(wp);
}
}
}
// does not clone next!
public WayPoint Clone()
{
var result = new WayPoint();
result.name = String.Copy(name);
result.flags = flags;
result.lat = lat;
result.lon = lon;
result.alt = alt;
return result;
}
// 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 Activate(George george, Vessel vessel, VesselData vdata)
{
planet = vessel.mainBody;
position.lat = vessel.latitude;
position.lon = vessel.longitude;
position.alt = vessel.altitude;
position.SetFlag(WPFlag.Current);
var start = position.Clone();
start.ClearFlag(WPFlag.Active);
start.SetFlag(WPFlag.Flown);
start.SetFlag(WPFlag.Vertical);
start.SetFlag(WPFlag.Current);
start.next = course.FirstOrDefault();
course.Insert(0, start);
prev = null;
next = start;
UpdateWayPointValues(planet);
SequenceWaypoint(george, vessel, vdata);
UpdateWayPointValues(planet);
}
public void AppendWayPoint(WayPoint wp)
{
var last = course.LastOrDefault();
var clone = wp.Clone();
clone.ClearFlag(WPFlag.Active);
clone.ClearFlag(WPFlag.Flown);
course.Add(clone);
if (last != null)
{
last.next = clone;
}
if (planet != null)
UpdateWayPointValues(planet);
}
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();
}
private void directToWaypoint(WayPoint wp)
{
if (flightPlan != null)
{
flightPlan.DirectToWaypoint(this, wp, vessel, vesselData);
}
}
private WayPoint AddNamedPoint(double lat, double lon, double alt, String name,
WPFlag flaga = 0,
WPFlag flagb = 0,
WPFlag flagc = 0,
WPFlag flagd = 0)
{
var wp = new WayPoint();
wp.lat = lat;
wp.lon = lon;
wp.alt = alt;
wp.name = name;
if (flaga != 0) wp.SetFlag(flaga);
if (flagb != 0) wp.SetFlag(flagb);
if (flagc != 0) wp.SetFlag(flagc);
if (flagd != 0) wp.SetFlag(flagd);
flightPlan.AppendWayPoint(wp);
return wp;
}
public void WayPointSequenced(WayPoint wp)
{
Deb.Log("WayPointSequenced: {0} {1} {2} {3}", wp.lat, wp.lon, wp.alt, wp.flags);
UpdateLandingMode(wp);
}
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);
}
}
