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? }
// 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; } }
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 Update(George george, Vessel vessel, VesselData vdata) { UpdateData(george, vessel, vdata); CheckForSequence(george, vessel, vdata); }
// what descent rate do we need to get to glideslope based on current state public double RequiredDescentRate(Vessel vessel, VesselData vesselData) { var speed = vessel.srfSpeed; var error = courseStatus.dVt; // we know the optimal descent slope from courseStatus // that would give us a nominal descent rate // if we are high we need a higher descent rate // if low we need lower descent rate // D = distance from glideslope // T = time in future // GS = glide slope // V = descent rate // GV = descent rate for glide slope // S = speed (assume constant) // In T time we will travel about S * T // Traveling S * T the slope will drop (S*T) * GS // In time T will will drop T * V meters // We want (S*T)*GS to match (T*V) // or (S*T*GS) == (T*V) or // S*GS = GV // But we are D above glideslope so need to drop extra D in time T // // If glideslope at our position is A, then our altitude is A+D // We want to get to an altitude in time T of A + T * (S*GS) // so (A+D)+T*V == A + T * (S*GS) // A+D+T*V == A + T * S * GS // D+T*V = T*S*GS // T*V = T*S*GS-D // V = S*GS - D/T // So to get there in 10 seconds we need to subtract D/10 from optimal glide slope rate if (speed <= 0) return 0; if (!Double.IsNaN(error) && !Double.IsNaN(courseStatus.courseSlope)) { var nominalRate = courseStatus.courseSlope * speed; var timeToIntercept = 5; var adjust = (-error / timeToIntercept); // if adjust is negative we will be going faster and so in fact we need more delta // if positive we will be going slower so need less var up_fudge = 0.8; var down_fudge = 1.2; if (adjust < 0) adjust = adjust * down_fudge; else adjust = adjust * up_fudge; var result = nominalRate + adjust; // Deb.Log("RequiredDescentRate: slp {5}, nom {0}, T {1}, error {2}, adj {3}, result {4}", // nominalRate, timeToIntercept, error, adjust, result, courseStatus.courseSlope, speed); return result; } else { return Double.NaN; } }
// If on glideslope what decent rate do we want given our speed public double GlideSlopeDescentRate(Vessel vessel, VesselData vesselData) { if (Double.IsNaN(courseStatus.courseSlope)) return 0; // maybe return NaN; fixme var speed = vessel.srfSpeed; if (speed > 0) { // need meters per second to follow glideslope // so slope is V/H dimensionless // multiply by H speed in M/S gives V speed in M/S return courseStatus.courseSlope * speed; } else { return 0; // maybe return NaN; fixme } }
// 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); }
private void SwitchVessels(Vessel v) { if (v == null) { Deb.Log("George.SwitchVessels: null vessel"); if (vessel != null) UnhookVessel(vessel); vesselData = null; vessel = null; } Deb.Log("George.SwitchVessels: new vessel {0}", v); if (vessel == v) { Deb.Log("George.switchVessels: same vessel"); return; } // so the on vessel change gets called with things that fall off during staging! // we do not want to control those and leave the main vessel. // hopefully by watching on active vessel we will be OK. if (!v.isActiveVessel || v.isEVA) { Deb.Log("George.switchVessels: new vessel is not active one or EVA, ignore."); return; } var lastVessel = vessel; vessel = v; if (lastVessel != null) UnhookVessel(lastVessel); if (vessel.mainBody == null) { Deb.Err("George.SwitchVessels: vessel has no main body"); } // update to use this new vessel vesselData = new VesselData(); vesselData.updateAttitude(vessel); vessel.OnPreAutopilotUpdate += new FlightInputCallback(PreAutoPilotUpdate); vessel.OnPostAutopilotUpdate += new FlightInputCallback(PostAutoPilotUpdate); if (HrztActive) InputLockManager.SetControlLock(ControlTypes.YAW, yawLockID); if (VertActive) InputLockManager.SetControlLock(ControlTypes.PITCH, pitchLockID); PresetManager.loadCraftAPPreset(this); // test flight plane; fixme if (flightPlan != null) flightPlan.Activate(this, vessel, vesselData); }
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); }