ROI - move handling to navigator (#7939)

msg/vehicle_command.msg

@@ -52,7 +52,7 @@ uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221			# set id of master controller |System
 uint16 VEHICLE_CMD_DO_GUIDED_LIMITS=222			# set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_LAST = 240			# NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241		# Trigger calibration. This command will be only accepted if in pre-flight mode. See mavlink spec MAV_CMD_PREFLIGHT_CALIBRATION
-uint32 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3		# param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration
+uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3		# param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration
 uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242	# Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|
 uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
@@ -69,27 +69,27 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 ==
 uint16 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
 uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
 
-uint16 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
+uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
-uint16 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |
+uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |
-uint16 VEHICLE_CMD_RESULT_DENIED = 2			# Command PERMANENTLY DENIED |
+uint8 VEHICLE_CMD_RESULT_DENIED = 2			# Command PERMANENTLY DENIED |
-uint16 VEHICLE_CMD_RESULT_UNSUPPORTED = 3		# Command UNKNOWN/UNSUPPORTED |
+uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3		# Command UNKNOWN/UNSUPPORTED |
-uint16 VEHICLE_CMD_RESULT_FAILED = 4			# Command executed, but failed |
+uint8 VEHICLE_CMD_RESULT_FAILED = 4			# Command executed, but failed |
-uint16 VEHICLE_CMD_RESULT_IN_PROGRESS = 5		# Command being executed |
+uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5		# Command being executed |
-uint16 VEHICLE_CMD_RESULT_ENUM_END = 6			#
+uint8 VEHICLE_CMD_RESULT_ENUM_END = 6			#
 
-uint16 VEHICLE_MOUNT_MODE_RETRACT = 0				# Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
+uint8 VEHICLE_MOUNT_MODE_RETRACT = 0				# Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
-uint16 VEHICLE_MOUNT_MODE_NEUTRAL = 1				# Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
+uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1				# Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
-uint16 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2		# Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization |
+uint8 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2		# Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization |
-uint16 VEHICLE_MOUNT_MODE_RC_TARGETING = 3			# Load neutral position and start RC Roll,Pitch,Yaw control with stabilization |
+uint8 VEHICLE_MOUNT_MODE_RC_TARGETING = 3			# Load neutral position and start RC Roll,Pitch,Yaw control with stabilization |
-uint16 VEHICLE_MOUNT_MODE_GPS_POINT = 4				# Load neutral position and start to point to Lat,Lon,Alt |
+uint8 VEHICLE_MOUNT_MODE_GPS_POINT = 4				# Load neutral position and start to point to Lat,Lon,Alt |
-uint16 VEHICLE_MOUNT_MODE_ENUM_END = 5				#
+uint8 VEHICLE_MOUNT_MODE_ENUM_END = 5				#
 
-uint16 VEHICLE_ROI_NONE = 0                         # No region of interest |
+uint8 VEHICLE_ROI_NONE = 0                         # No region of interest |
-uint16 VEHICLE_ROI_WPNEXT = 1                       # Point toward next MISSION |
+uint8 VEHICLE_ROI_WPNEXT = 1                       # Point toward next MISSION |
-uint16 VEHICLE_ROI_WPINDEX = 2                      # Point toward given MISSION |
+uint8 VEHICLE_ROI_WPINDEX = 2                      # Point toward given MISSION |
-uint16 VEHICLE_ROI_LOCATION = 3                     # Point toward fixed location |
+uint8 VEHICLE_ROI_LOCATION = 3                     # Point toward fixed location |
-uint16 VEHICLE_ROI_TARGET = 4                       # Point toward target
+uint8 VEHICLE_ROI_TARGET = 4                       # Point toward target
-uint16 VEHICLE_ROI_ENUM_END = 5
+uint8 VEHICLE_ROI_ENUM_END = 5
 
 uint8 ORB_QUEUE_LENGTH = 3
 

msg/vehicle_roi.msg

@@ -1,15 +1,17 @@
 # Vehicle Region Of Interest (ROI)
 
-uint8 VEHICLE_ROI_NONE = 0                         # No region of interest |
+uint8 ROI_NONE = 0                         # No region of interest
-uint8 VEHICLE_ROI_WPNEXT = 1                       # Point toward next MISSION |
+uint8 ROI_WPNEXT = 1                       # Point toward next MISSION
-uint8 VEHICLE_ROI_WPINDEX = 2                      # Point toward given MISSION |
+uint8 ROI_WPINDEX = 2                      # Point toward given MISSION
-uint8 VEHICLE_ROI_LOCATION = 3                     # Point toward fixed location |
+uint8 ROI_LOCATION = 3                     # Point toward fixed location
-uint8 VEHICLE_ROI_TARGET = 4                       # Point toward target
+uint8 ROI_TARGET = 4                       # Point toward target
-uint8 VEHICLE_ROI_ENUM_END = 5
+uint8 ROI_ENUM_END = 5
 
 uint8 mode          # ROI mode (see above)
+
 uint32 mission_seq  # mission sequence to point to
 uint32 target_seq   # target sequence to point to
+
 float64 lat			    # Latitude to point to
 float64 lon			    # Longitude to point to
 float32 alt			    # Altitude to point to

msg/vehicle_status.msg

@@ -48,8 +48,8 @@ uint8 hil_state				# current hil state
 bool failsafe				# true if system is in failsafe state
 
 uint8 system_type			# system type, contains mavlink MAV_TYPE
-uint32 system_id			# system id, contains MAVLink's system ID field
+uint8 system_id			# system id, contains MAVLink's system ID field
-uint32 component_id			# subsystem / component id, contains MAVLink's component ID field
+uint8 component_id			# subsystem / component id, contains MAVLink's component ID field
 
 bool is_rotary_wing			# True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
 bool is_vtol				# True if the system is VTOL capable

src/drivers/vmount/input_mavlink.cpp

@@ -49,15 +49,9 @@
 #include <px4_posix.h>
 #include <errno.h>
 
-
 namespace vmount
 {
 
-
-InputMavlinkROI::InputMavlinkROI()
-{
-}
-
 InputMavlinkROI::~InputMavlinkROI()
 {
 	if (_vehicle_roi_sub >= 0) {
@@ -116,27 +110,27 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
 
 			_control_data.gimbal_shutter_retract = false;
 
-			if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
+			if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) {
 
 				_control_data.type = ControlData::Type::Neutral;
 				*control_data = &_control_data;
 
-			} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
+			} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
 				_read_control_data_from_position_setpoint_sub();
 				_control_data.type_data.lonlat.roll_angle = 0.f;
 				_control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
 
 				*control_data = &_control_data;
 
-			} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
+			} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPINDEX) {
 				//TODO how to do this?
 
-			} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
+			} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
 				control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
 
 				*control_data = &_control_data;
 
