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

GCS_MAVLink: handle MAV_CMD_DO_SET_MISSION_CURRENT

Browse Source
This commit is contained in:
Peter Barker 2021-02-18 21:32:08 +11:00 committed by Peter Barker
parent f0ffaa5624
commit e3de88d1d4
2 changed files with 42 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/GCS_MAVLink/GCS.h
View File

@ -389,6 +389,12 @@ protected:
void handle_mission_request(const mavlink_message_t &msg) const;
void handle_mission_request_int(const mavlink_message_t &msg) const;
void handle_mission_clear_all(const mavlink_message_t &msg) const;
// Note that there exists a relatively new mavlink DO command,
// MAV_CMD_DO_SET_MISSION_CURRENT which provides an acknowledgement
// that the command has been received, rather than the GCS having to
// rely on getting back an identical sequence number as some currently
// do.
virtual void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg);
void handle_mission_count(const mavlink_message_t &msg);
void handle_mission_write_partial_list(const mavlink_message_t &msg);
@ -468,6 +474,8 @@ protected:
MAV_RESULT handle_command_preflight_can(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_battery_reset(const mavlink_command_long_t &packet);
void handle_command_long(const mavlink_message_t &msg);
MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet);

34
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -516,6 +516,12 @@ void GCS_MAVLINK::handle_mission_request(const mavlink_message_t &msg) const
/*
handle a MISSION_SET_CURRENT mavlink packet
Note that there exists a relatively new mavlink DO command,
MAV_CMD_DO_SET_MISSION_CURRENT which provides an acknowledgement
that the command has been received, rather than the GCS having to
rely on getting back an identical sequence number as some currently
do.
*/
void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg)
{
@ -3788,6 +3794,29 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_can(const mavlink_command_long_
#endif
}
// changes the current waypoint; at time of writing GCS
// implementations use the mavlink message MISSION_SET_CURRENT to set
// the current waypoint, rather than this DO command. It is hoped we
// can move to this command in the future to avoid acknowledgement
// issues with MISSION_SET_CURRENT
MAV_RESULT GCS_MAVLINK::handle_command_do_set_mission_current(const mavlink_command_long_t &packet)
{
AP_Mission *mission = AP::mission();
if (mission == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
const uint32_t seq = (uint32_t)packet.param1;
if (!mission->set_current_cmd(seq)) {
return MAV_RESULT_FAILED;
}
// volunteer the new current waypoint for all listeners
send_message(MSG_CURRENT_WAYPOINT);
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_battery_reset(const mavlink_command_long_t &packet)
{
const uint16_t battery_mask = packet.param1;
@ -4059,6 +4088,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_command_preflight_calibration(packet);
break;
case MAV_CMD_DO_SET_MISSION_CURRENT:
result = handle_command_do_set_mission_current(packet);
break;
case MAV_CMD_BATTERY_RESET:
result = handle_command_battery_reset(packet);
break;