Increment global path index to first unreached waypoint on new path

[patrick-5546]

This handles the case where we lose power in the middle of a global path. On startup local pathfinding assumes that the next global waypoint is index 1 of the global path, so I decided to increment the global path index to the first unreached waypoint whenever a new global path is published