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