// 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 Update(George george, Vessel vessel, VesselData vdata)
{
UpdateData(george, vessel, vdata);
CheckForSequence(george, vessel, vdata);
}
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?
}
// 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);
}
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 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;
}
}
}
}
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);
}
public static void updateAPPreset(George instance)
{
Instance.activeAPPreset.Update(instance.controllers);
saveToFile();
}
// 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);
}
}
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();
}
/*
* 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;
}
}
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;
}
}