diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index e526e3922c..dfab6165d1 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -195,6 +195,10 @@ public: GCS_MAVLINK(GCS_MAVLINK_Parameters ¶meters, AP_HAL::UARTDriver &uart); virtual ~GCS_MAVLINK() {} + // accessors used to retrieve objects used for parsing incoming messages: + mavlink_message_t *channel_buffer() { return &_channel_buffer; } + mavlink_status_t *channel_status() { return &_channel_status; } + void update_receive(uint32_t max_time_us=1000); void update_send(); bool init(uint8_t instance); @@ -727,6 +731,10 @@ protected: private: + // define the two objects used for parsing incoming messages: + mavlink_message_t _channel_buffer; + mavlink_status_t _channel_status; + const AP_SerialManager::UARTState *uartstate; // last time we got a non-zero RSSI from RADIO_STATUS diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ad8b11737b..c430aa302e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -164,18 +164,14 @@ bool GCS_MAVLINK::init(uint8_t instance) mavlink_comm_port[chan] = _port; const auto mavlink_protocol = uartstate->get_protocol(); - mavlink_status_t *status = mavlink_get_channel_status(chan); - if (status == nullptr) { - return false; - } - + if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 || mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) { // load signing key load_signing_key(); } else { // user has asked to only send MAVLink1 - status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + _channel_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; } #if HAL_HIGH_LATENCY2_ENABLED @@ -1512,11 +1508,8 @@ void GCS_MAVLINK::update_send() // update the number of packets transmitted base on seqno, making // the assumption that we don't send more than 256 messages // between the last pass through here - mavlink_status_t *status = mavlink_get_channel_status(chan); - if (status != nullptr) { - send_packet_count += uint8_t(status->current_tx_seq - last_tx_seq); - last_tx_seq = status->current_tx_seq; - } + send_packet_count += uint8_t(_channel_status.current_tx_seq - last_tx_seq); + last_tx_seq = _channel_status.current_tx_seq; } void GCS_MAVLINK::remove_message_from_bucket(int8_t bucket, ap_message id) @@ -1686,10 +1679,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, // if we receive any MAVLink2 packets on a connection // currently sending MAVLink1 then switch to sending // MAVLink2 - mavlink_status_t *cstatus = mavlink_get_channel_status(chan); - if (cstatus != nullptr) { - cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } + _channel_status.flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; } if (!routing.check_and_forward(*this, msg)) { // the routing code has indicated we should not handle this packet locally @@ -1878,11 +1868,6 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us) */ void GCS_MAVLINK::log_mavlink_stats() { - mavlink_status_t *status = mavlink_get_channel_status(chan); - if (status == nullptr) { - return; - } - uint8_t flags = 0; if (signing_enabled()) { flags |= (uint8_t)Flags::USING_SIGNING; @@ -1905,8 +1890,8 @@ void GCS_MAVLINK::log_mavlink_stats() time_us : AP_HAL::micros64(), chan : (uint8_t)chan, packet_tx_count : send_packet_count, - packet_rx_success_count: status->packet_rx_success_count, - packet_rx_drop_count : status->packet_rx_drop_count, + packet_rx_success_count: _channel_status.packet_rx_success_count, + packet_rx_drop_count : _channel_status.packet_rx_drop_count, flags : flags, stream_slowdown_ms : stream_slowdown_ms, times_full : out_of_space_to_send_count, @@ -1965,12 +1950,7 @@ void GCS_MAVLINK::send_rc_channels() const bool GCS_MAVLINK::sending_mavlink1() const { - const mavlink_status_t *status = mavlink_get_channel_status(chan); - if (status == nullptr) { - // should not happen - return true; - } - return ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0); + return ((_channel_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0); } void GCS_MAVLINK::send_rc_channels_raw() const diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index e93b26f7ec..41ef15b497 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -56,6 +56,22 @@ GCS_MAVLINK *GCS_MAVLINK::find_by_mavtype_and_compid(uint8_t mav_type, uint8_t c return gcs().chan(channel); } +mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { + GCS_MAVLINK *link = gcs().chan(chan); + if (link == nullptr) { + return nullptr; + } + return link->channel_buffer(); +} + +mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { + GCS_MAVLINK *link = gcs().chan(chan); + if (link == nullptr) { + return nullptr; + } + return link->channel_status(); +} + // set a channel as private. Private channels get sent heartbeats, but // don't get broadcast packets or forwarded packets void GCS_MAVLINK::set_channel_private(mavlink_channel_t _chan) diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index b59b78532b..dd4ab4271a 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -17,6 +17,9 @@ // allow five telemetry ports #define MAVLINK_COMM_NUM_BUFFERS 5 +#define MAVLINK_GET_CHANNEL_BUFFER 1 +#define MAVLINK_GET_CHANNEL_STATUS 1 + /* The MAVLink protocol code generator does its own alignment, so alignment cast warnings can be ignored @@ -52,6 +55,9 @@ static inline bool valid_channel(mavlink_channel_t chan) #pragma clang diagnostic pop } +mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); +mavlink_status_t* mavlink_get_channel_status(uint8_t chan); + void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len); /// Check for available transmit space on the nominated MAVLink channel diff --git a/libraries/GCS_MAVLink/GCS_Signing.cpp b/libraries/GCS_MAVLink/GCS_Signing.cpp index 93aaef90db..c0dff3b80a 100644 --- a/libraries/GCS_MAVLink/GCS_Signing.cpp +++ b/libraries/GCS_MAVLink/GCS_Signing.cpp @@ -132,11 +132,6 @@ void GCS_MAVLINK::load_signing_key(void) if (!signing_key_load(key)) { return; } - mavlink_status_t *status = mavlink_get_channel_status(chan); - if (status == nullptr) { - DEV_PRINTF("Failed to load signing key - no status"); - return; - } memcpy(signing.secret_key, key.secret_key, 32); signing.link_id = (uint8_t)chan; // use a timestamp 1 minute past the last recorded @@ -156,11 +151,11 @@ void GCS_MAVLINK::load_signing_key(void) } if (all_zero) { // disable signing - status->signing = nullptr; - status->signing_streams = nullptr; + _channel_status.signing = nullptr; + _channel_status.signing_streams = nullptr; } else { - status->signing = &signing; - status->signing_streams = &signing_streams; + _channel_status.signing = &signing; + _channel_status.signing_streams = &signing_streams; } }