Beispiel #1
0
 void reset_formation()
 {
     Maneuvering = false;
     Formation   = null;
     fnode       = null;
     tVSL        = null;
     tPN         = null;
 }
Beispiel #2
0
        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
        }
Beispiel #3
0
        void update_formation_info()
        {
            tVSL = CFG.Target.GetVessel();
            if (tVSL == null)
            {
                reset_formation();
                CanManeuver = false;
                return;
            }
            if (tPN == null || !tPN.Valid)
            {
                tTCA = ModuleTCA.EnabledTCA(tVSL);
                tPN  = tTCA != null?tTCA.GetModule <PointNavigator>() : null;
            }
            var only_count = false;

            if (tVSL.srf_velocity.sqrMagnitude < C.FormationSpeedSqr)
            {
                reset_formation(); CanManeuver = false; only_count = true;
            }
            //update followers
            var offset       = 0f;
            var can_maneuver = true;

            all_followers.Clear();
            for (int i = 0, num_vessels = FlightGlobals.Vessels.Count; i < num_vessels; i++)
            {
                var v = FlightGlobals.Vessels[i];
                if (v == null || v.packed || !v.loaded)
                {
                    continue;
                }
                var tca = ModuleTCA.EnabledTCA(v);
                if (tca != null &&
                    (tca.vessel == VSL.vessel ||
                     tca.CFG.Nav[Navigation.FollowTarget] &&
                     tca.CFG.Target.GetTarget() != null &&
                     tca.CFG.Target.GetTarget() == CFG.Target.GetTarget()))
                {
                    var vPN = tca.GetModule <PointNavigator>();
                    if (vPN == null)
                    {
                        continue;
                    }
                    all_followers.Add(v.id, vPN);
                    if (offset < vPN.VSL.Geometry.R)
                    {
                        offset = vPN.VSL.Geometry.R;
                    }
                    if (v.id != VSL.vessel.id)
                    {
                        can_maneuver &= !vPN.Maneuvering ||
                                        (Maneuvering && VSL.vessel.id.CompareTo(v.id) > 0);
                    }
                }
            }
            if (only_count)
            {
                return;
            }
            CanManeuver = can_maneuver;
            var follower_index = all_followers.IndexOfKey(VSL.vessel.id);

            if (follower_index == 0)
            {
                var forward     = tVSL == null? Vector3d.zero : -tVSL.srf_velocity.normalized;
                var side        = Vector3d.Cross(VSL.Physics.Up, forward).normalized;
                var num_offsets = all_followers.Count + (all_followers.Count % 2);
                offset *= 2;
                var target_size = tTCA != null? tTCA.VSL.Geometry.D : Utils.ClampL(Math.Pow(tVSL.totalMass, 1 / 3f), 1);
                if (offset < target_size)
                {
                    offset = (float)target_size;
                }
                offset *= C.MinDistance;
                if (Formation == null || Formation.Count != num_offsets || FormationUpdateTimer.TimePassed)
                {
                    FormationUpdateTimer.Reset();
                    Formation = new List <FormationNode>(num_offsets);
                    for (int i = 0; i < num_offsets; i++)
                    {
                        Formation.Add(new FormationNode(tVSL, i, forward, side, offset));
                    }
                    all_followers.ForEach(p => p.Value.UpdateFormation(Formation));
                }
                else
                {
                    for (int i = 0; i < num_offsets; i++)
                    {
                        Formation[i].Update(forward, side, offset);
                    }
                }
            }
            keep_formation = Formation != null;
            if (Formation == null || fnode != null)
            {
                return;
            }
            //compute follower offset
            var min_d   = -1f;
            var min_off = 0;

            for (int i = 0; i < Formation.Count; i++)
            {
                var node = Formation[i];
                if (node.Follower != null)
                {
                    continue;
                }
                var d = node.Distance(VSL.vessel);
                if (min_d < 0 || min_d > d)
                {
                    min_d   = d;
                    min_off = i;
                }
            }
            Formation[min_off].Follower = VSL.vessel;
            fnode = Formation[min_off];
        }
Beispiel #4
0
        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
        }
 void update_formation_info()
 {
     tVSL = CFG.Target.GetVessel();
     if(tVSL == null) { reset_formation(); CanManeuver = false; return; }
     if(tPN == null || !tPN.TCA.Valid || tPN.VSL.vessel != tVSL)
     {
         tTCA = ModuleTCA.EnabledTCA(tVSL);
         if(tTCA != null) tPN = tTCA.GetModule<PointNavigator>();
     }
     var only_count = false;
     if(tVSL.srf_velocity.sqrMagnitude < PN.FormationSpeedSqr)
     { reset_formation(); CanManeuver = false; only_count = true; }
     //update followers
     var offset = 0f;
     var can_maneuver = true;
     all_followers.Clear();
     for(int i = 0, num_vessels = FlightGlobals.Vessels.Count; i < num_vessels; i++)
     {
         var v = FlightGlobals.Vessels[i];
         if(v == null || v.packed || !v.loaded) continue;
         var tca = ModuleTCA.EnabledTCA(v);
         if(tca != null &&
            (tca.vessel == VSL.vessel ||
             tca.CFG.Nav[Navigation.FollowTarget] &&
             tca.CFG.Target.GetTarget() != null &&
             tca.CFG.Target.GetTarget() == CFG.Target.GetTarget()))
         {
             var vPN = tca.GetModule<PointNavigator>();
             if(vPN == null) continue;
             all_followers.Add(v.id, vPN);
             if(offset < vPN.VSL.Geometry.R)
                 offset = vPN.VSL.Geometry.R;
             if(v.id != VSL.vessel.id)
                 can_maneuver &= !vPN.Maneuvering ||
                     (Maneuvering && VSL.vessel.id.CompareTo(v.id) > 0);
         }
     }
     if(only_count) return;
     CanManeuver = can_maneuver;
     var follower_index = all_followers.IndexOfKey(VSL.vessel.id);
     if(follower_index == 0)
     {
         var forward = tVSL == null? Vector3d.zero : -tVSL.srf_velocity.normalized;
         var side = Vector3d.Cross(VSL.Physics.Up, forward).normalized;
         var num_offsets = all_followers.Count+(all_followers.Count%2);
         offset *= 2;
         var target_size = tTCA != null? tTCA.VSL.Geometry.D : Utils.ClampL(Math.Pow(tVSL.totalMass, 1/3f), 1);
         if(offset < target_size) offset = (float)target_size;
         offset *= PN.MinDistance;
         if(Formation == null || Formation.Count != num_offsets || FormationUpdateTimer.TimePassed)
         {
             FormationUpdateTimer.Reset();
             Formation = new List<FormationNode>(num_offsets);
             for(int i = 0; i < num_offsets; i++)
                 Formation.Add(new FormationNode(tVSL, i, forward, side, offset));
             all_followers.ForEach(p => p.Value.UpdateFormation(Formation));
         }
         else for(int i = 0; i < num_offsets; i++)
             Formation[i].Update(forward, side, offset);
     }
     keep_formation = Formation != null;
     if(Formation == null || fnode != null) return;
     //compute follower offset
     var min_d   = -1f;
     var min_off = 0;
     for(int i = 0; i < Formation.Count; i++)
     {
         var node = Formation[i];
         if(node.Follower != null) continue;
         var d = node.Distance(VSL.vessel);
         if(min_d < 0 || min_d > d)
         {
             min_d = d;
             min_off = i;
         }
     }
     Formation[min_off].Follower = VSL.vessel;
     fnode = Formation[min_off];
 }
 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
 }