protected override void Update()
{
if (CFG.Nav.Paused)
{
return;
}
//differentiate between flying in formation and going to the target
var vdistance = 0f; //vertical distance to the target
if (CFG.Nav[Navigation.FollowTarget])
{
update_formation_info();
}
else
{
VSL.Altitude.LowerThreshold = (float)CFG.Target.Pos.Alt;
if (ALT != null && CFG.VF[VFlight.AltitudeControl] && CFG.AltitudeAboveTerrain)
{
vdistance = VSL.Altitude.LowerThreshold + CFG.DesiredAltitude - VSL.Altitude.Absolute;
}
tVSL = null;
tPN = null;
}
//calculate direct distance
var vdir = Vector3.ProjectOnPlane(CFG.Target.GetTransform().position + formation_offset - VSL.Physics.wCoM, VSL.Physics.Up);
var hdistance = Utils.ClampL(vdir.magnitude - VSL.Geometry.R, 0);
var bearing_threshold = Utils.Clamp(1 / VSL.Torque.MaxCurrent.AngularAccelerationAroundAxis(VSL.Engines.CurrentDefThrustDir),
C.BearingCutoffCos, 0.98480775f); //10deg yaw error
//update destination
if (tPN != null && tPN.Valid && !tPN.VSL.Info.Destination.IsZero())
{
VSL.Info.Destination = tPN.VSL.Info.Destination;
}
else
{
VSL.Info.Destination = vdir;
}
//handle flying in formation
var tvel = Vector3.zero;
var vel_is_set = false;
var end_distance = CFG.Target.AbsRadius;
var dvel = VSL.HorizontalSpeed.Vector;
if (CFG.Target.Land)
{
end_distance /= 4;
}
if (tVSL != null && tVSL.loaded)
{
if (formation_offset.IsZero())
{
end_distance *= all_followers.Count / 2f;
}
tvel = Vector3d.Exclude(VSL.Physics.Up, tVSL.srf_velocity);
dvel -= tvel;
var tvel_m = tvel.magnitude;
var dir2vel_cos = Vector3.Dot(vdir.normalized, tvel.normalized);
var lat_dir = Vector3.ProjectOnPlane(vdir - VSL.HorizontalSpeed.Vector * C.LookAheadTime, tvel);
var lat_dist = lat_dir.magnitude;
FormationBreakTimer.RunIf(() => keep_formation = false,
tvel_m < C.FormationSpeedCutoff);
Maneuvering = CanManeuver && lat_dist > CFG.Target.AbsRadius && hdistance < CFG.Target.AbsRadius * 3;
if (keep_formation && tvel_m > 0 &&
(!CanManeuver ||
dir2vel_cos <= bearing_threshold ||
lat_dist < CFG.Target.AbsRadius * 3))
{
if (CanManeuver)
{
HSC.AddWeightedCorrection(lat_dir.normalized * Utils.ClampH(lat_dist / CFG.Target.AbsRadius, 1) *
tvel_m * C.FormationFactor * (Maneuvering? 1 : 0.5f));
}
hdistance = Utils.ClampL(Mathf.Abs(dir2vel_cos) * hdistance - VSL.Geometry.R, 0);
if (dir2vel_cos < 0)
{
if (hdistance < CFG.Target.AbsRadius)
{
HSC.AddRawCorrection(tvel * Utils.Clamp(-hdistance / CFG.Target.AbsRadius * C.FormationFactor, -C.FormationFactor, 0));
}
else if (Vector3.Dot(vdir, dvel) < 0 &&
(dvel.magnitude > C.FollowerMaxAwaySpeed ||
hdistance > CFG.Target.AbsRadius * 5))
{
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(vdir);
return;
}
else
{
HSC.AddRawCorrection(tvel * (C.FormationFactor - 1));
}
hdistance = 0;
}
vdir = tvel;
}
}
//if the distance is greater that the threshold (in radians), use the Great Circle navigation
if (hdistance / VSL.Body.Radius > C.DirectNavThreshold)
{
var next = CFG.Target.PointFrom(VSL.vessel, 0.1);
hdistance = (float)CFG.Target.DistanceTo(VSL.vessel);
vdir = Vector3.ProjectOnPlane(VSL.Body.GetWorldSurfacePosition(next.Lat, next.Lon, VSL.vessel.altitude)
- VSL.vessel.transform.position, VSL.Physics.Up);
tvel = Vector3.zero;
}
else if (!VSL.IsActiveVessel && hdistance > GLB.UnpackDistance)
{
VSL.SetUnpackDistance(hdistance * 1.2f);
}
vdir.Normalize();
//check if we have arrived to the target and stayed long enough
if (hdistance < end_distance)
{
if (CFG.Nav[Navigation.FollowTarget])
{
var prev_needed_speed = VSL.HorizontalSpeed.NeededVector.magnitude;
if (prev_needed_speed < 1 && !CFG.HF[HFlight.Move])
{
CFG.HF.OnIfNot(HFlight.Move);
}
else if (prev_needed_speed > 10 && !CFG.HF[HFlight.NoseOnCourse])
{
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
}
if (tvel.sqrMagnitude > 1)
{
//set needed velocity and starboard to match that of the target
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(tvel);
HSC.AddRawCorrection((tvel - VSL.HorizontalSpeed.Vector) * 0.9f);
}
else
{
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
}
vel_is_set = true;
}
else
{
CFG.HF.OnIfNot(HFlight.Move);
VSL.Altitude.DontCorrectIfSlow();
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
vel_is_set = true;
if (vdistance <= 0 && vdistance > -VSL.Geometry.R)
{
if (CFG.Nav[Navigation.FollowPath] && CFG.Path.Count > 0)
{
if (CFG.Path.Peek() == CFG.Target)
{
if (CFG.Path.Count > 1)
{
CFG.Path.Dequeue();
if (on_arrival())
{
return;
}
start_to(CFG.Path.Peek());
return;
}
if (ArrivedTimer.TimePassed)
{
CFG.Path.Clear();
if (on_arrival())
{
return;
}
anchor_at_target();
return;
}
}
else
{
if (on_arrival())
{
return;
}
start_to(CFG.Path.Peek());
return;
}
}
else if (ArrivedTimer.TimePassed)
{
if (on_arrival())
{
return;
}
finish();
return;
}
}
}
}
else
{
ArrivedTimer.Reset();
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
//if we need to make a sharp turn, stop and turn, then go on
var heading_dir = Vector3.Dot(VSL.OnPlanetParams.Heading, vdir);
var hvel_dir = Vector3d.Dot(VSL.HorizontalSpeed.normalized, vdir);
var sharp_turn_allowed = !CFG.Nav[Navigation.FollowTarget] ||
hdistance < end_distance *Utils.ClampL(all_followers.Count / 2, 2);
if (heading_dir < bearing_threshold &&
hvel_dir < bearing_threshold &&
sharp_turn_allowed)
{
SharpTurnTimer.Start();
}
if (SharpTurnTimer.Started)
{
VSL.HorizontalSpeed.SetNeeded(vdir);
Maneuvering = false;
vel_is_set = true;
if (heading_dir < bearing_threshold || sharp_turn_allowed &&
VSL.HorizontalSpeed.Absolute > 1 && Math.Abs(hvel_dir) < C.BearingCutoffCos)
{
SharpTurnTimer.Restart();
}
else if (SharpTurnTimer.TimePassed)
{
SharpTurnTimer.Reset();
}
}
// Log("timer: {}\nheading*dir {} < {}, vel {} > 1, vel*dir {} < {}",
// SharpTurnTimer,
// Vector3.Dot(VSL.OnPlanetParams.Heading, vdir), bearing_threshold,
// VSL.HorizontalSpeed.Absolute, Vector3d.Dot(VSL.HorizontalSpeed.normalized, vdir), bearing_threshold);//debug
}
var cur_vel = (float)Vector3d.Dot(dvel, vdir);
if (!vel_is_set)
{
//don't slow down on intermediate waypoints too much
var min_dist = C.OnPathMinDistance * VSL.Geometry.R;
if (!CFG.Target.Land && CFG.Nav[Navigation.FollowPath] &&
CFG.Path.Count > 1 && hdistance < min_dist)
{
WayPoint next_wp = null;
if (CFG.Path.Peek() == CFG.Target)
{
using (var iwp = CFG.Path.GetEnumerator())
{
if (iwp.MoveNext() &&
iwp.MoveNext())
{
next_wp = iwp.Current;
}
}
}
else
{
next_wp = CFG.Path.Peek();
}
if (next_wp != null)
{
next_wp.Update(VSL);
var next_dist = Vector3.ProjectOnPlane(next_wp.GetTransform().position - CFG.Target.GetTransform().position, VSL.Physics.Up);
var angle2next = Utils.Angle2(vdir, next_dist);
var minD = Utils.ClampL(min_dist * (1 - angle2next / 180 / VSL.Torque.MaxPitchRoll.AA_rad * C.PitchRollAAf), CFG.Target.AbsRadius);
if (minD > hdistance)
{
hdistance = minD;
}
}
else
{
hdistance = min_dist;
}
}
else if (CFG.Nav.Not(Navigation.FollowTarget))
{
hdistance = Utils.ClampL(hdistance - end_distance + VSL.Geometry.D, 0);
}
//tune maximum speed and PID
if (CFG.MaxNavSpeed < 10)
{
CFG.MaxNavSpeed = 10;
}
DistancePID.Min = HorizontalSpeedControl.C.TranslationMinDeltaV + 0.1f;
DistancePID.Max = CFG.MaxNavSpeed;
if (CFG.Nav[Navigation.FollowTarget])
{
DistancePID.P = C.DistancePID.P / 2;
DistancePID.D = DistancePID.P / 2;
}
else
{
DistancePID.P = C.DistancePID.P;
DistancePID.D = C.DistancePID.D;
}
if (cur_vel > 0)
{
var mg2 = VSL.Physics.mg * VSL.Physics.mg;
var brake_thrust = Mathf.Min(VSL.Physics.mg, VSL.Engines.MaxThrustM / 2 * VSL.OnPlanetParams.TWRf);
var max_thrust = Mathf.Min(Mathf.Sqrt(brake_thrust * brake_thrust + mg2), VSL.Engines.MaxThrustM * 0.99f);
var horizontal_thrust = VSL.Engines.TranslationThrustLimits.Project(VSL.LocalDir(vdir)).magnitude;
if (horizontal_thrust > brake_thrust)
{
brake_thrust = horizontal_thrust;
}
else
{
horizontal_thrust = -1;
}
if (brake_thrust > 0)
{
var brake_accel = brake_thrust / VSL.Physics.M;
var prep_time = 0f;
if (horizontal_thrust < 0)
{
var brake_angle = Utils.Angle2(VSL.Engines.CurrentDefThrustDir, vdir) - 45;
if (brake_angle > 0)
{
//count rotation of the vessel to braking position
var axis = Vector3.Cross(VSL.Engines.CurrentDefThrustDir, vdir);
if (VSL.Torque.Slow)
{
prep_time = VSL.Torque.NoEngines.RotationTime3Phase(brake_angle, axis, C.RotationAccelPhase);
//also count time needed for the engines to get to full thrust
prep_time += Utils.LerpTime(VSL.Engines.Thrust.magnitude, VSL.Engines.MaxThrustM, max_thrust, VSL.Engines.AccelerationSpeed);
}
else
{
prep_time = VSL.Torque.MaxCurrent.RotationTime2Phase(brake_angle, axis, VSL.OnPlanetParams.GeeVSF);
}
}
}
var prep_dist = cur_vel * prep_time + CFG.Target.AbsRadius;
var eta = hdistance / cur_vel;
max_speed.TauUp = C.MaxSpeedFilterUp / eta / brake_accel;
max_speed.TauDown = eta * brake_accel / C.MaxSpeedFilterDown;
max_speed.Update(prep_dist < hdistance?
(1 + Mathf.Sqrt(1 + 2 / brake_accel * (hdistance - prep_dist))) * brake_accel :
2 * brake_accel);
CorrectionPID.Min = -VSL.HorizontalSpeed.Absolute;
if (max_speed < cur_vel)
{
CorrectionPID.Update(max_speed - cur_vel);
}
else
{
CorrectionPID.IntegralError *= (1 - TimeWarp.fixedDeltaTime * C.CorrectionEasingRate);
CorrectionPID.Update(0);
}
HSC.AddRawCorrection(CorrectionPID.Action * VSL.HorizontalSpeed.Vector.normalized);
}
if (max_speed < CFG.MaxNavSpeed)
{
DistancePID.Max = Mathf.Max(DistancePID.Min, max_speed);
}
}
//take into account vertical distance and obstacle
var rel_ahead = VSL.Altitude.Ahead - VSL.Altitude.Absolute;
// Log("vdist {}, rel.ahead {}, vF {}, aF {}", vdistance, rel_ahead,
// Utils.ClampL(1 - Mathf.Atan(vdistance/hdistance)/Utils.HalfPI, 0),
// Utils.ClampL(1 - rel_ahead/RAD.DistanceAhead, 0));//debug
vdistance = Mathf.Max(vdistance, rel_ahead);
if (vdistance > 0)
{
hdistance *= Utils.ClampL(1 - Mathf.Atan(vdistance / hdistance) / (float)Utils.HalfPI, 0);
}
if (RAD != null && rel_ahead > 0 && RAD.DistanceAhead > 0)
{
hdistance *= Utils.ClampL(1 - rel_ahead / RAD.DistanceAhead, 0);
}
//update the needed velocity
DistancePID.Update(hdistance);
var nV = vdir * DistancePID.Action;
//correcto for Follow Target program
if (CFG.Nav[Navigation.FollowTarget] && Vector3d.Dot(tvel, vdir) > 0)
{
nV += tvel;
}
VSL.HorizontalSpeed.SetNeeded(nV);
}
//correct for lateral movement
var latV = -Vector3d.Exclude(vdir, VSL.HorizontalSpeed.Vector);
var latF = (float)Math.Min((latV.magnitude / Math.Max(VSL.HorizontalSpeed.Absolute, 0.1)), 1);
LateralPID.P = C.LateralPID.P * latF;
LateralPID.I = Math.Min(C.LateralPID.I, latF);
LateralPID.D = C.LateralPID.D * latF;
LateralPID.Update(latV);
HSC.AddWeightedCorrection(LateralPID.Action);
// Log("\ndir v {}\nlat v {}\nact v {}\nlatPID {}",
// VSL.HorizontalSpeed.NeededVector, latV,
// LateralPID.Action, LateralPID);//debug
}
public SurfaceNode(WayPoint wp, CelestialBody body)
{
position = wp.GetTransform().position;
up = body.GetSurfaceNVector(wp.Pos.Lat, wp.Pos.Lon);
}
public void WaypointOverlay()
{
if (PN == null || TCA == null || !TCA.Available || !GUIWindowBase.HUD_enabled)
{
return;
}
if (SelectingTarget)
{
var coords = MapView.MapIsEnabled?
Coordinates.GetAtPointer(vessel.mainBody) :
Coordinates.GetAtPointerInFlight();
if (coords != null)
{
var t = new WayPoint(coords);
Markers.DrawCBMarker(vessel.mainBody, coords, new Color(1.0f, 0.56f, 0.0f), WayPointMarker);
DrawLabelAtPointer(coords.FullDescription(vessel), t.DistanceTo(vessel));
if (!clicked)
{
if (Input.GetMouseButtonDown(0))
{
clicked = true;
}
else if (Input.GetMouseButtonDown(1))
{
clicked_time = DateTime.Now; clicked = true;
}
}
else
{
if (Input.GetMouseButtonUp(0))
{
t.Update(VSL);
t.Movable = true;
if (select_single)
{
t.Name = "Target";
SelectingTarget = false;
select_single = false;
VSL.SetTarget(null, t);
if (!was_in_map_view)
{
MapView.ExitMapView();
}
}
else
{
t.Name = "Waypoint " + (CFG.Path.Count + 1);
AddTargetDamper.Run(() => CFG.Path.Enqueue(t));
}
CFG.ShowPath = true;
clicked = false;
}
if (Input.GetMouseButtonUp(1))
{
SelectingTarget &= (DateTime.Now - clicked_time).TotalSeconds >= GLB.ClickDuration;
clicked = false;
}
}
}
}
bool current_target_drawn = false;
var camera = Markers.CurrentCamera;
if (CFG.ShowPath)
{
var i = 0;
var num = (float)(CFG.Path.Count - 1);
WayPoint wp0 = null;
var total_dist = 0f;
var dist2cameraF = Mathf.Pow(Mathf.Max((camera.transform.position -
VSL.vessel.transform.position).magnitude, 1),
GLB.CameraFadeinPower);
foreach (var wp in CFG.Path)
{
current_target_drawn |= wp.Equals(CFG.Target);
wp.UpdateCoordinates(vessel.mainBody);
var dist = -1f;
if (wp0 != null && wp != selected_waypoint)
{
total_dist += (float)wp.DistanceTo(wp0, VSL.Body) / dist2cameraF;
dist = total_dist;
}
var r = Markers.DefaultIconSize;
var c = marker_color(i, num, dist);
if (wp0 == null)
{
DrawPath(vessel, wp, c);
}
else
{
DrawPath(vessel.mainBody, wp0, wp, c);
}
if (wp == edited_waypoint)
{
c = edited_color; r = Markers.DefaultIconSize * 2;
}
else if (wp.Land)
{
r = Markers.DefaultIconSize * 2;
}
if (DrawWayPoint(wp, c, size:r))
{
DrawLabelAtPointer(wp.FullInfo(vessel), wp.DistanceTo(vessel));
select_waypoint(wp);
}
else if (wp == selected_waypoint)
{
DrawLabelAtPointer(wp.FullInfo(vessel), wp.DistanceTo(vessel));
}
wp0 = wp;
i++;
}
}
//current target and anchor
if (CFG.Anchor != null)
{
DrawWayPoint(CFG.Anchor, Color.cyan, "Anchor");
current_target_drawn |= CFG.Anchor.Equals(CFG.Target);
}
if (CFG.Target && !current_target_drawn &&
(!CFG.Target.IsVessel || CFG.Target.GetVessel().LandedOrSplashed))
{
DrawWayPoint(CFG.Target, Color.magenta, "Target");
}
//custom markers
VSL.Info.CustomMarkersWP.ForEach(m => DrawWayPoint(m, Color.red, m.Name));
VSL.Info.CustomMarkersVec.ForEach(m => Markers.DrawWorldMarker(m, Color.red, "Custom WayPoint", WayPointMarker));
//modify the selected waypoint
if (!SelectingTarget && selected_waypoint != null)
{
if (changing_altitude)
{
var dist2camera = (selected_waypoint.GetTransform().position - camera.transform.position).magnitude;
var dy = (Input.mousePosition.y - last_mouse_y) / Screen.height *
dist2camera * Math.Tan(camera.fieldOfView * Mathf.Deg2Rad) * 2;
last_mouse_y = Input.mousePosition.y;
selected_waypoint.Pos.Alt += dy;
selected_waypoint.Update(VSL);
}
else
{
var coords = MapView.MapIsEnabled?
Coordinates.GetAtPointer(vessel.mainBody) :
Coordinates.GetAtPointerInFlight();
if (coords != null)
{
selected_waypoint.Pos = coords;
selected_waypoint.Update(VSL);
}
}
if (Input.GetMouseButtonUp(0) ||
(changing_altitude && Input.GetMouseButtonUp(2)))
{
if (changing_altitude)
{
selected_waypoint.Pos.Alt = Math.Max(selected_waypoint.Pos.Alt,
selected_waypoint.Pos.SurfaceAlt(vessel.mainBody));
}
selected_waypoint = null;
orig_coordinates = null;
changing_altitude = false;
}
else if (Input.GetMouseButtonDown(1))
{
selected_waypoint.Pos = orig_coordinates;
selected_waypoint.Update(VSL);
selected_waypoint = null;
orig_coordinates = null;
changing_altitude = false;
}
}
#if DEBUG
// VSL.Engines.All.ForEach(e => e.engine.thrustTransforms.ForEach(t => DrawWorldMarker(t.position, Color.red, e.name)));
// DrawWorldMarker(VSL.vessel.transform.position, Color.yellow, "Vessel");
// DrawWorldMarker(VSL.Physics.wCoM, Color.green, "CoM");
#endif
}
protected override void Update()
{
if (!IsActive || CFG.Target == null || CFG.Nav.Paused)
{
return;
}
CFG.Target.Update(VSL);
//update things that are needed to fly in formation
if (CFG.Nav[Navigation.FollowTarget])
{
update_formation_info();
}
else
{
tVSL = null; tPN = null;
}
//calculate direct distance
var vdir = Vector3.ProjectOnPlane(CFG.Target.GetTransform().position + formation_offset - VSL.Physics.wCoM, VSL.Physics.Up);
var distance = Utils.ClampL(vdir.magnitude - VSL.Geometry.R, 0);
//update destination
if (tPN != null && !tPN.VSL.Info.Destination.IsZero())
{
VSL.Info.Destination = tPN.VSL.Info.Destination;
}
else
{
VSL.Info.Destination = vdir;
}
//handle flying in formation
var tvel = Vector3.zero;
var vel_is_set = false;
var end_distance = CFG.Target.AbsRadius;
if (CFG.Target.Land)
{
end_distance /= 4;
}
var dvel = VSL.HorizontalSpeed.Vector;
if (tVSL != null && tVSL.loaded)
{
if (formation_offset.IsZero())
{
end_distance *= all_followers.Count / 2f;
}
tvel = Vector3d.Exclude(VSL.Physics.Up, tVSL.srf_velocity);
dvel -= tvel;
var tvel_m = tvel.magnitude;
var dir2vel_cos = Vector3.Dot(vdir.normalized, tvel.normalized);
var lat_dir = Vector3.ProjectOnPlane(vdir - VSL.HorizontalSpeed.Vector * PN.LookAheadTime, tvel);
var lat_dist = lat_dir.magnitude;
FormationBreakTimer.RunIf(() => keep_formation = false,
tvel_m < PN.FormationSpeedCutoff);
Maneuvering = CanManeuver && lat_dist > CFG.Target.AbsRadius && distance < CFG.Target.AbsRadius * 3;
if (keep_formation && tvel_m > 0 &&
(!CanManeuver ||
dir2vel_cos <= PN.BearingCutoffCos ||
lat_dist < CFG.Target.AbsRadius * 3))
{
if (CanManeuver)
{
HSC.AddWeightedCorrection(lat_dir.normalized * Utils.ClampH(lat_dist / CFG.Target.AbsRadius, 1) *
tvel_m * PN.FormationFactor * (Maneuvering? 1 : 0.5f));
}
distance = Utils.ClampL(Mathf.Abs(dir2vel_cos) * distance - VSL.Geometry.R, 0);
if (dir2vel_cos < 0)
{
if (distance < CFG.Target.AbsRadius)
{
HSC.AddRawCorrection(tvel * Utils.Clamp(-distance / CFG.Target.AbsRadius * PN.FormationFactor, -PN.FormationFactor, 0));
}
else if (Vector3.Dot(vdir, dvel) < 0 &&
(dvel.magnitude > PN.FollowerMaxAwaySpeed ||
distance > CFG.Target.AbsRadius * 5))
{
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(vdir);
return;
}
else
{
HSC.AddRawCorrection(tvel * (PN.FormationFactor - 1));
}
distance = 0;
}
vdir = tvel;
}
}
//if the distance is greater that the threshold (in radians), use Great Circle navigation
if (distance / VSL.Body.Radius > PN.DirectNavThreshold)
{
var next = CFG.Target.PointFrom(VSL.vessel, 0.1);
distance = (float)CFG.Target.DistanceTo(VSL.vessel);
vdir = Vector3.ProjectOnPlane(VSL.Body.GetWorldSurfacePosition(next.Lat, next.Lon, VSL.vessel.altitude)
- VSL.vessel.transform.position, VSL.Physics.Up);
tvel = Vector3.zero;
}
else if (!VSL.IsActiveVessel && distance > GLB.UnpackDistance)
{
VSL.SetUnpackDistance(distance * 1.2f);
}
vdir.Normalize();
//check if we have arrived to the target and stayed long enough
if (distance < end_distance)
{
var prev_needed_speed = VSL.HorizontalSpeed.NeededVector.magnitude;
if (prev_needed_speed < 1 && !CFG.HF[HFlight.Move])
{
CFG.HF.OnIfNot(HFlight.Move);
}
else if (prev_needed_speed > 10 && !CFG.HF[HFlight.NoseOnCourse])
{
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
}
if (CFG.Nav[Navigation.FollowTarget])
{
if (tvel.sqrMagnitude > 1)
{
//set needed velocity and starboard to match that of the target
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(tvel);
HSC.AddRawCorrection((tvel - VSL.HorizontalSpeed.Vector) * 0.9f);
}
else
{
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
}
vel_is_set = true;
}
else if (CFG.Nav[Navigation.FollowPath] && CFG.Waypoints.Count > 0)
{
if (CFG.Waypoints.Peek() == CFG.Target)
{
if (CFG.Waypoints.Count > 1)
{
CFG.Waypoints.Dequeue();
if (on_arrival())
{
return;
}
start_to(CFG.Waypoints.Peek());
return;
}
else if (ArrivedTimer.TimePassed)
{
CFG.Waypoints.Clear();
if (on_arrival())
{
return;
}
finish();
return;
}
}
else
{
if (on_arrival())
{
return;
}
start_to(CFG.Waypoints.Peek());
return;
}
}
else if (ArrivedTimer.TimePassed)
{
if (on_arrival())
{
return;
}
finish(); return;
}
}
else
{
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
ArrivedTimer.Reset();
}
//if we need to make a sharp turn, stop and turn, then go on
if (Vector3.Dot(vdir, VSL.OnPlanetParams.Fwd) < PN.BearingCutoffCos &&
Vector3d.Dot(VSL.HorizontalSpeed.normalized, vdir) < PN.BearingCutoffCos)
{
VSL.HorizontalSpeed.SetNeeded(vdir);
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
Maneuvering = false;
vel_is_set = true;
}
var cur_vel = (float)Vector3d.Dot(dvel, vdir);
if (!vel_is_set)
{
//don't slow down on intermediate waypoints too much
var min_dist = PN.OnPathMinDistance * VSL.Geometry.R;
if (!CFG.Target.Land && CFG.Nav[Navigation.FollowPath] &&
CFG.Waypoints.Count > 1 && distance < min_dist)
{
WayPoint next_wp = null;
if (CFG.Waypoints.Peek() == CFG.Target)
{
var iwp = CFG.Waypoints.GetEnumerator();
try
{
iwp.MoveNext(); iwp.MoveNext();
next_wp = iwp.Current;
}
catch {}
}
else
{
next_wp = CFG.Waypoints.Peek();
}
if (next_wp != null)
{
next_wp.Update(VSL);
var next_dist = Vector3.ProjectOnPlane(next_wp.GetTransform().position - CFG.Target.GetTransform().position, VSL.Physics.Up);
var angle2next = Vector3.Angle(vdir, next_dist);
var minD = Utils.ClampL(min_dist * (1 - angle2next / 180 / VSL.Torque.MaxPitchRoll.AA_rad * PN.PitchRollAAf), CFG.Target.AbsRadius);
if (minD > distance)
{
distance = minD;
}
}
else
{
distance = min_dist;
}
}
else if (CFG.Nav.Not(Navigation.FollowTarget))
{
distance = Utils.ClampL(distance - end_distance + VSL.Geometry.D, 0);
}
//tune maximum speed and PID
if (CFG.MaxNavSpeed < 10)
{
CFG.MaxNavSpeed = 10;
}
DistancePID.Min = 0;
DistancePID.Max = CFG.MaxNavSpeed;
if (cur_vel > 0)
{
var brake_thrust = Mathf.Max(VSL.Engines.ManualThrustLimits.Project(VSL.LocalDir(vdir)).magnitude,
VSL.Physics.mg * VSL.OnPlanetParams.TWRf);
var eta = distance / cur_vel;
var max_speed = 0f;
if (brake_thrust > 0)
{
var brake_accel = brake_thrust / VSL.Physics.M;
var brake_time = cur_vel / brake_accel;
max_speed = brake_accel * eta;
if (eta <= brake_time * PN.BrakeOffset)
{
HSC.AddRawCorrection((eta / brake_time / PN.BrakeOffset - 1) * VSL.HorizontalSpeed.Vector);
}
}
if (max_speed < CFG.MaxNavSpeed)
{
DistancePID.Max = max_speed;
}
}
//update the needed velocity
DistancePID.Update(distance * PN.DistanceFactor);
var nV = vdir * DistancePID.Action;
//correcto for Follow Target program
if (CFG.Nav[Navigation.FollowTarget] && Vector3d.Dot(tvel, vdir) > 0)
{
nV += tvel;
}
VSL.HorizontalSpeed.SetNeeded(nV);
}
//correct for lateral movement
var latV = -Vector3d.Exclude(vdir, VSL.HorizontalSpeed.Vector);
var latF = (float)Math.Min((latV.magnitude / Math.Max(VSL.HorizontalSpeed.Absolute, 0.1)), 1);
LateralPID.P = PN.LateralPID.P * latF;
LateralPID.I = Mathf.Min(PN.LateralPID.I, latF);
LateralPID.D = PN.LateralPID.D * latF;
LateralPID.Update(latV);
HSC.AddWeightedCorrection(LateralPID.Action);
// LogF("\ndir v {}\nlat v {}\nact v {}\nlatPID {}",
// VSL.HorizontalSpeed.NeededVector, latV,
// LateralPID.Action, LateralPID);//debug
}
