Adding a takeoff mode.

Needs Jason code from https://github.com/jason4short/ardupilot/commits/Takeoff