zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

* mavlink: yaw should wrap at 2 pi, not pi
* mavlink: use newer rad/deg conversion
sbg
Julian Oes 6 years ago committed by Daniel Agar
parent
9594ebf72e
commit
d7c34ddee4
1 changed files with 10 additions and 7 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      src/modules/mavlink/mavlink_mission.cpp

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

@ -53,7 +53,7 @@ @@ -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 * @@ -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 * @@ -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 * @@ -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 * @@ -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:

Write Preview
Loading…
Cancel
Save