diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 1694927ea16..44964eb88df 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/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. diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 2a36b543daf..d61f949630c 100644 --- a/src/modules/commander/Commander.cpp +++ b/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: diff --git a/src/modules/mavlink/mavlink_command_params.hpp b/src/modules/mavlink/mavlink_command_params.hpp index aa21c39e261..1271c5f58ff 100644 --- a/src/modules/mavlink/mavlink_command_params.hpp +++ b/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 diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7462117d981..b6560bbe08c 100644 --- a/src/modules/navigator/mission.cpp +++ b/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() diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 8a24ac4dc0d..da159102a37 100644 --- a/src/modules/navigator/mission.h +++ b/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(); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 42098e909c7..47a58d90843 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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(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)) {