Merge pull request #884 from Courseplay/881-headland-turn-right-after-pathfinder-course

fix: leaving course after pathfinder track

Author: Tensuko

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