Improved Arducopter's waypoint navigation