navigator: skip takeoff if already above takeoff altitude
mission: takeoff and next waypoint related fixes
Merge branch 'takeoff_fix' of https://github.com/PX4/Firmware into mpc_track_Vz_gps