aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: Handle param2 (reset) in DO_SET_MISSION_CURRENT cmd

Browse Source
This commit is contained in:
Nick Exton 2023-09-28 11:12:29 +10:00 committed by Andrew Tridgell
parent a29cd0321b
commit 59c5092810
1 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -4460,9 +4460,24 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_mission_current(const mavlink_comm
}
const uint32_t seq = (uint32_t)packet.param1;
if (!mission->is_valid_index(seq)) {
return MAV_RESULT_DENIED;
}
// From https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_MISSION_CURRENT:
// Param 2: Reset Mission
// - Resets mission. 1: true, 0: false. Resets jump counters to initial values
// and changes mission state "completed" to be "active" or "paused".
const bool reset_and_restart = is_equal(packet.param2, 1.0f);
if (reset_and_restart) {
mission->reset();
}
if (!mission->set_current_cmd(seq)) {
return MAV_RESULT_FAILED;
}
if (reset_and_restart) {
mission->resume();
}
// volunteer the new current waypoint for all listeners
send_message(MSG_CURRENT_WAYPOINT);