fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 15:14:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: handle param4/5 from mission_item_int

Browse Source 
It turns out that we did not properly handle param4 and param5 if they
are transferred using mission_item_int if the frame is
MAV_FRAME_MISSION.
This commit is contained in:
Julian Oes 2020-10-28 14:29:15 +01:00 committed by Daniel Agar
parent 12ebbd995d
commit 5ae5c03c66
1 changed files with 32 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1427,8 +1427,21 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->params[1] = mavlink_mission_item->param2;
mission_item->params[2] = mavlink_mission_item->param3;
mission_item->params[3] = mavlink_mission_item->param4;
mission_item->params[4] = mavlink_mission_item->x;
mission_item->params[5] = mavlink_mission_item->y;
if (_int_mode) {
/* The argument is actually a mavlink_mission_item_int_t in int_mode.
* mavlink_mission_item_t and mavlink_mission_item_int_t have the same
* alignment, so we can just swap float for int32_t. */
const mavlink_mission_item_int_t *item_int
= reinterpret_cast<const mavlink_mission_item_int_t *>(mavlink_mission_item);
mission_item->params[4] = ((double)item_int->x);
mission_item->params[5] = ((double)item_int->y);
} else {
mission_item->params[4] = (double)mavlink_mission_item->x;
mission_item->params[5] = (double)mavlink_mission_item->y;
}
mission_item->params[6] = mavlink_mission_item->z;
switch (mavlink_mission_item->command) {
@ -1519,8 +1532,25 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
mavlink_mission_item->param2 = mission_item->params[1];
mavlink_mission_item->param3 = mission_item->params[2];
mavlink_mission_item->param4 = mission_item->params[3];
mavlink_mission_item->x = mission_item->params[4];
mavlink_mission_item->y = mission_item->params[5];
if (_int_mode) {
// This function actually receives a mavlink_mission_item_int_t in _int_mode
// which has the same alignment as mavlink_mission_item_t and the only
// difference is int32_t vs. float for x and y.
mavlink_mission_item_int_t *item_int =
reinterpret_cast<mavlink_mission_item_int_t *>(mavlink_mission_item);
item_int->x = round(mission_item->params[4]);
item_int->y = round(mission_item->params[5]);
} else {
mavlink_mission_item->x = (float)mission_item->params[4];
mavlink_mission_item->y = (float)mission_item->params[5];
}
mavlink_mission_item->z = mission_item->params[6];
switch (mavlink_mission_item->command) {