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?
}
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);
}
// assumes that cs.fraction is already set
public static void UpdateCourseVertical(CelestialBody planet,
double alt1, double alt2, // course start and end altitudes
double alt3, // current altitude
CourseStatus cs)
{
if (Double.IsNaN(alt1) || Double.IsNaN(alt2) || Double.IsNaN(cs.fraction))
{
return;
}
var frac = Utils.Clamp(cs.fraction, 0, 1);
cs.currentAltitude = alt2 * frac + alt1 * (1.0 - frac);
cs.dVt = alt3 - cs.currentAltitude;
if (cs.courseDistance > 0)
cs.courseSlope = (alt2 - alt1) / cs.courseDistance;
else
cs.courseSlope = Double.NaN;
}
// 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);
}
// course is from lat1,lon1 to lat2,lon2
// check where lat3,lon3 is in respect to that course
public static void CrossTrackError(CelestialBody planet,
double lat1, double lon1, // course start point
double lat2, double lon2, // course end point
double lat3, double lon3, // point we are at
CourseStatus cs)
{
// all from:
// http://www.movable-type.co.uk/scripts/latlong.html
double R = planet.Radius;
double rlat1 = ToRadians(lat1);
double rlon1 = ToRadians(lon1);
double rlat2 = ToRadians(lat2);
double rlon2 = ToRadians(lon2);
double rlat3 = ToRadians(lat3);
double rlon3 = ToRadians(lon3);
// first get the distance from start point to current point
double b13, d13;
BearingAndDistanceRad(planet, rlat1, rlon1, rlat3, rlon3, out b13, out d13);
var dA13 = d13 / R;
double b12;
double d12;
// distance of course on ideal course line
BearingAndDistanceRad(planet, rlat1, rlon1, rlat2, rlon2, out b12, out d12);
// finally distance from point 3 to the end point (point 2)
double b32, d32;
BearingAndDistanceRad(planet, rlat3, rlon3, rlat2, rlon2, out b32, out d32);
cs.courseBearing = ToDegrees(b12);
cs.courseDistance = d12;
// cross track error, sign tells which side of course you are on
// this is valid even when outside start/end points
cs.dXt = Math.Asin(Math.Sin(d13 / R) * Math.Sin(b13 - b12)) * R;
if (d13 > d12)
{
// if the distance d13 is GREATER than d12 it means we are beyond the end point
cs.distanceTraveled = d12;
cs.currentDistance = 0;
}
else
{
// distance along the course that nearest point coresponds to
cs.distanceTraveled = Math.Acos(Math.Cos(dA13) / Math.Cos(cs.dXt / R)) * R;
if (d32 > d12)
{
// if the distance d32 is GREATER than d12 it means we are actually further
// away than the start point (beyond end of start point)
cs.distanceTraveled = -cs.distanceTraveled;
}
else
{
cs.currentDistance = d12 - cs.distanceTraveled;
}
// distance remaining if you were traveling along the course line,
// may be more than direct distance if went beyond start point in
// wrong direction.
cs.currentDistance = d12 - cs.distanceTraveled;
}
cs.directBearing = ToDegrees(b32);
cs.directDistance = d32;
// fraction along course traveled 0 to 1; do not clamp cause we want to be able to detect
// valid range.
cs.fraction = cs.distanceTraveled / cs.courseDistance;
var a = Math.Cos(rlat1) * Math.Cos(rlat2);
var dA12 = cs.courseDistance / R;
var b = Math.Sin(cs.distanceTraveled / R) / Math.Sin(dA12);
var x = a * Math.Cos(rlat1) * Math.Cos(rlon1) + b * Math.Cos(rlat2) * Math.Cos(rlon2);
var y = a * Math.Cos(rlat1) * Math.Sin(rlon1) + b * Math.Cos(rlat2) * Math.Sin(rlon2);
var z = a * Math.Sin(rlat1) + b * Math.Sin(rlat2);
var rlat_i = Math.Atan2(z, Math.Sqrt(x * x + y * y));
var rlon_i = Math.Atan2(y, x);
double ri2, di2; // di2 should match curentDistance
BearingAndDistanceRad(planet, rlat_i, rlon_i, rlat2, rlon2, out ri2, out di2);
// Deb.Log("d12:{0:F2} d13:{1:F2} d32:{2:F2}, cd:{3:F2} di2:{4:F2} fr:{5:F3}",
// d12, d13, d32, cs.currentDistance, di2, cs.fraction);
cs.currentBearing = ToDegrees(ri2);
}
