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