diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg
index 927c8e6503..eaff918152 100644
--- a/msg/offboard_control_mode.msg
+++ b/msg/offboard_control_mode.msg
@@ -6,3 +6,4 @@ bool ignore_bodyrate
 bool ignore_position
 bool ignore_velocity
 bool ignore_acceleration_force
+bool ignore_alt_hold
diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg
index eba8ed9160..fa3b40389d 100644
--- a/msg/position_setpoint.msg
+++ b/msg/position_setpoint.msg
@@ -19,6 +19,7 @@ float32 vx			# local velocity setpoint in m/s in NED
 float32 vy			# local velocity setpoint in m/s in NED
 float32 vz			# local velocity setpoint in m/s in NED
 bool velocity_valid		# true if local velocity setpoint valid
+bool alt_valid		# do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control.
 float64 lat			# latitude, in deg
 float64 lon			# longitude, in deg
 float32 alt			# altitude AMSL, in m
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 0cd5bd633d..513f29ff3b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -391,6 +391,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 				// not even running. The main mavlink thread takes care of this by waiting for an ack
 				// from the logger.
 				_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid);
+
 			} else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
 				_mavlink->request_stop_ulog_streaming();
 			}
@@ -749,6 +750,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 
 		/* convert mavlink type (local, NED) to uORB offboard control struct */
 		offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
+		offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4);
 		offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
 		offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
 		bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
@@ -846,6 +848,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 						pos_sp_triplet.current.velocity_valid = false;
 					}
 
+					if (!offboard_control_mode.ignore_alt_hold) {
+						pos_sp_triplet.current.alt_valid = true;
+						pos_sp_triplet.current.z = set_position_target_local_ned.z;
+
+					} else {
+						pos_sp_triplet.current.alt_valid = false;
+					}
+
 					/* set the local acceleration values if the setpoint type is 'local pos' and none
 					 * of the accelerations fields is set to 'ignore' */
 					if (!offboard_control_mode.ignore_acceleration_force) {
@@ -1242,6 +1252,7 @@ MavlinkReceiver::handle_message_logging_ack(mavlink_message_t *msg)
 	mavlink_msg_logging_ack_decode(msg, &logging_ack);
 
 	MavlinkULog *ulog_streaming = _mavlink->get_ulog_streaming();
+
 	if (ulog_streaming) {
 		ulog_streaming->handle_ack(logging_ack);
 	}
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 37c782af13..3c3f1c3802 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -937,9 +937,15 @@ MulticopterPositionControl::control_offboard(float dt)
 			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
 		}
 
-		if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
-			/* Control altitude */
+		if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.alt_valid) {
+			/* control altitude as it is enabled */
 			_pos_sp(2) = _pos_sp_triplet.current.z;
+			_run_alt_control = true;
+
+		} else if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
+			/* control altitude because full position control is enabled */
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+			_run_alt_control = true;
 
 		} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* reset alt setpoint to current altitude if needed */