-			} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
+			} else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) {
 				//TODO is this even suported?
 			}
 
@@ -150,7 +144,7 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
 
 		// check whether the position setpoint got updated
 		if (polls[1].revents & POLLIN) {
-			if (_cur_roi_mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
+			if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) {
 				_read_control_data_from_position_setpoint_sub();
 				*control_data = &_control_data;
 

src/drivers/vmount/input_mavlink.h

@@ -57,7 +57,7 @@ namespace vmount
 class InputMavlinkROI : public InputBase
 {
 public:
-	InputMavlinkROI();
+	InputMavlinkROI() = default;
 	virtual ~InputMavlinkROI();
 
 	virtual void print_status();
@@ -71,7 +71,7 @@ private:
 
 	int _vehicle_roi_sub = -1;
 	int _position_setpoint_triplet_sub = -1;
-	uint8_t _cur_roi_mode = vehicle_roi_s::VEHICLE_ROI_NONE;
+	uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
 };
 
 

src/modules/commander/arm_auth.cpp

@@ -66,7 +66,7 @@ struct packed_struct {
 	uint8_t authentication_method;
 } arm_parameters;
 
-static uint32_t *system_id;
+static uint8_t *system_id;
 
 static uint8_t _auth_method_arm_req_check();
 static uint8_t _auth_method_two_arm_check();
@@ -260,7 +260,7 @@ void arm_auth_update(hrt_abstime now)
 	}
 }
 
-void arm_auth_init(orb_advert_t *mav_log_pub, uint32_t *sys_id)
+void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id)
 {
 	system_id = sys_id;
 	param_arm_parameters = param_find("COM_ARM_AUTH");

src/modules/commander/arm_auth.h

@@ -42,7 +42,7 @@ enum arm_auth_methods {
 	ARM_AUTH_METHOD_LAST
 };
 
-void arm_auth_init(orb_advert_t *mav_log_pub, uint32_t *system_id);
+void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *system_id);
 void arm_auth_update(hrt_abstime now);
 uint8_t arm_auth_check();
 enum arm_auth_methods arm_auth_method_get();

src/modules/commander/commander.cpp

@@ -106,7 +106,6 @@
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/power_button_state.h>
-#include <uORB/topics/vehicle_roi.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/sensor_combined.h>
@@ -225,7 +224,6 @@ static float max_imu_gyr_diff = 0.09f;
 static float min_stick_change = 0.25f;
 
 static struct vehicle_status_s status = {};
-static struct vehicle_roi_s _roi = {};
 static struct battery_status_s battery = {};
 static struct actuator_armed_s armed = {};
 static struct safety_s safety = {};
@@ -286,8 +284,7 @@ void usage(const char *reason);
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
 		    struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
 		    struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
-		    orb_advert_t *command_ack_pub, struct vehicle_roi_s *roi,
+		    orb_advert_t *command_ack_pub, bool *changed);
-		    orb_advert_t *roi_pub, bool *changed);
 
 /**
  * Mainloop of commander.
@@ -514,8 +511,8 @@ int commander_main(int argc, char *argv[])
 					.param4 = NAN,
 					.param7 = NAN,
 					.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF,
-					.target_system = (uint8_t)status.system_id,
+					.target_system = status.system_id,
-					.target_component = (uint8_t)status.component_id
+					.target_component = status.component_id
 				};
 
 				orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -545,8 +542,8 @@ int commander_main(int argc, char *argv[])
 			.param4 = NAN,
 			.param7 = NAN,
 			.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND,
-			.target_system = (uint8_t)status.system_id,
+			.target_system = status.system_id,
-			.target_component = (uint8_t)status.component_id
+			.target_component = status.component_id
 		};
 
 		orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -568,8 +565,8 @@ int commander_main(int argc, char *argv[])
 			.param4 = NAN,
 			.param7 = NAN,
 			.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
-			.target_system = (uint8_t)status.system_id,
+			.target_system = status.system_id,
-			.target_component = (uint8_t)status.component_id
+			.target_component = status.component_id
 		};
 
 		orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -637,8 +634,8 @@ int commander_main(int argc, char *argv[])
 			.param4 = 0.0f,
 			.param7 = 0.0f,
 			.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
-			.target_system = (uint8_t)status.system_id,
+			.target_system = status.system_id,
-			.target_component = (uint8_t)status.component_id
+			.target_component = status.component_id
 		};
 
 		orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -764,8 +761,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		    struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
 		    struct home_position_s *home, struct vehicle_global_position_s *global_pos,
 		    struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
-		    orb_advert_t *command_ack_pub, struct vehicle_roi_s *roi,
+		    orb_advert_t *command_ack_pub, bool *changed)
-		    orb_advert_t *roi_pub, bool *changed)
 {
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@@ -1159,34 +1155,6 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
-	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: {
-
-		roi->mode = cmd->param1;
-
-		if (roi->mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
-			roi->mission_seq =  cmd->param2;
-		}
-		else if (roi->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
-			roi->lat = cmd->param5;
-			roi->lon = cmd->param6;
-			roi->alt = cmd->param7;
-		}
-		else if (roi->mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
-			roi->target_seq = cmd->param2;
-		}
-
-		if (*roi_pub != nullptr) {
-			orb_publish(ORB_ID(vehicle_roi), *roi_pub, roi);
-
-		} else {
-			*roi_pub = orb_advertise(ORB_ID(vehicle_roi), roi);
-		}
-
-		cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
-		break;
-	}
 	case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
 
 		cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
@@ -1529,10 +1497,6 @@ int commander_thread_main(int argc, char *argv[])
 	orb_advert_t home_pub = nullptr;
 	memset(&_home, 0, sizeof(_home));
 
-	/* region of interest */
-	orb_advert_t roi_pub = nullptr;
-	memset(&_roi, 0, sizeof(_roi));
-
 	/* command ack */
 	orb_advert_t command_ack_pub = nullptr;
 
@@ -1818,13 +1782,12 @@ int commander_thread_main(int argc, char *argv[])
 	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr);
 	pthread_attr_destroy(&commander_low_prio_attr);
 
-	arm_auth_init(&mavlink_log_pub, &(status.system_id));
+	arm_auth_init(&mavlink_log_pub, &status.system_id);
 
 	while (!thread_should_exit) {
 
 		arming_ret = TRANSITION_NOT_CHANGED;
 
-
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 
@@ -1852,8 +1815,13 @@ int commander_thread_main(int argc, char *argv[])
 				status.is_vtol = is_vtol(&status);
 
 				/* check and update system / component ID */
-				param_get(_param_system_id, &(status.system_id));
+				int32_t sys_id = 0;
-				param_get(_param_component_id, &(status.component_id));
+				param_get(_param_system_id, &sys_id);
+				status.system_id = sys_id;
+
+				int32_t comp_id = 0;
+				param_get(_param_component_id, &comp_id);
+				status.component_id = comp_id;
 
 				get_circuit_breaker_params();
 
@@ -3008,7 +2976,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* handle it */
 			if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position,
-					&attitude, &home_pub, &command_ack_pub, &_roi, &roi_pub, &status_changed)) {
+					&attitude, &home_pub, &command_ack_pub, &status_changed)) {
 				status_changed = true;
 			}
 		}

src/modules/mavlink/mavlink_mission.cpp

@@ -1333,6 +1333,22 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0);
 			break;
 
+		case MAV_CMD_NAV_ROI:
+		case MAV_CMD_DO_SET_ROI:
+			if ((int)mavlink_mission_item->param1 == MAV_ROI_LOCATION) {
+				mission_item->nav_cmd = NAV_CMD_DO_SET_ROI;
+				mission_item->params[0] = MAV_ROI_LOCATION;
+
+				mission_item->params[4] = mavlink_mission_item->x;
+				mission_item->params[5] = mavlink_mission_item->y;
+				mission_item->params[6] = mavlink_mission_item->z;
+
+			} else {
+				return MAV_MISSION_INVALID_PARAM1;
+			}
+
+			break;
+
 		case MAV_CMD_NAV_VTOL_TAKEOFF:
 		case MAV_CMD_NAV_VTOL_LAND:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
@@ -1391,6 +1407,19 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
 			break;
 
+		case MAV_CMD_NAV_ROI:
+		case MAV_CMD_DO_SET_ROI: {
+				const int roi_mode = mavlink_mission_item->param1;
+
+				if (roi_mode == MAV_ROI_NONE || roi_mode == MAV_ROI_WPNEXT || roi_mode == MAV_ROI_WPINDEX) {
+					mission_item->nav_cmd = NAV_CMD_DO_SET_ROI;
+
+				} else {
+					return MAV_MISSION_INVALID_PARAM1;
+				}
+			}
+			break;
+
 		case MAV_CMD_DO_CHANGE_SPEED:
 		case MAV_CMD_DO_SET_SERVO:
 		case MAV_CMD_DO_LAND_START:
@@ -1402,8 +1431,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		case MAV_CMD_IMAGE_STOP_CAPTURE:
 		case MAV_CMD_VIDEO_START_CAPTURE:
 		case MAV_CMD_VIDEO_STOP_CAPTURE:
-		case NAV_CMD_DO_SET_ROI:
-		case NAV_CMD_ROI:
 		case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
 		case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 		case MAV_CMD_SET_CAMERA_MODE:
@@ -1478,7 +1505,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 		case NAV_CMD_DO_MOUNT_CONFIGURE:
 		case NAV_CMD_DO_MOUNT_CONTROL:
 		case NAV_CMD_DO_SET_ROI:
-		case NAV_CMD_ROI:
 		case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
 		case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 		case NAV_CMD_SET_CAMERA_MODE:

src/modules/navigator/datalinkloss.cpp

@@ -168,7 +168,7 @@ DataLinkLoss::set_dll_item()
 	reset_mission_item_reached();
 
 	/* convert mission item to current position setpoint and make it valid */
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/enginefailure.cpp

