Ejemplo n.º 1
0
        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?
        }
Ejemplo n.º 2
0
        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);
        }
Ejemplo n.º 3
0
        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);
        }
Ejemplo n.º 4
0
        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);
                }
            }
        }
Ejemplo n.º 5
0
        // 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;
        }
Ejemplo n.º 6
0
        // 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);
        }
Ejemplo n.º 7
0
        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);
        }
Ejemplo n.º 8
0
        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);
        }
Ejemplo n.º 9
0
        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();
        }
Ejemplo n.º 10
0
 private void directToWaypoint(WayPoint wp)
 {
     if (flightPlan != null)
     {
         flightPlan.DirectToWaypoint(this, wp, vessel, vesselData);
     }
 }
Ejemplo n.º 11
0
        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;
        }
Ejemplo n.º 12
0
 public void WayPointSequenced(WayPoint wp)
 {
     Deb.Log("WayPointSequenced: {0} {1} {2} {3}", wp.lat, wp.lon, wp.alt, wp.flags);
     UpdateLandingMode(wp);
 }
Ejemplo n.º 13
0
        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);
            }
        }