From d7c34ddee4b266bfe83dc8a5211b50b082501158 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 13 Nov 2018 20:32:40 +0100 Subject: [PATCH] mavlink: fix yawing to North for LOITER_TIME (#10828) * mavlink: yaw should wrap at 2 pi, not pi * mavlink: use newer rad/deg conversion --- src/modules/mavlink/mavlink_mission.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index fcecccc79a..5a5b7c674b 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -53,7 +53,7 @@ #include #include -using matrix::wrap_pi; +using matrix::wrap_2pi; dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; bool MavlinkMissionManager::_dataman_init = false; @@ -1321,13 +1321,13 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->nav_cmd = NAV_CMD_WAYPOINT; mission_item->time_inside = mavlink_mission_item->param1; mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_LOITER_UNLIM: mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; mission_item->loiter_radius = mavlink_mission_item->param3; - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_LOITER_TIME: @@ -1335,19 +1335,22 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->time_inside = mavlink_mission_item->param1; mission_item->loiter_radius = mavlink_mission_item->param3; mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0); + // Yaw is only valid for multicopter but we set it always because + // it's just ignored for fixedwing. + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_LAND: mission_item->nav_cmd = NAV_CMD_LAND; // TODO: abort alt param1 - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); mission_item->land_precision = mavlink_mission_item->param2; break; case MAV_CMD_NAV_TAKEOFF: mission_item->nav_cmd = NAV_CMD_TAKEOFF; mission_item->pitch_min = mavlink_mission_item->param1; - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_LOITER_TO_ALT: @@ -1383,7 +1386,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_NAV_VTOL_TAKEOFF: case MAV_CMD_NAV_VTOL_LAND: mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_FENCE_RETURN_POINT: @@ -1621,7 +1624,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case MAV_CMD_NAV_VTOL_TAKEOFF: case MAV_CMD_NAV_VTOL_LAND: - mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param4 = math::degrees(mission_item->yaw); break; case MAV_CMD_NAV_FENCE_RETURN_POINT: