feat(mavlink): MAV_CMD_DO_SET_MISSION_CURRENT support (#28105)

msg/versioned/VehicleCommand.msg

@@ -66,6 +66,7 @@ uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_I
 uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)|q2 - quaternion param #2, x (0 in null-rotation)|q3 - quaternion param #3, y (0 in null-rotation)|q4 - quaternion param #4, z (0 in null-rotation)|Unused|Unused|Unused|
 uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221 # Set id of master controller. |System ID|Component ID|Unused|Unused|Unused|Unused|Unused|
 uint16 VEHICLE_CMD_DO_GUIDED_LIMITS=222 # Set limits for external control. |[s] Timeout - maximum time that external controller will be allowed to control vehicle. 0 means no timeout|[m] Absolute altitude min(AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit|[m] Absolute altitude max - if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit|[m] Horizontal move limit (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|Unused|Unused|Unused|
+uint16 VEHICLE_CMD_DO_SET_MISSION_CURRENT = 224 # Set the mission item with sequence number seq as current item and emit MISSION_CURRENT (whether or not the mission mode is active). ACKs FAILED if seq is out of range or there is no current mission item. |Mission sequence value to set, -1 for the current mission item (use to reset jump counters without changing the current item)|Reset repeat/jump counters and clear mission complete flag (1=true,0=false; default:0)|Unused|Unused|Unused|Unused|Unused|
 uint16 VEHICLE_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration. |Unused|Unused|Unused|Unused|Unused|Unused|Unused|
 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.
 uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3# Param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration.

src/modules/commander/Commander.cpp

@@ -1697,6 +1697,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
 	case vehicle_command_s::VEHICLE_CMD_DO_LAND_START:
 	case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_MISSION_CURRENT:
 	case vehicle_command_s::VEHICLE_CMD_LOGGING_START:
 	case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP:
 	case vehicle_command_s::VEHICLE_CMD_NAV_DELAY:

src/modules/mavlink/mavlink_command_params.hpp

@@ -106,6 +106,7 @@ static constexpr Entry SupportedCommandParams[] = {
 	{  211, 0x03, 0x03 }, // DO_GRIPPER:                  p1:id,p2:action
 	{  212, 0x03, 0x03 }, // DO_AUTOTUNE_ENABLE:          p1:enable,p2:axis
 	{  214, 0x07, 0x07 }, // DO_SET_CAM_TRIGG_INTERVAL:  p1:cycle,p2:shutter,p3:camera_id
+	{  224, 0x00, 0x03 }, // DO_SET_MISSION_CURRENT:      cmd:p1:seq,p2:reset_jump_counters
 	{  400, 0x03, 0x03 }, // COMPONENT_ARM_DISARM:        p1:arm,p2:force
 	{  420, 0x07, 0x07 }, // INJECT_FAILURE:              p1:unit,p2:type,p3:instance
 	{  530, 0x03, 0x03 }, // SET_CAMERA_MODE:             p1:camera_id,p2:mode

src/modules/navigator/mission.cpp

@@ -93,40 +93,62 @@ Mission::on_activation()
 
 
 bool
-Mission::set_current_mission_index(uint16_t index)
+Mission::set_current_mission_index(int32_t index, bool reset_jump_counters)
 {
-	if (index == _mission.current_seq) {
-		return true;
+	if (!_navigator->get_mission_result()->valid || (_mission.count == 0)) {
+		return false;
 	}
 
-	if (_navigator->get_mission_result()->valid && (index < _mission.count)) {
-		if (goToItem(index, MissionTraversalType::FollowMissionControlFlow) != PX4_OK) {
-			// Keep the old mission index (it was not updated by the interface) and report back.
-			return false;
+	if ((index != -1) && ((index < 0) || (index >= _mission.count))) {
+		return false;
+	}
+
+	if ((index == -1) || (index == _mission.current_seq)) {
+		// Keep the current mission item unchanged.
+		if (reset_jump_counters) {
+			resetMissionJumpCounter();
+
+			// A reset can bring a finished mission back to a resumable state (mirrors
+			// checkMissionRestart()'s use of the same assignment).
+			_is_current_planned_mission_item_valid = isMissionValid();
+
+			if (isActive()) {
+				update_mission();
+				set_mission_items();
+			}
 		}
 
-		_is_current_planned_mission_item_valid = true;
-
-		// we start from the first item so can reset the cache
-		if (_mission.current_seq == 0) {
-			resetItemCache();
-		}
-
-		// update mission items if already in active mission
-		if (isActive()) {
-			// prevent following "previous - current" line
-			_navigator->reset_triplets();
-			update_mission();
-			set_mission_items();
-		}
-
-		// User has actively set new index, reset.
-		_inactivation_index = -1;
-
 		return true;
 	}
 
-	return false;
+	if (goToItem(index, MissionTraversalType::FollowMissionControlFlow) != PX4_OK) {
+		// Keep the old mission index (it was not updated by the interface) and report back.
+		return false;
+	}
+
+	if (reset_jump_counters) {
+		resetMissionJumpCounter();
+	}
+
+	_is_current_planned_mission_item_valid = true;
+
+	// we start from the first item so can reset the cache
+	if (_mission.current_seq == 0) {
+		resetItemCache();
+	}
+
+	// update mission items if already in active mission
+	if (isActive()) {
+		// prevent following "previous - current" line
+		_navigator->reset_triplets();
+		update_mission();
+		set_mission_items();
+	}
+
+	// User has actively set new index, reset.
+	_inactivation_index = -1;
+
+	return true;
 }
 
 bool Mission::setNextMissionItem()

src/modules/navigator/mission.h

@@ -62,7 +62,16 @@ public:
 	virtual void on_inactive() override;
 	virtual void on_activation() override;
 
-	bool set_current_mission_index(uint16_t index);
+	/**
+	 * Set the current mission item.
+	 *
+	 * @param index Mission sequence to set as current, or -1 to keep the current item unchanged
+	 *              (e.g. to reset jump counters in place).
+	 * @param reset_jump_counters Reset all DO_JUMP repeat counters and mission completion flag
+	 * @return true if the current mission item was set successfully (or the reset-only, unchanged-index
+	 *         request could be honored), false if index is out of range or there is no current mission item.
+	 */
+	bool set_current_mission_index(int32_t index, bool reset_jump_counters = false);
 
 	uint16_t get_land_start_index() const { return _mission.land_start_index; }
 	bool get_land_start_available() const { return hasMissionLandStart(); }

src/modules/navigator/navigator_main.cpp

@@ -831,6 +831,28 @@ void Navigator::run()
 
 				publish_vehicle_command_ack(cmd, result);
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_MISSION_CURRENT) {
+				uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
+
+				// param2 is a MAV_BOOL: only 0 or 1 are valid values.
+				const bool param2_valid = PX4_ISFINITE(cmd.param2)
+							  && ((fabsf(cmd.param2) < FLT_EPSILON) || (fabsf(cmd.param2 - 1.f) < FLT_EPSILON));
+
+				// -1 is a valid param1: keep the current mission item unchanged (e.g. to only reset jump counters).
+				if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && param2_valid) {
+					const bool reset_jump_counters = cmd.param2 > 0.5f;
+
+					if (_mission.set_current_mission_index(static_cast<int32_t>(cmd.param1), reset_jump_counters)) {
+						result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+					} else {
+						// Sequence number out of range, or no mission / no current mission item.
+						result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+					}
+				}
+
+				publish_vehicle_command_ack(cmd, result);
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
 				if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) {
 					if (!_mission.set_current_mission_index(cmd.param1)) {