From bedaafcc200c4d99ac586d35dc8a834e88c9a303 Mon Sep 17 00:00:00 2001 From: Nicolas de Palezieux Date: Fri, 5 Jan 2018 16:48:06 +0100 Subject: [PATCH] NAV_CMD_DO_SET_ROI: handle relative altitude in mission ROI commands --- src/modules/navigator/mission_block.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 6f143bea14..ff87359161 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -448,7 +448,13 @@ MissionBlock::issue_command(const mission_item_s &item) vcmd.param4 = item.params[3]; vcmd.param5 = item.params[4]; vcmd.param6 = item.params[5]; - vcmd.param7 = item.params[6]; + + if (item.nav_cmd == NAV_CMD_DO_SET_ROI && item.altitude_is_relative) { + vcmd.param7 = item.params[6] + _navigator->get_home_position()->alt; + + } else { + vcmd.param7 = item.params[6]; + } _navigator->publish_vehicle_cmd(&vcmd); }