@@ -126,7 +126,7 @@ EngineFailure::set_ef_item()
 	reset_mission_item_reached();
 
 	/* convert mission item to current position setpoint and make it valid */
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/follow_target.cpp

@@ -361,7 +361,7 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
 
 	pos_sp_triplet->previous.valid = use_position;
 	pos_sp_triplet->previous = pos_sp_triplet->current;
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
 	pos_sp_triplet->current.position_valid = use_position;
 	pos_sp_triplet->current.velocity_valid = use_velocity;

src/modules/navigator/land.cpp

@@ -82,7 +82,7 @@ Land::on_activation()
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->previous.valid = false;
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_can_loiter_at_sp(false);
@@ -99,7 +99,7 @@ Land::on_active()
 		set_idle_item(&_mission_item);
 
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 }

src/modules/navigator/loiter.cpp

@@ -129,7 +129,7 @@ Loiter::set_loiter_position()
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->current.velocity_valid = false;
 	pos_sp_triplet->previous.valid = false;
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

src/modules/navigator/mission.cpp

@@ -145,13 +145,12 @@ void
 Mission::on_inactivation()
 {
 	// Disable camera trigger
-	vehicle_command_s cmd{};
+	vehicle_command_s cmd = {};
 	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
 	// Pause trigger
 	cmd.param1 = -1.0f;
 	cmd.param3 = 1.0f;
-	cmd.timestamp = hrt_absolute_time();
+	_navigator->publish_vehicle_cmd(&cmd);
-	_navigator->publish_vehicle_cmd(cmd);
 }
 
 void
@@ -160,13 +159,12 @@ Mission::on_activation()
 	set_mission_items();
 
 	// unpause triggering if it was paused
-	vehicle_command_s cmd{};
+	vehicle_command_s cmd = {};
 	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
 	// unpause trigger
 	cmd.param1 = -1.0f;
 	cmd.param3 = 0.0f;
-	cmd.timestamp = hrt_absolute_time();
+	_navigator->publish_vehicle_cmd(&cmd);
-	_navigator->publish_vehicle_cmd(cmd);
 }
 
 void
@@ -510,7 +508,7 @@ Mission::set_mission_items()
 
 		/* update position setpoint triplet  */
 		pos_sp_triplet->previous.valid = false;
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		pos_sp_triplet->next.valid = false;
 
 		/* reuse setpoint for LOITER only if it's not IDLE */
@@ -542,7 +540,7 @@ Mission::set_mission_items()
 	/*********************************** handle mission item *********************************************/
 
 	/* handle position mission items */
