Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: Add explicit handling of DO_JUMP_TAG

Browse Source
This commit is contained in:
Nick Exton 2023-09-28 11:09:18 +10:00 committed by Andrew Tridgell
parent 12642b5793
commit 0567d8576e
2 changed files with 26 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -639,6 +639,7 @@ protected:
virtual MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg); virtual MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg);
virtual MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet); virtual MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_jump_tag(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_battery_reset(const mavlink_command_int_t &packet); MAV_RESULT handle_command_battery_reset(const mavlink_command_int_t &packet);
void handle_command_long(const mavlink_message_t &msg); void handle_command_long(const mavlink_message_t &msg);

28
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -4452,8 +4452,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_run_prearm_checks(const mavlink_command_i
// the current waypoint, rather than this DO command. It is hoped we // the current waypoint, rather than this DO command. It is hoped we
// can move to this command in the future to avoid acknowledgement // can move to this command in the future to avoid acknowledgement
// issues with MISSION_SET_CURRENT // issues with MISSION_SET_CURRENT
// This also handles DO_JUMP_TAG by converting the tag to an index
// and setting the index as if it was a normal do_set_mission_current
MAV_RESULT GCS_MAVLINK::handle_command_do_set_mission_current(const mavlink_command_long_t &packet) MAV_RESULT GCS_MAVLINK::handle_command_do_set_mission_current(const mavlink_command_long_t &packet)
{ {
AP_Mission *mission = AP::mission(); AP_Mission *mission = AP::mission();
@ -4462,11 +4460,28 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_mission_current(const mavlink_comm
} }
const uint32_t seq = (uint32_t)packet.param1; const uint32_t seq = (uint32_t)packet.param1;
if (packet.command == MAV_CMD_DO_JUMP_TAG) { if (!mission->set_current_cmd(seq)) {
if (!mission->jump_to_tag(seq)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
} else if (!mission->set_current_cmd(seq)) {
// volunteer the new current waypoint for all listeners
send_message(MSG_CURRENT_WAYPOINT);
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_do_jump_tag(const mavlink_command_long_t &packet)
{
AP_Mission *mission = AP::mission();
if (mission == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
const uint32_t tag = (uint32_t)packet.param1;
if (tag > UINT16_MAX) {
return MAV_RESULT_DENIED;
}
if (!mission->jump_to_tag(tag)) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
@ -4735,6 +4750,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
#endif #endif
case MAV_CMD_DO_JUMP_TAG: case MAV_CMD_DO_JUMP_TAG:
result = handle_command_do_jump_tag(packet);
break;
case MAV_CMD_DO_SET_MISSION_CURRENT: case MAV_CMD_DO_SET_MISSION_CURRENT:
result = handle_command_do_set_mission_current(packet); result = handle_command_do_set_mission_current(packet);
break; break;