Exemplo n.º 1
0
 void start_to(WayPoint wp)
 {
     wp.Update(VSL);
     if (CFG.Nav[Navigation.GoToTarget] && wp.CloseEnough(VSL))
     {
         CFG.Nav.Off();
         return;
     }
     if (VSL.LandedOrSplashed)
     {
         CFG.AltitudeAboveTerrain = true;
         CFG.VF.OnIfNot(VFlight.AltitudeControl);
         CFG.DesiredAltitude = C.TakeoffAltitude + VSL.Geometry.H;
     }
     else if (CFG.VTOLAssistON)
     {
         VSL.GearOn(false);
     }
     max_speed.Set(CFG.MaxNavSpeed);
     reset_formation();
     SetTarget(wp);
     DistancePID.Reset();
     LateralPID.Reset();
     CorrectionPID.Reset();
     CFG.HF.OnIfNot(HFlight.NoseOnCourse);
     NeedCPS();
 }
Exemplo n.º 2
0
 void start_to(WayPoint wp)
 {
     VSL.UpdateOnPlanetStats();
     wp.Update(VSL);
     if (CFG.Nav[Navigation.GoToTarget] && wp.CloseEnough(VSL))
     {
         CFG.Nav.Off();
         return;
     }
     if (VSL.LandedOrSplashed)
     {
         CFG.AltitudeAboveTerrain = true;
         CFG.VF.OnIfNot(VFlight.AltitudeControl);
         CFG.DesiredAltitude = PN.TakeoffAltitude + VSL.Geometry.H;
     }
     else if (CFG.VTOLAssistON)
     {
         VSL.GearOn(false);
     }
     reset_formation();
     SetTarget(wp);
     DistancePID.Reset();
     LateralPID.Reset();
     CFG.HF.OnIfNot(HFlight.NoseOnCourse);
     RegisterTo <Radar>();
 }
 public void Resume(ModuleTCA TCA)
 {
     if (Target != null)
     {
         Target.Update(TCA.VSL);
         TCA.VSL.SetTarget(null, Target);
     }
     if (Anchor != null)
     {
         Anchor.Update(TCA.VSL);
     }
     Path.ForEach(wp => wp.Update(TCA.VSL));
     multiplexers.ForEach(m => m.Resume());
 }
Exemplo n.º 4
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
        }
Exemplo n.º 5
0
        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
        }
Exemplo n.º 6
0
        void waypoint_editor(int windowID)
        {
            var close = false;

            GUILayout.BeginVertical();
            GUILayout.BeginHorizontal();
            GUILayout.Label("Name:", GUILayout.Width(70));
            edited_waypoint_name = GUILayout.TextField(edited_waypoint_name, GUILayout.ExpandWidth(true));
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            GUILayout.Label("Latitude:", GUILayout.Width(70));
            LatField.Draw("°", 1, "F1");
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            GUILayout.Label("Longitude:", GUILayout.Width(70));
            LonField.Draw("°", 1, "F1");
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            GUILayout.Label("Altitude:", GUILayout.Width(70));
            AltField.Draw("°", 100, "F0");
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            if (GUILayout.Button("Delete", Styles.danger_button))
            {
                CFG.Path.Remove(edited_waypoint);
                close = true;
            }
            GUILayout.FlexibleSpace();
            if (GUILayout.Button("Cancel", Styles.active_button))
            {
                close = true;
            }
            GUILayout.FlexibleSpace();
            if (GUILayout.Button(new GUIContent("◉", "Target this waypoint"), Styles.enabled_button))
            {
                VSL.SetTarget(null, edited_waypoint);
            }
            if (GUILayout.Button(new GUIContent("⊥", "Set altitude to ground level"), Styles.active_button))
            {
                edited_waypoint.Pos.SetAlt2Surface(VSL.Body);
                AltField.Value = (float)edited_waypoint.Pos.Alt;
            }
            if (Utils.ButtonSwitch("||", edited_waypoint.Pause, "Pause on arrival", GUILayout.Width(25)))
            {
                edited_waypoint.Pause = !edited_waypoint.Pause;
            }
            if (LND != null &&
                Utils.ButtonSwitch("Land", edited_waypoint.Land, "Land on arrival"))
            {
                edited_waypoint.Land = !edited_waypoint.Land;
            }
            if (GUILayout.Button("Apply", Styles.confirm_button))
            {
                LatField.UpdateValue(); LonField.UpdateValue(); AltField.UpdateValue();
                edited_waypoint.Name    = edited_waypoint_name;
                edited_waypoint.Pos.Lat = Utils.ClampAngle(LatField.Value);
                edited_waypoint.Pos.Lon = Utils.ClampAngle(LonField.Value);
                edited_waypoint.Pos.Alt = Math.Max(AltField.Value,
                                                   edited_waypoint.Pos.SurfaceAlt(vessel.mainBody));
                edited_waypoint.Update(VSL);
                close = true;
            }
            GUILayout.EndHorizontal();
            GUILayout.EndVertical();
            GUIWindowBase.TooltipsAndDragWindow();
            if (close)
            {
                edited_waypoint = null;
            }
        }
Exemplo n.º 7
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
        }