-	if (item_contains_position(&_mission_item)) {
+	if (item_contains_position(_mission_item)) {
 
 		/* force vtol land */
 		if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
@@ -766,7 +764,7 @@ Mission::set_mission_items()
 			set_align_mission_item(&_mission_item, &mission_item_next_position);
 
 			/* set position setpoint to target during the transition */
-			mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
+			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
 		}
 
 		/* yaw is aligned now */
@@ -808,10 +806,10 @@ Mission::set_mission_items()
 	/*********************************** set setpoints and check next *********************************************/
 
 	/* set current position setpoint from mission item (is protected against non-position items) */
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 	/* issue command if ready (will do nothing for position mission items) */
-	issue_command(&_mission_item);
+	issue_command(_mission_item);
 
 	/* set current work item type */
 	_work_item_type = new_work_item_type;
@@ -834,7 +832,7 @@ Mission::set_mission_items()
 		/* try to process next mission item */
 		if (has_next_position_item) {
 			/* got next mission item, update setpoint triplet */
-			mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
+			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
 
 		} else {
 			/* next mission item is not available */
@@ -1025,10 +1023,10 @@ Mission::heading_sp_update()
 			point_to_latlon[1] = _navigator->get_home_position()->lon;
 
 		} else if (_param_yawmode.get() == MISSION_YAWMODE_TO_ROI
-			   && _navigator->get_vroi()->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
+			   && _navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION) {
 			/* target location is ROI */
-			point_to_latlon[0] = _navigator->get_vroi()->lat;
+			point_to_latlon[0] = _navigator->get_vroi().lat;
-			point_to_latlon[1] = _navigator->get_vroi()->lon;
+			point_to_latlon[1] = _navigator->get_vroi().lon;
 
 		} else {
 			/* target location is next (current) waypoint */
@@ -1185,7 +1183,7 @@ Mission::do_abort_landing()
 	_mission_item.origin = ORIGIN_ONBOARD;
 
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	_navigator->set_position_setpoint_triplet_updated();
 
 	mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.",
@@ -1204,8 +1202,7 @@ Mission::do_abort_landing()
 
 	// send reposition cmd to get out of mission
 	vehicle_command_s vcmd = {};
-	mission_item_to_vehicle_command(&_mission_item, &vcmd);
+
-	vcmd.timestamp = hrt_absolute_time();
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
 	vcmd.param1 = -1;
 	vcmd.param2 = 1;
@@ -1213,7 +1210,7 @@ Mission::do_abort_landing()
 	vcmd.param6 = _mission_item.lon;
 	vcmd.param7 = alt_sp;
 
-	_navigator->publish_vehicle_cmd(vcmd);
+	_navigator->publish_vehicle_cmd(&vcmd);
 }
 
 bool
@@ -1230,7 +1227,7 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
 		/* trying to find next position mission item */
 		while (read_mission_item(onboard, offset, next_position_mission_item)) {
 
-			if (item_contains_position(next_position_mission_item)) {
+			if (item_contains_position(*next_position_mission_item)) {
 				*has_next_position_item = true;
 				break;
 			}

src/modules/navigator/mission_block.cpp

@@ -54,8 +54,6 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
-
-
 MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
 	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
@@ -97,7 +95,6 @@ MissionBlock::is_mission_item_reached()
 	case NAV_CMD_DO_MOUNT_CONFIGURE:
 	case NAV_CMD_DO_MOUNT_CONTROL:
 	case NAV_CMD_DO_SET_ROI:
-	case NAV_CMD_ROI:
 	case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
 	case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 	case NAV_CMD_SET_CAMERA_MODE:
@@ -413,61 +410,24 @@ MissionBlock::reset_mission_item_reached()
 }
 
 void
-MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd)
+MissionBlock::issue_command(const mission_item_s &item)
 {
-	// we're expecting a mission command item here so assign the "raw" inputs to the command
-	// (MAV_FRAME_MISSION mission item)
-	cmd->param1 = item->params[0];
-	cmd->param2 = item->params[1];
-	cmd->param3 = item->params[2];
-	cmd->param4 = item->params[3];
-	cmd->param5 = item->params[4];
-	cmd->param6 = item->params[5];
-	cmd->param7 = item->params[6];
-	cmd->command = item->nav_cmd;
-
-	cmd->target_system = _navigator->get_vstatus()->system_id;
-
-	// The camera commands are not processed on the autopilot but will be
-	// sent to the mavlink links to other components.
-	switch (item->nav_cmd) {
-	case NAV_CMD_IMAGE_START_CAPTURE:
-	case NAV_CMD_IMAGE_STOP_CAPTURE:
-	case NAV_CMD_VIDEO_START_CAPTURE:
-	case NAV_CMD_VIDEO_STOP_CAPTURE:
-		cmd->target_component = 100; // MAV_COMP_ID_CAMERA
-		break;
-
-	default:
-		cmd->target_component = _navigator->get_vstatus()->component_id;
-		break;
-	}
-
-	cmd->source_system = _navigator->get_vstatus()->system_id;
-	cmd->source_component = _navigator->get_vstatus()->component_id;
-	cmd->confirmation = false;
-}
-
-void
-MissionBlock::issue_command(const struct mission_item_s *item)
-{
-
 	if (item_contains_position(item)) {
 		return;
 	}
 
 	// NAV_CMD_DO_LAND_START is only a marker
-	if (item->nav_cmd == NAV_CMD_DO_LAND_START) {
+	if (item.nav_cmd == NAV_CMD_DO_LAND_START) {
 		return;
 	}
 
-	if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
+	if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
 		PX4_INFO("do_set_servo command");
 		// XXX: we should issue a vehicle command and handle this somewhere else
 		_actuators = {};
 		// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
 		// params[1] new value for selected actuator in ms 900...2000
-		_actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
+		_actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1];
 		_actuators.timestamp = hrt_absolute_time();
 
 		if (_actuator_pub != nullptr) {
@@ -478,16 +438,23 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 		}
 
 	} else {
-		const hrt_abstime now = hrt_absolute_time();
+		_action_start = hrt_absolute_time();
 
-		struct vehicle_command_s cmd = {
+		// mission_item -> vehicle_command
-			.timestamp = now
-		};
 
-		mission_item_to_vehicle_command(item, &cmd);
+		// we're expecting a mission command item here so assign the "raw" inputs to the command
-		_action_start = now;
+		// (MAV_FRAME_MISSION mission item)
+		vehicle_command_s vcmd = {};
+		vcmd.command = item.nav_cmd;
+		vcmd.param1 = item.params[0];
+		vcmd.param2 = item.params[1];
+		vcmd.param3 = item.params[2];
+		vcmd.param4 = item.params[3];
+		vcmd.param5 = item.params[4];
+		vcmd.param6 = item.params[5];
+		vcmd.param7 = item.params[6];
 
-		_navigator->publish_vehicle_cmd(cmd);
+		_navigator->publish_vehicle_cmd(&vcmd);
 	}
 }
 
@@ -502,41 +469,41 @@ MissionBlock::get_time_inside(const struct mission_item_s &item)
 }
 
 bool
-MissionBlock::item_contains_position(const struct mission_item_s *item)
+MissionBlock::item_contains_position(const mission_item_s &item)
 {
-	return item->nav_cmd == NAV_CMD_WAYPOINT ||
+	return item.nav_cmd == NAV_CMD_WAYPOINT ||
-	       item->nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
+	       item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
-	       item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	       item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-	       item->nav_cmd == NAV_CMD_LAND ||
+	       item.nav_cmd == NAV_CMD_LAND ||
-	       item->nav_cmd == NAV_CMD_TAKEOFF ||
+	       item.nav_cmd == NAV_CMD_TAKEOFF ||
-	       item->nav_cmd == NAV_CMD_LOITER_TO_ALT ||
+	       item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
-	       item->nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
+	       item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
-	       item->nav_cmd == NAV_CMD_VTOL_LAND;
+	       item.nav_cmd == NAV_CMD_VTOL_LAND;
 }
 
 bool
-MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
+MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
 {
 	/* don't change the setpoint for non-position items */
 	if (!item_contains_position(item)) {
 		return false;
 	}
 
-	sp->lat = item->lat;
+	sp->lat = item.lat;
-	sp->lon = item->lon;
+	sp->lon = item.lon;
-	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
+	sp->alt = item.altitude_is_relative ? item.altitude + _navigator->get_home_position()->alt : item.altitude;
-	sp->yaw = item->yaw;
+	sp->yaw = item.yaw;
-	sp->yaw_valid = PX4_ISFINITE(item->yaw);
+	sp->yaw_valid = PX4_ISFINITE(item.yaw);
-	sp->loiter_radius = (fabsf(item->loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item->loiter_radius) :
+	sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
 			    _navigator->get_loiter_radius();
-	sp->loiter_direction = (item->loiter_radius > 0) ? 1 : -1;
+	sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
-	sp->acceptance_radius = item->acceptance_radius;
+	sp->acceptance_radius = item.acceptance_radius;
-	sp->disable_mc_yaw_control = item->disable_mc_yaw;
+	sp->disable_mc_yaw_control = item.disable_mc_yaw;
 
 	sp->cruising_speed = _navigator->get_cruising_speed();
 	sp->cruising_throttle = _navigator->get_cruising_throttle();
 
-	switch (item->nav_cmd) {
+	switch (item.nav_cmd) {
 	case NAV_CMD_IDLE:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
 		break;
@@ -553,7 +520,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 			sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
 
 			// set pitch and ensure that the hold time is zero
-			sp->pitch_min = item->pitch_min;
+			sp->pitch_min = item.pitch_min;
 		}
 
 		break;
@@ -720,19 +687,10 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 	    !_navigator->get_vstatus()->is_rotary_wing &&
 	    _param_force_vtol.get() == 1) {
 
-		struct vehicle_command_s cmd = {
+		vehicle_command_s cmd = {};
-			.timestamp = hrt_absolute_time(),
+		cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
-			.param5 = 0.0f,
+		cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
-			.param6 = 0.0f,
+		_navigator->publish_vehicle_cmd(&cmd);
-			.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC,
-			.param2 = 0.0f,
-			.param3 = 0.0f,
-			.param4 = 0.0f,
-			.param7 = 0.0f,
-			.command = NAV_CMD_DO_VTOL_TRANSITION
-		};
-
-		_navigator->publish_vehicle_cmd(cmd);
 	}
 
 	/* set the land item */

src/modules/navigator/mission_block.h

@@ -66,7 +66,7 @@ public:
 	MissionBlock(const MissionBlock &) = delete;
 	MissionBlock &operator=(const MissionBlock &) = delete;
 
-	static bool item_contains_position(const struct mission_item_s *item);
+	static bool item_contains_position(const mission_item_s &item);
 
 protected:
 	/**
@@ -86,7 +86,7 @@ protected:
 	 * @param the mission item to convert
 	 * @param the position setpoint that needs to be set
 	 */
-	bool mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
+	bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
 
 	/**
 	 * Set previous position setpoint to current setpoint
@@ -120,12 +120,7 @@ protected:
 	 */
 	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
 
-	/**
+	void issue_command(const mission_item_s &item);
-	 * Convert a mission item to a command
-	 */
-	void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
-
-	void issue_command(const struct mission_item_s *item);
 
 	float get_time_inside(const struct mission_item_s &item);
 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -184,7 +184,7 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
 			// Geofence function checks against home altitude amsl
 			missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
 
-			if (MissionBlock::item_contains_position(&missionitem) &&
+			if (MissionBlock::item_contains_position(missionitem) &&
 			    !geofence.check(missionitem)) {
 
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %d", i + 1);
@@ -212,7 +212,7 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
 		}
 
 		/* reject relative alt without home set */
-		if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(&missionitem)) {
+		if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(missionitem)) {
 
 			warning_issued = true;
 
@@ -229,7 +229,7 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
 		/* calculate the global waypoint altitude */
 		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
 
-		if ((home_alt > wp_alt) && MissionBlock::item_contains_position(&missionitem)) {
+		if ((home_alt > wp_alt) && MissionBlock::item_contains_position(missionitem)) {
 
 			warning_issued = true;
 
@@ -286,7 +286,6 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
 		    missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
 		    missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
-		    missionitem.nav_cmd != NAV_CMD_ROI &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
@@ -411,7 +410,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM
 					return false;
 				}
 
-				if (MissionBlock::item_contains_position(&missionitem_previous)) {
+				if (MissionBlock::item_contains_position(missionitem_previous)) {
 					float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat,
 							    missionitem.lon);
 
@@ -489,7 +488,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 		/* find first waypoint (with lat/lon) item in datamanager */
 		for (size_t i = 0; i < nMissionItems; i++) {
 			if (dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
-				if (MissionBlock::item_contains_position(&mission_item)) {
+				if (MissionBlock::item_contains_position(mission_item)) {
 					/* check only items with valid lat/lon */
 
 					/* check distance from current position to item */

src/modules/navigator/navigation.h

@@ -66,7 +66,6 @@ enum NAV_CMD {
 	NAV_CMD_TAKEOFF = 22,
 	NAV_CMD_LOITER_TO_ALT = 31,
 	NAV_CMD_DO_FOLLOW_REPOSITION = 33,
-	NAV_CMD_ROI = 80,
 	NAV_CMD_VTOL_TAKEOFF = 84,
 	NAV_CMD_VTOL_LAND = 85,
 	NAV_CMD_DELAY = 93,

src/modules/navigator/navigator.h

@@ -106,13 +106,7 @@ public:
 	 */
 	void		publish_geofence_result();
 
-	/**
+	void		publish_vehicle_cmd(vehicle_command_s *vcmd);
-	 * Publish the attitude sp, only to be used in very special modes when position control is deactivated
-	 * Example: mode that is triggered on gps failure
-	 */
-	void		publish_att_sp();
-
-	void		publish_vehicle_cmd(const struct vehicle_command_s &vcmd);
 
 	/**
 	 * Setters
@@ -131,17 +125,19 @@ public:
 	struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
 	struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
 	struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
-	struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
 	struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
 	struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
 	struct vehicle_status_s *get_vstatus() { return &_vstatus; }
-	struct vehicle_roi_s *get_vroi() { return &_vroi; }
+
+	const vehicle_roi_s &get_vroi() { return _vroi; }
 
 	bool home_position_valid() { return (_home_pos.timestamp > 0); }
 
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+
 	Geofence	&get_geofence() { return _geofence; }
+
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
 
@@ -215,6 +211,7 @@ public:
 	 * @return the distance at which the next waypoint should be used
 	 */
 	float		get_acceptance_radius(float mission_item_radius);
+
 	orb_advert_t	*get_mavlink_log_pub() { return &_mavlink_log_pub; }
 
 	void		increment_mission_instance_count() { _mission_instance_count++; }
@@ -230,8 +227,6 @@ private:
 	bool		_task_should_exit{false};	/**< if true, sensor task should exit */
 	int		_navigator_task{-1};		/**< task handle for sensor task */
 
-	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */
-
 	int		_fw_pos_ctrl_status_sub{-1};	/**< notification of vehicle capabilities updates */
 	int		_global_pos_sub{-1};		/**< global position subscription */
 	int		_gps_pos_sub{-1};		/**< gps position subscription */
@@ -244,12 +239,14 @@ private:
 	int		_sensor_combined_sub{-1};	/**< sensor combined subscription */
 	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
 	int		_vstatus_sub{-1};		/**< vehicle status subscription */
-	int		_vehicle_roi_sub{-1};		/**< vehicle ROI subscription */
 
 	orb_advert_t	_geofence_result_pub{nullptr};
+	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */
 	orb_advert_t	_mission_result_pub{nullptr};
 	orb_advert_t	_pos_sp_triplet_pub{nullptr};
+	orb_advert_t	_vehicle_cmd_ack_pub{nullptr};
 	orb_advert_t	_vehicle_cmd_pub{nullptr};
+	orb_advert_t	_vehicle_roi_pub{nullptr};
 
 	fw_pos_ctrl_status_s				_fw_pos_ctrl_status{};	/**< fixed wing navigation capabilities */
 	geofence_result_s				_geofence_result{};
@@ -264,8 +261,8 @@ private:
 	vehicle_gps_position_s				_gps_pos{};		/**< gps position */
 	vehicle_land_detected_s				_land_detected{};	/**< vehicle land_detected */
 	vehicle_local_position_s			_local_pos{};		/**< local vehicle position */
-	vehicle_status_s				_vstatus{};		/**< vehicle status */
 	vehicle_roi_s					_vroi{};		/**< vehicle ROI */
+	vehicle_status_s				_vstatus{};		/**< vehicle status */
 
 	int		_mission_instance_count{-1};	/**< instance count for the current mission */
 
@@ -313,7 +310,6 @@ private:
 	void		sensor_combined_update();
 	void		vehicle_land_detected_update();
 	void		vehicle_status_update();
-	void		vehicle_roi_update();
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -339,5 +335,7 @@ private:
 	 * Publish the mission result so commander and mavlink know what is going on
 	 */
 	void		publish_mission_result();
+
+	void		publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
 };
 #endif

src/modules/navigator/navigator_main.cpp

@@ -64,6 +64,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/uORB.h>
 
@@ -213,12 +214,6 @@ Navigator::params_update()
 	}
 }
 
-void
-Navigator::vehicle_roi_update()
-{
-	orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &_vroi);
-}
-
 void
 Navigator::task_main_trampoline(int argc, char *argv[])
 {
@@ -252,7 +247,6 @@ Navigator::task_main()
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
-	_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -264,7 +258,6 @@ Navigator::task_main()
 	home_position_update(true);
 	fw_pos_ctrl_status_update(true);
 	params_update();
-	vehicle_roi_update();
 
 	/* wakeup source(s) */
 	px4_pollfd_struct_t fds[1] = {};
@@ -365,13 +358,6 @@ Navigator::task_main()
 			home_position_update();
 		}
 
-		/* ROI updated */
-		orb_check(_vehicle_roi_sub, &updated);
-
-		if (updated) {
-			vehicle_roi_update();
-		}
-
 		/* vehicle_command updated */
 		orb_check(_vehicle_command_sub, &updated);
 
@@ -379,7 +365,13 @@ Navigator::task_main()
 			vehicle_command_s cmd;
 			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
 
-			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
+			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
+
+				// DO_GO_AROUND is currently handled by the position controller (unacknowledged)
+				// TODO: move DO_GO_AROUND handling to navigator
+				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
 
 				struct position_setpoint_triplet_s *rep = get_reposition_triplet();
 				struct position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
@@ -435,6 +427,8 @@ Navigator::task_main()
 				rep->current.valid = true;
 				rep->next.valid = false;
 
+				// CMD_DO_REPOSITION is acknowledged by commander
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
 				struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
 
@@ -472,6 +466,8 @@ Navigator::task_main()
 				rep->current.valid = true;
 				rep->next.valid = false;
 
+				// CMD_NAV_TAKEOFF is acknowledged by commander
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {
 
 				/* find NAV_CMD_DO_LAND_START in the mission and
@@ -480,30 +476,17 @@ Navigator::task_main()
 				int land_start = _mission.find_offboard_land_start();
 
 				if (land_start != -1) {
-					struct vehicle_command_s vcmd = {
+					vehicle_command_s vcmd = {};
-						.timestamp = hrt_absolute_time(),
+					vcmd.param1 = land_start;
-						.param5 = 0.0f,
+					publish_vehicle_cmd(&vcmd);
-						.param6 = 0.0f,
-						.param1 = (float)land_start,
-						.param2 = 0.0f,
-						.param3 = 0.0f,
-						.param4 = 0.0f,
-						.param7 = 0.0f,
-						.command = vehicle_command_s::VEHICLE_CMD_MISSION_START,
-						.target_system = (uint8_t)get_vstatus()->system_id,
-						.target_component = (uint8_t)get_vstatus()->component_id,
-						.source_system = (uint8_t)get_vstatus()->system_id,
-						.source_component = (uint8_t)get_vstatus()->component_id
-					};
-
-					publish_vehicle_cmd(vcmd);
 
 				} else {
 					PX4_WARN("planned landing not available");
 				}
 
-			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
+				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
 				if (get_mission_result()->valid &&
 				    PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
 
@@ -526,6 +509,51 @@ Navigator::task_main()
 						set_cruising_throttle();
 					}
 				}
+
+				// TODO: handle responses for supported DO_CHANGE_SPEED options?
+				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI) {
+				_vroi = {};
+				_vroi.mode = cmd.param1;
+
+				switch (_vroi.mode) {
+				case vehicle_command_s::VEHICLE_ROI_NONE:
+					break;
+
+				case vehicle_command_s::VEHICLE_ROI_WPNEXT:
+					// TODO: implement point toward next MISSION
+					break;
+
+				case vehicle_command_s::VEHICLE_ROI_WPINDEX:
+					_vroi.mission_seq = cmd.param2;
+					break;
+
+				case vehicle_command_s::VEHICLE_ROI_LOCATION:
+					_vroi.lat = cmd.param5;
+					_vroi.lon = cmd.param6;
+					_vroi.alt = cmd.param7;
+					break;
+
+				case vehicle_command_s::VEHICLE_ROI_TARGET:
+					_vroi.target_seq = cmd.param2;
+					break;
+
+				default:
+					_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
+					break;
+				}
+
+				_vroi.timestamp = hrt_absolute_time();
+
+				if (_vehicle_roi_pub != nullptr) {
+					orb_publish(ORB_ID(vehicle_roi), _vehicle_roi_pub, &_vroi);
+
+				} else {
+					_vehicle_roi_pub = orb_advertise(ORB_ID(vehicle_roi), &_vroi);
+				}
+
+				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 			}
 		}
 
@@ -979,17 +1007,6 @@ Navigator::publish_geofence_result()
 	}
 }
 
-void
-Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
-{
-	if (_vehicle_cmd_pub != nullptr) {
-		orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vcmd);
-
-	} else {
-		_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
-	}
-}
-
 void
 Navigator::set_mission_failure(const char *reason)
 {
@@ -999,3 +1016,60 @@ Navigator::set_mission_failure(const char *reason)
 		mavlink_log_critical(&_mavlink_log_pub, "%s", reason);
 	}
 }
+
+void
+Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
+{
+	vcmd->timestamp = hrt_absolute_time();
+	vcmd->source_system = _vstatus.system_id;
+	vcmd->source_component = _vstatus.component_id;
+	vcmd->target_system = _vstatus.system_id;
+	vcmd->confirmation = false;
+	vcmd->from_external = false;
+
+	// The camera commands are not processed on the autopilot but will be
+	// sent to the mavlink links to other components.
+	switch (vcmd->command) {
+	case NAV_CMD_IMAGE_START_CAPTURE:
+	case NAV_CMD_IMAGE_STOP_CAPTURE:
+	case NAV_CMD_VIDEO_START_CAPTURE:
+	case NAV_CMD_VIDEO_STOP_CAPTURE:
+		vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
+		break;
+
+	default:
+		vcmd->target_component = _vstatus.component_id;
+		break;
+	}
+
+	if (_vehicle_cmd_pub != nullptr) {
+		orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd);
+
+	} else {
+		_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+	}
+}
+
+void
+Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
+{
+	vehicle_command_ack_s command_ack = {};
+
+	command_ack.timestamp = hrt_absolute_time();
+	command_ack.command = cmd.command;
+	command_ack.target_system = cmd.source_system;
+	command_ack.target_component = cmd.source_component;
+	command_ack.from_external = cmd.from_external;
+
+	command_ack.result = result;
+	command_ack.result_param1 = 0;
+	command_ack.result_param2 = 0;
+
+	if (_vehicle_cmd_ack_pub != nullptr) {
+		orb_publish(ORB_ID(vehicle_command_ack), _vehicle_cmd_ack_pub, &command_ack);
+
+	} else {
+		_vehicle_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
+				       vehicle_command_ack_s::ORB_QUEUE_LENGTH);
+	}
+}

src/modules/navigator/rcloss.cpp

@@ -140,7 +140,7 @@ RCLoss::set_rcl_item()
 	reset_mission_item_reached();
 
 	/* convert mission item to current position setpoint and make it valid */
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/rtl.cpp

@@ -84,7 +84,7 @@ RTL::on_activation()
 {
 	set_current_position_item(&_mission_item);
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->next.valid = false;
 
@@ -286,12 +286,12 @@ RTL::set_rtl_item()
 	reset_mission_item_reached();
 
 	/* execute command if set. This is required for commands like VTOL transition */
-	if (!item_contains_position(&_mission_item)) {
+	if (!item_contains_position(_mission_item)) {
-		issue_command(&_mission_item);
+		issue_command(_mission_item);
 	}
 
 	/* convert mission item to current position setpoint and make it valid */
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/takeoff.cpp

@@ -92,7 +92,7 @@ Takeoff::on_active()
 		// set loiter item so position controllers stop doing takeoff logic
 		set_loiter_item(&_mission_item);
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 }
@@ -148,7 +148,7 @@ Takeoff::set_takeoff_position()
 
 	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;