diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4c849c14da63..a5ad8fab6604 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -642,11 +642,24 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 	struct offboard_control_mode_s offboard_control_mode;
 	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
 
+	bool values_finite =
+		PX4_ISFINITE(set_position_target_local_ned.x) &&
+		PX4_ISFINITE(set_position_target_local_ned.y) &&
+		PX4_ISFINITE(set_position_target_local_ned.z) &&
+		PX4_ISFINITE(set_position_target_local_ned.vx) &&
+		PX4_ISFINITE(set_position_target_local_ned.vy) &&
+		PX4_ISFINITE(set_position_target_local_ned.vz) &&
+		PX4_ISFINITE(set_position_target_local_ned.afx) &&
+		PX4_ISFINITE(set_position_target_local_ned.afy) &&
+		PX4_ISFINITE(set_position_target_local_ned.afz) &&
+		PX4_ISFINITE(set_position_target_local_ned.yaw);
+
 	/* Only accept messages which are intended for this system */
 	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
 				set_position_target_local_ned.target_system == 0) &&
 			(mavlink_system.compid == set_position_target_local_ned.target_component ||
-			 set_position_target_local_ned.target_component == 0)) {
+				set_position_target_local_ned.target_component == 0) &&
+			values_finite) {
 
 		/* convert mavlink type (local, NED) to uORB offboard control struct */
 		offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
@@ -658,6 +671,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 		/* yawrate ignore flag mapps to ignore_bodyrate */
 		offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
 
+
+		bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000);
+		bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000);
+		bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000);
+		bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);
+
 		offboard_control_mode.timestamp = hrt_absolute_time();
 
 		if (_offboard_control_mode_pub == nullptr) {
@@ -696,7 +715,22 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					pos_sp_triplet.previous.valid = false;
 					pos_sp_triplet.next.valid = false;
 					pos_sp_triplet.current.valid = true;
-					pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others
+
+					if (is_takeoff_sp) {
+						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
+
+					} else if (is_land_sp) {
+						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
+
+					} else if (is_loiter_sp) {
+						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+
+					} else if (is_idle_sp) {
+						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
+
+					} else {
+						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+					}
 
 					/* set the local pos values */
 					if (!offboard_control_mode.ignore_position) {
@@ -733,7 +767,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					}
 
 					/* set the yaw sp value */
-					if (!offboard_control_mode.ignore_attitude && PX4_ISFINITE(set_position_target_local_ned.yaw)) {
+					if (!offboard_control_mode.ignore_attitude) {
 						pos_sp_triplet.current.yaw_valid = true;
 						pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
 
@@ -742,7 +776,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					}
 
 					/* set the yawrate sp value */
-					if (!offboard_control_mode.ignore_bodyrate && PX4_ISFINITE(set_position_target_local_ned.yaw)) {
+					if (!offboard_control_mode.ignore_bodyrate) {
 						pos_sp_triplet.current.yawspeed_valid = true;
 						pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
 
@@ -751,7 +785,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					}
 					//XXX handle global pos setpoints (different MAV frames)
 
-
 					if (_pos_sp_triplet_pub == nullptr) {
 						_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
 								&pos_sp_triplet);
@@ -780,10 +813,21 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints
 
+	bool values_finite =
+		PX4_ISFINITE(set_actuator_control_target.controls[0]) &&
+		PX4_ISFINITE(set_actuator_control_target.controls[1]) &&
+		PX4_ISFINITE(set_actuator_control_target.controls[2]) &&
+		PX4_ISFINITE(set_actuator_control_target.controls[3]) &&
+		PX4_ISFINITE(set_actuator_control_target.controls[4]) &&
+		PX4_ISFINITE(set_actuator_control_target.controls[5]) &&
+		PX4_ISFINITE(set_actuator_control_target.controls[6]) &&
+		PX4_ISFINITE(set_actuator_control_target.controls[7]);
+
 	if ((mavlink_system.sysid == set_actuator_control_target.target_system ||
 		    set_actuator_control_target.target_system == 0) &&
 		(mavlink_system.compid == set_actuator_control_target.target_component ||
-		    set_actuator_control_target.target_component == 0)) {
+		    set_actuator_control_target.target_component == 0) &&
+		values_finite) {
 
 		/* ignore all since we are setting raw actuators here */
 		offboard_control_mode.ignore_thrust             = true;
@@ -873,11 +917,22 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 	mavlink_set_attitude_target_t set_attitude_target;
 	mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
 
+	bool values_finite =
+		PX4_ISFINITE(set_attitude_target.q[0]) &&
+		PX4_ISFINITE(set_attitude_target.q[1]) &&
+		PX4_ISFINITE(set_attitude_target.q[2]) &&
+		PX4_ISFINITE(set_attitude_target.q[3]) &&
+		PX4_ISFINITE(set_attitude_target.thrust) &&
+		PX4_ISFINITE(set_attitude_target.body_roll_rate) &&
+		PX4_ISFINITE(set_attitude_target.body_pitch_rate) &&
+		PX4_ISFINITE(set_attitude_target.body_yaw_rate);
+
 	/* Only accept messages which are intended for this system */
 	if ((mavlink_system.sysid == set_attitude_target.target_system ||
 				set_attitude_target.target_system == 0) &&
 			(mavlink_system.compid == set_attitude_target.target_component ||
-			 set_attitude_target.target_component == 0)) {
+				set_attitude_target.target_component == 0) &&
+			values_finite) {
 
 		/* set correct ignore flags for thrust field: copy from mavlink message */
 		_offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));