Wheelsteering fixes and improvements (#3131)

* Fix `IFlightControlParameter.GetValue` returning vessel throttle for wheelsteering and wheelthrottle managers when they are not active

* Make wheelsteering getter return target heading instead of bearing

* Fix #2106. Add `wheelsteeringpid` bound variable

* Change default proportional term coefficient on wheelsteeringpid from 0.1 to 0.03

This is a breaking change. This doesn't reduce turning speed nearly as much as it reduces oscillations for both rovers and planes. Mostly because of the steering smoothing applied by ksp.

doc/source/commands/flight/cooked.rst

@@ -203,6 +203,13 @@ Like all ``LOCK`` expressions, the steering and throttle continually update on t
     It's a very bad idea to ``WAIT`` during the execution of the expression in a
     LOCK WHEELSTEERING.  See the note in the next section below.
 
+.. global:: WHEELSTEERINGPID
+
+   :access: Get/Set
+   :type: :struct:`PIDLoop`
+
+   The PIDLoop used to control wheelsteering. Can be used to optimize
+   steering performance and eliminate steering oscillations on some vessels.
 
 Don't 'WAIT' or run slow script code during cooked control calculation
 ----------------------------------------------------------------------

src/kOS/Binding/FlightControlManager.cs

@@ -12,6 +12,7 @@
 using kOS.Control;
 using kOS.Module;
 using kOS.Communication;
+using kOS.Safe.Encapsulation;
 
 namespace kOS.Binding
 {
@@ -49,6 +50,18 @@ public override void AddTo(SharedObjects shared)
 
             shared.BindingMgr.AddBoundVariable("SASMODE", GetAutopilotModeName, SelectAutopilotMode);
             shared.BindingMgr.AddBoundVariable("NAVMODE", GetNavModeName, SetNavMode);
+
+            shared.BindingMgr.AddBoundVariable("WHEELSTEERINGPID", () =>
+            {
+                return ((WheelSteeringManager)kOSVesselModule.GetInstance(shared.Vessel).GetFlightControlParameter("wheelsteering")).SteeringPID;
+            }, value =>
+            {
+                if (value is PIDLoop pidLoop)
+                {
+                    ((WheelSteeringManager)kOSVesselModule.GetInstance(shared.Vessel).GetFlightControlParameter("wheelsteering")).SteeringPID = pidLoop;
+                }
+                else throw new KOSCastException(value.GetType(), typeof(PIDLoop));
+            });
         }
 
         private object GetThrottleValue()

src/kOS/Control/WheelSteeringManager.cs

@@ -16,6 +16,7 @@ internal class WheelSteeringManager : IFlightControlParameter
 
         public bool Enabled { get; private set; }
         public float Value { get; set; }
+        public PIDLoop SteeringPID = new PIDLoop(0.03, 0, 0);
 
         public bool FightsWithSas { get { return false; } }
 
@@ -90,26 +91,26 @@ Vessel IFlightControlParameter.GetResponsibleVessel()
 
         object IFlightControlParameter.GetValue()
         {
-            if (Enabled)
-            {
-                return Value;
-            }
-            return internalVessel.ctrlState.mainThrottle;
+            return Value;
         }
 
         void IFlightControlParameter.UpdateAutopilot(FlightCtrlState c, ControlTypes ctrlLock)
         {
             if (!Enabled) return;
 
             if (!(controlShared.Vessel.horizontalSrfSpeed > 0.1f)) return;
+
+            float vesselHeading = VesselUtils.GetHeading(controlShared.Vessel);
+            float bearing = VesselUtils.AngleDelta(vesselHeading, Value);
+            float PIDOutput = SteeringPID.Update(controlShared.UpdateHandler.CurrentFixedTime, bearing);
 
-            if (Mathf.Abs(VesselUtils.AngleDelta(VesselUtils.GetHeading(controlShared.Vessel), VesselUtils.GetVelocityHeading(controlShared.Vessel))) <= 90)
+            if (Mathf.Abs(VesselUtils.AngleDelta(vesselHeading, VesselUtils.GetVelocityHeading(controlShared.Vessel))) <= 90)
             {
-                c.wheelSteer = Mathf.Clamp(Value / -10, -1, 1);
+                c.wheelSteer = Mathf.Clamp(PIDOutput, -1, 1);
             }
             else
             {
-                c.wheelSteer = -Mathf.Clamp(Value / -10, -1, 1);
+                c.wheelSteer = -Mathf.Clamp(PIDOutput, -1, 1);
             }
         }
 
@@ -123,29 +124,21 @@ void IFlightControlParameter.UpdateValue(object value, SharedObjects shared)
             if (!Enabled)
                 ((IFlightControlParameter)this).EnableControl(shared);
 
-            float bearing = 0;
-
-            if (value is VesselTarget)
+            if (value is VesselTarget vessel)
             {
-                bearing = VesselUtils.GetTargetBearing(controlShared.Vessel, ((VesselTarget)value).Vessel);
+                Value = VesselUtils.GetTargetHeading(controlShared.Vessel, vessel.Vessel);
             }
-            else if (value is GeoCoordinates)
+            else if (value is GeoCoordinates gc)
             {
-                bearing = ((GeoCoordinates)value).GetBearing();
+                Value = gc.GetHeading();
             }
             else
             {
                 try
                 {
-                    double doubleValue = Convert.ToDouble(value);
-                    if (Utils.IsValidNumber(doubleValue))
-                    {
-                        bearing = (float)(Math.Round(doubleValue) - Mathf.Round(FlightGlobals.ship_heading));
-                        if (bearing < -180)
-                            bearing += 360; // i.e. 359 degrees to the left is really 1 degree to the right.
-                        else if (bearing > 180)
-                            bearing -= 360; // i.e. 359 degrees to the right is really 1 degree to the left
-                    }
+                    float heading = Convert.ToSingle(value);
+                    if (!float.IsInfinity(heading) && !float.IsNaN(heading))
+                        Value = heading;
                 }
                 catch
                 {
@@ -157,11 +150,10 @@ void IFlightControlParameter.UpdateValue(object value, SharedObjects shared)
                             KOSNomenclature.GetKOSName(typeof(VesselTarget)),
                             KOSNomenclature.GetKOSName(typeof(GeoCoordinates)),
                             KOSNomenclature.GetKOSName(typeof(ScalarValue))
-                            )
-                        );
+                        )
+                    );
                 }
             }
