mission_block: handle SET_ROI_LOCATION with absolute altitude correct…

…ly (#19258)
PX4 · Mar 3, 2022 · 666cf23 · 666cf23
1 parent 49df00c
commit 666cf23
Showing 1 changed file with 8 additions and 6 deletions.
diff --git 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<double>(item.params[4]);
+		vcmd.param6 = static_cast<double>(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);