Implemented altitude and velocity hold mode

Implemented Fixedwing Velocity and Altitude hold mode based on fw_pos_control_l1. 

In this mode throttle controlls airspeed between min and max speeds (set by FW_AIRSPD_MAX and FW_AIRSPD_MIN) and pitch controls vertical speed between FW_T_CLIMB_MAX and FW_T_SINK_MAX.