Fix detection of RCS for PVG terminal RCS

This check is relative to the maximum RCS in any one direction so that slightly misaligned transforms won't register as the RCS axis that we need. Hopefully people don't have massive pitch/yaw/roll thrusters with only tiny ullage RCS. Signed-off-by: Lamont Granquist <[email protected]>
rnlahaye · Apr 9, 2022 · fd21847 · fd21847
1 parent eb34316
commit fd21847
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 1 deletion.
diff --git a/MechJeb2/MathExtensions.cs b/MechJeb2/MathExtensions.cs
@@ -25,6 +25,11 @@ public static Vector3d Sqrt(this Vector3d vector)
             return new Vector3d(Math.Sqrt(vector.x), Math.Sqrt(vector.y), Math.Sqrt(vector.z));
         }
 
+        public static double MaxMagnitude(this Vector3d vector)
+        {
+            return Math.Max(Math.Max(Math.Abs(vector.x),Math.Abs(vector.y)),Math.Abs(vector.z));
+        }
+
         public static Vector3d Reorder(this Vector3d vector, int order)
         {
             switch (order)

diff --git a/MechJeb2/MechJebModuleGuidanceController.cs b/MechJeb2/MechJebModuleGuidanceController.cs
@@ -140,7 +140,9 @@ public override void OnFixedUpdate()
 
             if ( p != null && p.Solution != null && isTerminalGuidance() )
             {
-                bool has_rcs = vessel.hasEnabledRCSModules(); // && ( vesselState.rcsThrustAvailable.up > 0 );
+                // We might have wonky transforms and have a tiny bit of fore RCS, so require at least 10% of the max RCS thrust to be
+                // in the pointy direction (which should be "up" / y-axis per KSP/Unity semantics).
+                bool has_rcs = vessel.hasEnabledRCSModules() && vesselState.rcsThrustAvailable.up > 0.1 * vesselState.rcsThrustAvailable.MaxMagnitude();
 
                 // stopping one tick short is more accurate for rockets without RCS, but sometimes we overshoot with only one tick
                 int ticks = 1;
@@ -156,6 +158,7 @@ public override void OnFixedUpdate()
                 // bit of a hack to predict velocity + position in the next tick or two
                 // FIXME: what exactly does KSP do to integrate over timesteps?
                 Vector3d a0 = vessel.acceleration_immediate;
+
                 double dt = ticks * TimeWarp.fixedDeltaTime;
                 Vector3d v1 = vesselState.orbitalVelocity + a0 * dt;
                 Vector3d x1 = vesselState.orbitalPosition + vesselState.orbitalVelocity * dt + 1/2 * a0 * dt * dt;

diff --git a/MechJeb2/Vector6.cs b/MechJeb2/Vector6.cs
@@ -107,6 +107,11 @@ public double GetMagnitude(Vector3d direction)
             return Math.Sqrt(sqrMagnitude);
         }
 
+        public double MaxMagnitude()
+        {
+            return Math.Max(positive.MaxMagnitude(),negative.MaxMagnitude());
+        }
+
         public void Load(ConfigNode node)
         {
             if (node.HasValue("positive"))