yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: fix yawing to North for LOITER_TIME (#10828)

Browse Source 
* mavlink: yaw should wrap at 2 pi, not pi
* mavlink: use newer rad/deg conversion
This commit is contained in:
Julian Oes
2018-11-13 20:32:40 +01:00
committed by Daniel Agar
parent 9594ebf72e
commit d7c34ddee4
1 changed files with 10 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/mavlink/mavlink_mission.cpp
View File

@@ -53,7 +53,7 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
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: