Inheritance: KramaxReloadExtensions.ReloadableMonoBehaviour
示例#1
0
        // does not try to sequence
        private void UpdateData(George george, Vessel vessel, VesselData vdata)
        {
            planet = vessel.mainBody;

            // var mypos = vessel.findWorldCenterOfMass();
            double mylat = vessel.latitude;
            double mylon = vessel.longitude;
            double myalt = vessel.altitude;

            position.lat = mylat;
            position.lon = mylon;
            position.alt = myalt;
            position.SetFlag(WPFlag.Current);

            if (prev == null || next == null)
                return;

            double prev_dXt = courseStatus.dXt;
            double prev_distance = courseStatus.currentDistance;

            CrossTrackError(planet,
                            prev.lat, prev.lon,
                            next.lat, next.lon,
                            mylat, mylon,
                            courseStatus);

            // track if this course has ever been valid to avoid sequencing over things entirely
            // without user being aware. Not fraction is clamped 0 to 1 so do not use that.
            if (!courseStatus.beenValid && (0 < courseStatus.fraction && courseStatus.fraction < 1))
            {
                Deb.Log("UpdateData: marking course to {0} with dist {1} as valid", next, courseStatus.distanceTraveled);
                courseStatus.beenValid = true;
            }

            double timestamp = Planetarium.GetUniversalTime();
            double delta_time = timestamp - courseStatus.timestamp;
            courseStatus.timestamp = timestamp;

            // who knows if the sign will be right here; fixme
            // heading is not stable if craft is wallowing
            // courseStatus.vXt = vessel.srfSpeed * Math.Sin(ToRadians(vdata.heading-courseStatus.currentBearing));
            double delta_dXt = courseStatus.dXt - prev_dXt;
            double delta_distance = prev_distance - courseStatus.currentDistance;

            if (delta_time > 0 && !Double.IsNaN(delta_dXt) && !Double.IsNaN(delta_distance))
            {
                courseStatus.vXt = delta_dXt / delta_time;
                courseStatus.vC = delta_distance / delta_time;
            }
            else
            {
                courseStatus.vXt = Double.NaN;
                courseStatus.vC = Double.NaN;
            }

            // Deb.Log("FP: delta_t={2}, dXt={0}, vXt={1}", courseStatus.dXt, courseStatus.vXt, delta_time);
            // Deb.Log("Along track velocity: {0}", courseStatus.vC);

            if (next.HasFlag(WPFlag.Vertical))
            {
                if (prev.HasFlag(WPFlag.Vertical))
                {
                    UpdateCourseVertical(planet, prev.alt, next.alt, myalt, courseStatus);
                }
                else
                {
                    courseStatus.dVt = myalt - next.alt;
                    courseStatus.currentAltitude = next.alt;
                }
            }
            else
            {
                courseStatus.dVt = Double.NaN;
                courseStatus.currentAltitude = Double.NaN;
            }
        }
示例#2
0
 public void Update(George george, Vessel vessel, VesselData vdata)
 {
     UpdateData(george, vessel, vdata);
     CheckForSequence(george, vessel, vdata);
 }
示例#3
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?
        }
示例#4
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);
        }
示例#5
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);
        }
示例#6
0
        public void UpdateFlightPlans(George george, Vessel vessel)
        {
            if (vessel == null)
            {
                current = null;
                plans.Clear();
                planet = null;
            }
            else
            {
                if (!System.Object.ReferenceEquals(current, george.flightPlan))
                {
                    current = george.flightPlan;
                    currentName = String.Copy(current.name);
                    currentDesc = String.Copy(current.description);
                }

                if (planet != vessel.mainBody || plans == null)
                {
                    planet = vessel.mainBody;

                    if (planet != null)
                    {
                        if (flightPlansDict.TryGetValue(planet.name, out plans))
                        {
                            Deb.Log("UpdateFlightPlans: got plans for planet {0}", planet.name);
                        }
                    }
                    else
                    {
                        plans = null;
                    }
                }
            }
        }
示例#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);
        }
示例#8
0
 public static void updateAPPreset(George instance)
 {
     Instance.activeAPPreset.Update(instance.controllers);
     saveToFile();
 }
示例#9
0
 // called on vessel load
 public static void loadCraftAPPreset(George instance)
 {
     if (Instance.craftPresetDict.ContainsKey(FlightGlobals.ActiveVessel.vesselName) &&
         Instance.craftPresetDict[FlightGlobals.ActiveVessel.vesselName].apPreset != null)
     {
         loadAPPreset(Instance.craftPresetDict[FlightGlobals.ActiveVessel.vesselName].apPreset, instance);
     }
     else
     {
         loadAPPreset(Instance.craftPresetDict[craftDefaultName].apPreset, instance);
     }
 }
示例#10
0
        public static void loadAPPreset(APPreset p, George instance)
        {
            APController[] c = instance.controllers;
            for (int i = 0; i < c.Length && i < p.PIDGains.Count; i++)
            {
                c[i].PGain = p.PIDGains[i][0];
                c[i].IGain = p.PIDGains[i][1];
                c[i].DGain = p.PIDGains[i][2];
                c[i].OutMin = p.PIDGains[i][3];
                c[i].OutMax = p.PIDGains[i][4];
                c[i].ClampLower = p.PIDGains[i][5];
                c[i].ClampUpper = p.PIDGains[i][6];
                c[i].Scalar = p.PIDGains[i][7];
                c[i].Easing = p.PIDGains[i][8];
            }

            Instance.activeAPPreset = p;
            GeneralUI.postMessage("Loaded preset " + p.name);

            if (Instance.activeAPPreset != Instance.craftPresetDict[craftDefaultName].apPreset)
                updateCraftPreset(Instance.activeAPPreset, instance.vessel);

            saveToFile();
        }
示例#11
0
        /*
         * Called next.
         */
        public void Start()
        {
            Deb.Log("KramaxAutoPilot: Start {0}", this.GetInstanceID());

            // read in config
            // initialize any variables
            // install any callbacks
            // install in toolbar
            if (UseAppLauncher())
                Kramax.Toolbar.AppLauncherAutoPilot.Start();
            else
                Kramax.Toolbar.ToolbarMod.Start();

            if (mainPilot != null)
            {
                Deb.Err("KramaxAutoPilot.Start: mainPilot stil exists");
            }
            else
            {
                Deb.Log("KramaxAutoPilot.Start: creating mainPilot");
                mainPilot = AddComponent(typeof(George)) as George;
            }
        }
示例#12
0
        public void OnDestroy()
        {
            Deb.Log("KramaxAutoPilot: OnDestroy {0}", this.GetInstanceID());

            if (UseAppLauncher())
                Kramax.Toolbar.AppLauncherAutoPilot.OnDestroy();
            else
                Kramax.Toolbar.ToolbarMod.OnDestroy();

            if (mainPilot)
            {
                Destroy(mainPilot);
                mainPilot = null;
            }
        }