diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 93c549403b..02d7f71ef4 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -256,3 +256,8 @@ bool GCS::out_of_time() const return true; } + +void gcs_out_of_space_to_send_count(mavlink_channel_t chan) +{ + gcs().chan(chan)->out_of_space_to_send(); +} diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 48f759d0e0..898d7ab62b 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -29,6 +29,8 @@ // macros used to determine if a message will fit in the space available. +void gcs_out_of_space_to_send_count(mavlink_channel_t chan); + // important note: despite the names, these messages do NOT check to // see if the payload will fit in the buffer. They check to see if // the packed message along with any channel overhead will fit. @@ -40,15 +42,16 @@ // HAVE_PAYLOAD_SPACE evaluates to an expression that can be used // anywhere in the code to determine if the mavlink message with ID id -// can currently fit in the output of _chan. -#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= PAYLOAD_SIZE(chan, id)) +// can currently fit in the output of _chan. Note the use of the "," +// operator here to increment a counter. +#define HAVE_PAYLOAD_SPACE(_chan, id) (comm_get_txspace(_chan) >= PAYLOAD_SIZE(_chan, id) ? (gcs_out_of_space_to_send_count(_chan), true) : false) // CHECK_PAYLOAD_SIZE - macro which may only be used within a // GCS_MAVLink object's methods. It inserts code which will // immediately return false from the current function if there is no // room to fit the mavlink message with id id on the current object's // output -#define CHECK_PAYLOAD_SIZE(id) if (txspace() < unsigned(packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN)) return false +#define CHECK_PAYLOAD_SIZE(id) if (txspace() < unsigned(packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN)) { gcs_out_of_space_to_send_count(chan); return false; } // CHECK_PAYLOAD_SIZE2 - macro which inserts code which will // immediately return false from the current function if there is no @@ -115,6 +118,9 @@ public: return MIN(_port->txspace(), 8192U); } + // this is called when we discover we'd like to send something but can't: + void out_of_space_to_send() { out_of_space_to_send_count++; } + void send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_TYPE mission_type, MAV_MISSION_RESULT result) const { @@ -804,6 +810,7 @@ private: uint8_t last_tx_seq; uint16_t send_packet_count; + uint16_t out_of_space_to_send_count; // number of times HAVE_PAYLOAD_SPACE and friends have returned false #if GCS_DEBUG_SEND_MESSAGE_TIMINGS struct { diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 795ab4ab31..24b4886b86 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1512,7 +1512,7 @@ void GCS_MAVLINK::log_mavlink_stats() flags |= (uint8_t)Flags::LOCKED; } - const struct log_MAV pkt = { + const struct log_MAV pkt{ LOG_PACKET_HEADER_INIT(LOG_MAV_MSG), time_us : AP_HAL::micros64(), chan : (uint8_t)chan, @@ -1520,7 +1520,8 @@ void GCS_MAVLINK::log_mavlink_stats() packet_rx_success_count: status->packet_rx_success_count, packet_rx_drop_count : status->packet_rx_drop_count, flags : flags, - stream_slowdown_ms : stream_slowdown_ms + stream_slowdown_ms : stream_slowdown_ms, + times_full : out_of_space_to_send_count, }; AP::logger().WriteBlock(&pkt, sizeof(pkt));