From 666cf2326d0d15fbd1c8daa3e86ba13dc8045a12 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 3 Mar 2022 13:25:00 +0100 Subject: [PATCH] mission_block: handle SET_ROI_LOCATION with absolute altitude correctly (#19258) --- src/modules/navigator/mission_block.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 46577383d7..c384809676 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -538,16 +538,18 @@ MissionBlock::issue_command(const mission_item_s &item) vcmd.param2 = item.params[1]; vcmd.param3 = item.params[2]; vcmd.param4 = item.params[3]; + vcmd.param5 = static_cast(item.params[4]); + vcmd.param6 = static_cast(item.params[5]); + vcmd.param7 = item.params[6]; - if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION && item.altitude_is_relative) { + if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { + // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision vcmd.param5 = item.lat; vcmd.param6 = item.lon; - vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; - } else { - vcmd.param5 = (double)item.params[4]; - vcmd.param6 = (double)item.params[5]; - vcmd.param7 = item.params[6]; + if (item.altitude_is_relative) { + vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; + } } _navigator->publish_vehicle_cmd(&vcmd);