Fixed offboard_control_pos_sp to be passed to _pos_sp vector. Added checks for position and altitude control modes.