-            Value = bearing;
         }
     }
 }

src/kOS/Control/WheelThrottleManager.cs

@@ -91,7 +91,7 @@ object IFlightControlParameter.GetValue()
             {
                 return Value;
             }
-            return internalVessel.ctrlState.mainThrottle;
+            return internalVessel.ctrlState.wheelThrottle;
         }
 
         void IFlightControlParameter.UpdateAutopilot(FlightCtrlState c, ControlTypes ctrlLock)

src/kOS/Suffixed/GeoCoordinates.cs

@@ -127,7 +127,7 @@ public static GeoCoordinates CreateFromDump(SafeSharedObjects shared, Dump d)
         /// <returns> bearing </returns>
         public ScalarValue GetBearing()
         {
-            return VesselUtils.AngleDelta(VesselUtils.GetHeading(Shared.Vessel), (float) GetHeadingFrom());
+            return VesselUtils.AngleDelta(VesselUtils.GetHeading(Shared.Vessel), (float) GetHeading());
         }
 
         /// <summary>
@@ -240,7 +240,7 @@ private bool RaycastForTerrain(Vector3d worldRayCastStart, Vector3d worldRayCast
         ///   LAT/LANG position on the SOI body's surface.
         /// </summary>
         /// <returns>compass heading in degrees</returns>
-        private ScalarValue GetHeadingFrom()
+        public ScalarValue GetHeading()
         {
             var up = Shared.Vessel.upAxis;
             var north = VesselUtils.GetNorthVector(Shared.Vessel);
@@ -321,7 +321,7 @@ private void GeoCoordsInitializeSuffixes()
             AddSuffix("BODY", new Suffix<BodyTarget>(()=> BodyTarget.CreateOrGetExisting(Body, Shared)));
             AddSuffix("TERRAINHEIGHT", new Suffix<ScalarValue>(GetTerrainAltitude));
             AddSuffix("DISTANCE", new Suffix<ScalarValue>(GetDistanceFrom));
-            AddSuffix("HEADING", new Suffix<ScalarValue>(GetHeadingFrom));
+            AddSuffix("HEADING", new Suffix<ScalarValue>(GetHeading));
             AddSuffix("BEARING", new Suffix<ScalarValue>(GetBearing));
             AddSuffix("POSITION", new Suffix<Vector>(GetPosition,
                                                      "Get the 3-D space position relative to the ship center, of this lat/long, " +