diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index e3d0461976..886d1f72a4 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -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, diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.cpp b/libraries/GCS_MAVLink/MissionItemProtocol.cpp index 5792adc9f1..b90cc3b166 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol.cpp @@ -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; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp index e707a1e286..6878ae92bb 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp @@ -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; }