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. // scope.
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false #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: // convenience macros for defining which ap_message ids are in which streams:
#define MAV_STREAM_ENTRY(stream_name) \ #define MAV_STREAM_ENTRY(stream_name) \
{ \ { \
@ -170,6 +176,7 @@ public:
void send_mission_ack(const mavlink_message_t &msg, void send_mission_ack(const mavlink_message_t &msg,
MAV_MISSION_TYPE mission_type, MAV_MISSION_TYPE mission_type,
MAV_MISSION_RESULT result) const { MAV_MISSION_RESULT result) const {
CHECK_PAYLOAD_SIZE2_VOID(chan, MISSION_ACK);
mavlink_msg_mission_ack_send(chan, mavlink_msg_mission_ack_send(chan,
msg.sysid, msg.sysid,
msg.compid, 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 // reply with number of commands in the mission. The GCS will
// then request each command separately // then request each command separately
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_COUNT);
mavlink_msg_mission_count_send(_link.get_chan(), mavlink_msg_mission_count_send(_link.get_chan(),
msg.sysid, msg.sysid,
msg.compid, msg.compid,
@ -148,6 +149,7 @@ void MissionItemProtocol::handle_mission_request_int(const GCS_MAVLINK &_link,
return; return;
} }
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ITEM_INT);
_link.send_message(MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char*)&ret_packet); _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; return;
} }
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ITEM_INT);
_link.send_message(MAVLINK_MSG_ID_MISSION_ITEM, (const char*)&ret_packet); _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, const mavlink_message_t &msg,
MAV_MISSION_RESULT result) const MAV_MISSION_RESULT result) const
{ {
CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ACK);
mavlink_msg_mission_ack_send(_link.get_chan(), mavlink_msg_mission_ack_send(_link.get_chan(),
msg.sysid, msg.sysid,
msg.compid, msg.compid,
@ -316,6 +320,7 @@ void MissionItemProtocol::queued_request_send()
INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link); INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return; return;
} }
CHECK_PAYLOAD_SIZE2_VOID(link->get_chan(), MISSION_REQUEST);
mavlink_msg_mission_request_send( mavlink_msg_mission_request_send(
link->get_chan(), link->get_chan(),
dest_sysid, dest_sysid,
@ -340,11 +345,14 @@ void MissionItemProtocol::update()
if (tnow - timelast_receive_ms > upload_timeout_ms) { if (tnow - timelast_receive_ms > upload_timeout_ms) {
receiving = false; receiving = false;
timeout(); timeout();
mavlink_msg_mission_ack_send(link->get_chan(), const mavlink_channel_t chan = link->get_chan();
dest_sysid, if (HAVE_PAYLOAD_SPACE(chan, MISSION_ACK)) {
dest_compid, mavlink_msg_mission_ack_send(chan,
MAV_MISSION_OPERATION_CANCELLED, dest_sysid,
mission_type()); dest_compid,
MAV_MISSION_OPERATION_CANCELLED,
mission_type());
}
link = nullptr; link = nullptr;
free_upload_resources(); free_upload_resources();
return; 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 if (packet.seq != 0 && // always allow HOME to be read
packet.seq >= mission.num_commands()) { packet.seq >= mission.num_commands()) {
// try to educate the GCS on the actual size of the mission: // try to educate the GCS on the actual size of the mission:
mavlink_msg_mission_count_send(_link.get_chan(), const mavlink_channel_t chan = _link.get_chan();
msg.sysid, if (HAVE_PAYLOAD_SPACE(chan, MISSION_COUNT)) {
msg.compid, mavlink_msg_mission_count_send(chan,
mission.num_commands(), msg.sysid,
MAV_MISSION_TYPE_MISSION); msg.compid,
mission.num_commands(),
MAV_MISSION_TYPE_MISSION);
}
return MAV_MISSION_ERROR; return MAV_MISSION_ERROR;
} }