Skip to content

Commit

Permalink
libmavconn: add the possibility to set the source component ID throug…
Browse files Browse the repository at this point in the history 
…h the send_message method
  • Loading branch information
@TSC21 @vooon
TSC21 authored and vooon committed Nov 19, 2018
1 parent aa69d4f commit 7c8b06b
Show file tree
Hide file tree
Showing 9 changed files with 43 additions and 20 deletions.
31 changes: 29 additions & 2 deletions libmavconn/include/mavconn/interface.h
View file
Expand Up @@ -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)
Expand All @@ -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;
Expand Down
1 change: 0 additions & 1 deletion libmavconn/include/mavconn/msgbuffer.h
View file
Expand Up @@ -92,4 +92,3 @@ struct MsgBuffer {
}
};
} // namespace mavconn

3 changes: 1 addition & 2 deletions libmavconn/include/mavconn/serial.h
View file
Expand Up @@ -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 {
Expand All @@ -66,4 +66,3 @@ class MAVConnSerial : public MAVConnInterface,
void do_write(bool check_tx_state);
};
} // namespace mavconn

5 changes: 2 additions & 3 deletions libmavconn/include/mavconn/tcp.h
View file
Expand Up @@ -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 {
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -137,4 +137,3 @@ class MAVConnTCPServer : public MAVConnInterface,
void recv_message(const mavlink::mavlink_message_t *message, const Framing framing);
};
} // namespace mavconn

3 changes: 1 addition & 2 deletions libmavconn/include/mavconn/udp.h
View file
Expand Up @@ -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 {
Expand Down Expand Up @@ -82,4 +82,3 @@ class MAVConnUDP : public MAVConnInterface,
void do_sendto(bool check_tx_state);
};
} // namespace mavconn

4 changes: 2 additions & 2 deletions libmavconn/src/interface.cpp
View file
Expand Up @@ -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",
Expand Down
4 changes: 2 additions & 2 deletions libmavconn/src/serial.cpp
View file
Expand Up @@ -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);
Expand All @@ -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));
}
Expand Down
8 changes: 4 additions & 4 deletions libmavconn/src/tcp.cpp
View file
Expand Up @@ -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);
Expand All @@ -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));
}
Expand Down Expand Up @@ -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);
}
}

Expand Down
4 changes: 2 additions & 2 deletions libmavconn/src/udp.cpp
View file
Expand Up @@ -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);
Expand All @@ -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));
}
Expand Down

0 comments on commit 7c8b06b

Please sign in to comment.