fix: leaving course after pathfinder track

When CP uses the pathfinder for a connecting track and
there is a headland turn with more than 90 degrees right
after the connecting track, won't move the relevant waypoint
past the corner as the corner waypoint heading points
"backwards" (>90).

Make sure that when resuming from the StartRowOnly turn,
if there is no waypoint in front of the vehicle, use the
one after the turn end wp in the context.

This will still skip the turn maneuver but at least the
driver will continue the course.

#881

Author: Peter Vaiko

scripts/ai/PurePursuitController.lua

@@ -603,18 +603,6 @@ function PurePursuitController:isReversing()
     end
 end
 
-function PurePursuitController:getDirection(lz)
-    local ctx, cty, ctz = self:getClosestWaypointData()
-    if not ctx then
-        return lz
-    end
-    local dx, _, dz = worldToLocal(self.controlledNode, ctx, cty, ctz)
-    local distance = math.sqrt(dx * dx + dz * dz)
-    local r = distance * distance / 2 / dx
-    local steeringAngle = math.atan(self.vehicle.cp.distances.frontWheelToRearWheel / r)
-    return math.cos(steeringAngle)
-end
-
 -- goal point local position in the vehicle's coordinate system
 function PurePursuitController:getGoalPointLocalPosition()
     return localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0)

scripts/ai/strategies/AIDriveStrategyFieldWorkCourse.lua

@@ -411,9 +411,12 @@ function AIDriveStrategyFieldWorkCourse:resumeFieldworkAfterTurn(ix)
     self.ppc:setNormalLookaheadDistance()
     self:startWaitingForLower()
     self:lowerImplements()
-    local startIx = self.fieldWorkCourse:getNextFwdWaypointIxFromVehiclePosition(ix,
+    local startIx, found = self.fieldWorkCourse:getNextFwdWaypointIxFromVehiclePosition(ix,
             self.vehicle:getAIDirectionNode(), self.workWidth / 2)
-    self:startCourse(self.fieldWorkCourse, startIx)
+    -- if we can't found a waypoint in front of us, just use the next (ix would be the turn end, this is after that)
+    -- ix may be problematic, especially if the next waypoint is a headland corner with > 90 degrees angle, PPC
+    -- may never advance to the next waypoint
+    self:startCourse(self.fieldWorkCourse, found and startIx or ix + 1)
 end
 
 --- Attempt to recover from a turn where the vehicle got blocked. This replaces the current turn with a