void reset_formation() { Maneuvering = false; Formation = null; fnode = null; tVSL = null; tPN = null; }
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 }
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]; }
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 }