Added auto-takeoff changes
Auto take-off fix with pitch working. Needs testing