fix stanley ab

MrPoke21 · Apr 5, 2024 · 5780ef3 · 5780ef3
1 parent 7e83332
commit 5780ef3
Showing 1 changed file with 4 additions and 4 deletions.
diff --git a/SourceCode/GPS/Classes/CGuidance.cs b/SourceCode/GPS/Classes/CGuidance.cs
@@ -119,7 +119,7 @@ private void DoSteerAngleCalc()
         /// <param name="isValid"></param>
         public void StanleyGuidanceABLine(vec3 curPtA, vec3 curPtB, vec3 pivot, vec3 steer)
         {
-            //get the steer distance from currently active AB segment   ///////////  steer  ////////////
+            //get the pivot distance from currently active AB segment   ///////////  Pivot  ////////////
             double dx = curPtB.easting - curPtA.easting;
             double dy = curPtB.northing - curPtA.northing;
             if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dy) < Double.Epsilon) return;
@@ -128,16 +128,16 @@ public void StanleyGuidanceABLine(vec3 curPtA, vec3 curPtB, vec3 pivot, vec3 ste
             mf.yt.dxAB = dx; mf.yt.dyAB = dy;
 
             //how far from current AB Line is fix
-            distanceFromCurrentLinePivot = ((dy * steer.easting) - (dx * steer.northing) + (curPtB.easting
+            distanceFromCurrentLinePivot = ((dy * pivot.easting) - (dx * pivot.northing) + (curPtB.easting
                         * curPtA.northing) - (curPtB.northing * curPtA.easting))
                             / Math.Sqrt((dy * dy) + (dx * dx));
 
             if (!mf.ABLine.isHeadingSameWay)
                 distanceFromCurrentLinePivot *= -1.0;
 
             mf.ABLine.distanceFromCurrentLinePivot = distanceFromCurrentLinePivot;
-            double U = (((steer.easting - curPtA.easting) * dx)
-                            + ((steer.northing - curPtA.northing) * dy))
+            double U = (((pivot.easting - curPtA.easting) * dx)
+                            + ((pivot.northing - curPtA.northing) * dy))
                             / ((dx * dx) + (dy * dy));
 
             rEastPivot = curPtA.easting + (U * dx);