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