diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4e3da6c7c0..e76f907176 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -680,7 +680,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					}
 
 					/* set the yaw sp value */
-					if (!offboard_control_mode.ignore_attitude) {
+					if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) {
 						pos_sp_triplet.current.yaw_valid = true;
 						pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
 
@@ -689,7 +689,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					}
 
 					/* set the yawrate sp value */
-					if (!offboard_control_mode.ignore_bodyrate) {
+					if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) {
 						pos_sp_triplet.current.yawspeed_valid = true;
 						pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
 
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp
index 2758979a29..c92dd0843d 100644
--- a/src/platforms/ros/nodes/mavlink/mavlink.cpp
+++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp
@@ -273,7 +273,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *
 		}
 
 		/* set the yaw sp value */
-		if (!offboard_control_mode.ignore_attitude) {
+		if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) {
 			pos_sp_triplet.current.yaw_valid = true;
 			pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
 
@@ -282,7 +282,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *
 		}
 
 		/* set the yawrate sp value */
-		if (!offboard_control_mode.ignore_bodyrate) {
+		if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) {
 			pos_sp_triplet.current.yawspeed_valid = true;
 			pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;