Ardu copter 3.1.1

Copter, AC3.1.1 + nav waypoint initialization changes