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: Check for mission space before sending items

Browse Source
This commit is contained in:
Michael du Breuil 2022-08-30 16:50:29 -07:00 committed by Andrew Tridgell
parent 152ef7f78f
commit ff94ef1c60
3 changed files with 28 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/GCS_MAVLink/GCS.h
View File

@ -75,6 +75,12 @@ void gcs_out_of_space_to_send(mavlink_channel_t chan);
// scope.
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
// CHECK_PAYLOAD_SIZE2_VOID - macro which inserts code which will
// immediately return from the current (void) function if there is no
// room to fit the mavlink message with id id on the mavlink output
// channel "chan".
#define CHECK_PAYLOAD_SIZE2_VOID(chan, id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return
// convenience macros for defining which ap_message ids are in which streams:
#define MAV_STREAM_ENTRY(stream_name) \
{ \
@ -170,6 +176,7 @@ public:
void send_mission_ack(const mavlink_message_t &msg,
MAV_MISSION_TYPE mission_type,
MAV_MISSION_RESULT result) const {
CHECK_PAYLOAD_SIZE2_VOID(chan, MISSION_ACK);
mavlink_msg_mission_ack_send(chan,
msg.sysid,
msg.compid,

18
libraries/GCS_MAVLink/MissionItemProtocol.cpp
View File

@ -111,6 +111,7 @@ void MissionItemProtocol::handle_mission_request_list(
// reply with number of commands in the mission. The GCS will
// then request each command separately
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_COUNT);
mavlink_msg_mission_count_send(_link.get_chan(),
msg.sysid,
msg.compid,
@ -148,6 +149,7 @@ void MissionItemProtocol::handle_mission_request_int(const GCS_MAVLINK &_link,
return;
}
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ITEM_INT);
_link.send_message(MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char*)&ret_packet);
}
@ -186,6 +188,7 @@ void MissionItemProtocol::handle_mission_request(const GCS_MAVLINK &_link,
return;
}
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ITEM_INT);
_link.send_message(MAVLINK_MSG_ID_MISSION_ITEM, (const char*)&ret_packet);
}
@ -293,6 +296,7 @@ void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link,
const mavlink_message_t &msg,
MAV_MISSION_RESULT result) const
{
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ACK);
mavlink_msg_mission_ack_send(_link.get_chan(),
msg.sysid,
msg.compid,
@ -316,6 +320,7 @@ void MissionItemProtocol::queued_request_send()
INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
CHECK_PAYLOAD_SIZE2_VOID(link->get_chan(), MISSION_REQUEST);
mavlink_msg_mission_request_send(
link->get_chan(),
dest_sysid,
@ -340,11 +345,14 @@ void MissionItemProtocol::update()
if (tnow - timelast_receive_ms > upload_timeout_ms) {
receiving = false;
timeout();
mavlink_msg_mission_ack_send(link->get_chan(),
dest_sysid,
dest_compid,
MAV_MISSION_OPERATION_CANCELLED,
mission_type());
const mavlink_channel_t chan = link->get_chan();
if (HAVE_PAYLOAD_SPACE(chan, MISSION_ACK)) {
mavlink_msg_mission_ack_send(chan,
dest_sysid,
dest_compid,
MAV_MISSION_OPERATION_CANCELLED,
mission_type());
}
link = nullptr;
free_upload_resources();
return;

13
libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp
View File

@ -65,11 +65,14 @@ MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_l
if (packet.seq != 0 && // always allow HOME to be read
packet.seq >= mission.num_commands()) {
// try to educate the GCS on the actual size of the mission:
mavlink_msg_mission_count_send(_link.get_chan(),
msg.sysid,
msg.compid,
mission.num_commands(),
MAV_MISSION_TYPE_MISSION);
const mavlink_channel_t chan = _link.get_chan();
if (HAVE_PAYLOAD_SPACE(chan, MISSION_COUNT)) {
mavlink_msg_mission_count_send(chan,
msg.sysid,
msg.compid,
mission.num_commands(),
MAV_MISSION_TYPE_MISSION);
}
return MAV_MISSION_ERROR;
}