diff --git a/libmavconn/include/mavconn/interface.h b/libmavconn/include/mavconn/interface.h index f1b5beb6c..d309dd89b 100644 --- a/libmavconn/include/mavconn/interface.h +++ b/libmavconn/include/mavconn/interface.h @@ -144,7 +144,22 @@ class MAVConnInterface { * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) * @param[in] &message not changed */ - virtual void send_message(const mavlink::Message &message) = 0; + virtual void send_message(const mavlink::Message &message) { + send_message(message, this->comp_id); + } + + /** + * @brief Send message (child of mavlink::Message) + * + * Does serialization inside. + * System ID = from this object. + * Component ID passed by argument. + * + * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) + * @param[in] &message not changed + * @param[in] src_compid sets the component ID of the message source + */ + virtual void send_message(const mavlink::Message &message, const uint8_t src_compid) = 0; /** * @brief Send raw bytes (for some quirks) @@ -159,8 +174,20 @@ class MAVConnInterface { /** * @brief Send message and ignore possible drop due to Tx queue limit + * + * System and Component ID = from this object. + */ + void send_message_ignore_drop(const mavlink::Message &message) { + send_message_ignore_drop(message, this->comp_id); + } + + /** + * @brief Send message and ignore possible drop due to Tx queue limit + * + * System ID = from this object. + * Component ID passed by argument. */ - void send_message_ignore_drop(const mavlink::Message &message); + void send_message_ignore_drop(const mavlink::Message &message, const uint8_t src_compid); //! Message receive callback ReceivedCb message_received_cb; diff --git a/libmavconn/include/mavconn/msgbuffer.h b/libmavconn/include/mavconn/msgbuffer.h index 97eadcbbb..9d2f523f7 100644 --- a/libmavconn/include/mavconn/msgbuffer.h +++ b/libmavconn/include/mavconn/msgbuffer.h @@ -92,4 +92,3 @@ struct MsgBuffer { } }; } // namespace mavconn - diff --git a/libmavconn/include/mavconn/serial.h b/libmavconn/include/mavconn/serial.h index d1b18cccb..3f380d6a1 100644 --- a/libmavconn/include/mavconn/serial.h +++ b/libmavconn/include/mavconn/serial.h @@ -45,7 +45,7 @@ class MAVConnSerial : public MAVConnInterface, void close() override; void send_message(const mavlink::mavlink_message_t *message) override; - void send_message(const mavlink::Message &message) override; + void send_message(const mavlink::Message &message, const uint8_t source_compid) override; void send_bytes(const uint8_t *bytes, size_t length) override; inline bool is_open() override { @@ -66,4 +66,3 @@ class MAVConnSerial : public MAVConnInterface, void do_write(bool check_tx_state); }; } // namespace mavconn - diff --git a/libmavconn/include/mavconn/tcp.h b/libmavconn/include/mavconn/tcp.h index 246fce5ff..878f3ee4d 100644 --- a/libmavconn/include/mavconn/tcp.h +++ b/libmavconn/include/mavconn/tcp.h @@ -54,7 +54,7 @@ class MAVConnTCPClient : public MAVConnInterface, void close() override; void send_message(const mavlink::mavlink_message_t *message) override; - void send_message(const mavlink::Message &message) override; + void send_message(const mavlink::Message &message, const uint8_t source_compid) override; void send_bytes(const uint8_t *bytes, size_t length) override; inline bool is_open() override { @@ -108,7 +108,7 @@ class MAVConnTCPServer : public MAVConnInterface, void close() override; void send_message(const mavlink::mavlink_message_t *message) override; - void send_message(const mavlink::Message &message) override; + void send_message(const mavlink::Message &message, const uint8_t source_compid) override; void send_bytes(const uint8_t *bytes, size_t length) override; mavlink::mavlink_status_t get_status() override; @@ -137,4 +137,3 @@ class MAVConnTCPServer : public MAVConnInterface, void recv_message(const mavlink::mavlink_message_t *message, const Framing framing); }; } // namespace mavconn - diff --git a/libmavconn/include/mavconn/udp.h b/libmavconn/include/mavconn/udp.h index fb7bf1b5a..964eb05db 100644 --- a/libmavconn/include/mavconn/udp.h +++ b/libmavconn/include/mavconn/udp.h @@ -53,7 +53,7 @@ class MAVConnUDP : public MAVConnInterface, void close() override; void send_message(const mavlink::mavlink_message_t *message) override; - void send_message(const mavlink::Message &message) override; + void send_message(const mavlink::Message &message, const uint8_t source_compid) override; void send_bytes(const uint8_t *bytes, size_t length) override; inline bool is_open() override { @@ -82,4 +82,3 @@ class MAVConnUDP : public MAVConnInterface, void do_sendto(bool check_tx_state); }; } // namespace mavconn - diff --git a/libmavconn/src/interface.cpp b/libmavconn/src/interface.cpp index eb235753b..19cc4c157 100644 --- a/libmavconn/src/interface.cpp +++ b/libmavconn/src/interface.cpp @@ -168,10 +168,10 @@ void MAVConnInterface::send_message_ignore_drop(const mavlink::mavlink_message_t } } -void MAVConnInterface::send_message_ignore_drop(const mavlink::Message &msg) +void MAVConnInterface::send_message_ignore_drop(const mavlink::Message &msg, uint8_t source_compid) { try { - send_message(msg); + send_message(msg, source_compid); } catch (std::length_error &e) { CONSOLE_BRIDGE_logError(PFX "%zu: DROPPED Message %s: %s", diff --git a/libmavconn/src/serial.cpp b/libmavconn/src/serial.cpp index c62a56af6..fa3be0df4 100644 --- a/libmavconn/src/serial.cpp +++ b/libmavconn/src/serial.cpp @@ -184,7 +184,7 @@ void MAVConnSerial::send_message(const mavlink_message_t *message) io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true)); } -void MAVConnSerial::send_message(const mavlink::Message &message) +void MAVConnSerial::send_message(const mavlink::Message &message, const uint8_t source_compid) { if (!is_open()) { CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); @@ -199,7 +199,7 @@ void MAVConnSerial::send_message(const mavlink::Message &message) if (tx_q.size() >= MAX_TXQ_SIZE) throw std::length_error("MAVConnSerial::send_message: TX queue overflow"); - tx_q.emplace_back(message, get_status_p(), sys_id, comp_id); + tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); } io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true)); } diff --git a/libmavconn/src/tcp.cpp b/libmavconn/src/tcp.cpp index 4d0cd61e1..4538adc08 100644 --- a/libmavconn/src/tcp.cpp +++ b/libmavconn/src/tcp.cpp @@ -191,7 +191,7 @@ void MAVConnTCPClient::send_message(const mavlink_message_t *message) socket.get_io_service().post(std::bind(&MAVConnTCPClient::do_send, shared_from_this(), true)); } -void MAVConnTCPClient::send_message(const mavlink::Message &message) +void MAVConnTCPClient::send_message(const mavlink::Message &message, const uint8_t source_compid) { if (!is_open()) { CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); @@ -206,7 +206,7 @@ void MAVConnTCPClient::send_message(const mavlink::Message &message) if (tx_q.size() >= MAX_TXQ_SIZE) throw std::length_error("MAVConnTCPClient::send_message: TX queue overflow"); - tx_q.emplace_back(message, get_status_p(), sys_id, comp_id); + tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); } socket.get_io_service().post(std::bind(&MAVConnTCPClient::do_send, shared_from_this(), true)); } @@ -397,11 +397,11 @@ void MAVConnTCPServer::send_message(const mavlink_message_t *message) } } -void MAVConnTCPServer::send_message(const mavlink::Message &message) +void MAVConnTCPServer::send_message(const mavlink::Message &message, const uint8_t source_compid) { lock_guard lock(mutex); for (auto &instp : client_list) { - instp->send_message(message); + instp->send_message(message, source_compid); } } diff --git a/libmavconn/src/udp.cpp b/libmavconn/src/udp.cpp index d05d42097..fa13298de 100644 --- a/libmavconn/src/udp.cpp +++ b/libmavconn/src/udp.cpp @@ -208,7 +208,7 @@ void MAVConnUDP::send_message(const mavlink_message_t *message) io_service.post(std::bind(&MAVConnUDP::do_sendto, shared_from_this(), true)); } -void MAVConnUDP::send_message(const mavlink::Message &message) +void MAVConnUDP::send_message(const mavlink::Message &message, const uint8_t source_compid) { if (!is_open()) { CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); @@ -228,7 +228,7 @@ void MAVConnUDP::send_message(const mavlink::Message &message) if (tx_q.size() >= MAX_TXQ_SIZE) throw std::length_error("MAVConnUDP::send_message: TX queue overflow"); - tx_q.emplace_back(message, get_status_p(), sys_id, comp_id); + tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); } io_service.post(std::bind(&MAVConnUDP::do_sendto, shared_from_this(), true)); }