From a0176474c7740a4a04c2eb91eba57d8ce64027a1 Mon Sep 17 00:00:00 2001 From: Elikos default Date: Thu, 11 Jun 2015 20:28:47 -0400 Subject: [PATCH] fix NaN yaw breaking attitude setpoints when going back into posctl from offboard --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/platforms/ros/nodes/mavlink/mavlink.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7ede93ec2d..1a6494ad93 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -621,7 +621,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; @@ -630,7 +630,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;