Drone pathfinding fix and improvement