26 using boost::system::error_code;
27 using boost::asio::io_service;
28 using boost::asio::ip::udp;
29 using boost::asio::buffer;
31 using utils::operator
"" _KiB;
32 using mavlink::mavlink_message_t;
34 #define PFX "mavconn: udp" 35 #define PFXd PFX "%zu: " 38 static bool resolve_address_udp(io_service &io,
size_t chan, std::string host,
unsigned short port, udp::endpoint &ep)
41 udp::resolver resolver(io);
44 udp::resolver::query query(host,
"");
46 auto fn = [&](
const udp::endpoint & q_ep) {
53 #if BOOST_ASIO_VERSION >= 101200 54 for (
auto q_ep : resolver.resolve(query, ec)) fn(q_ep);
56 std::for_each(resolver.resolve(query, ec), udp::resolver::iterator(), fn);
69 std::string bind_host,
unsigned short bind_port,
70 std::string remote_host,
unsigned short remote_port) :
73 io_work(new io_service::work(io_service)),
74 permanent_broadcast(false),
77 tx_in_progress(false),
81 using udps = boost::asio::ip::udp::socket;
84 throw DeviceError(
"udp: resolve",
"Bind address resolve failed");
88 if (remote_host !=
"") {
93 remote_ep = udp::endpoint(boost::asio::ip::address_v4::broadcast(), remote_port);
106 socket.set_option(udps::reuse_address(
true));
107 socket.set_option(udps::send_buffer_size(256_KiB));
108 socket.set_option(udps::receive_buffer_size(512_KiB));
113 socket.set_option(udps::broadcast(
true));
115 socket.set_option(udps::broadcast(
true));
119 catch (boost::system::system_error &err) {
131 const ClosedCb &cb_handle_closed_port)
183 throw std::length_error(
"MAVConnUDP::send_bytes: TX queue overflow");
185 tx_q.emplace_back(bytes, length);
192 assert(message !=
nullptr);
210 throw std::length_error(
"MAVConnUDP::send_message: TX queue overflow");
212 tx_q.emplace_back(message);
235 throw std::length_error(
"MAVConnUDP::send_message: TX queue overflow");
244 auto sthis = shared_from_this();
245 socket.async_receive_from(
248 [sthis] (error_code error,
size_t bytes_transferred) {
255 if (!sthis->permanent_broadcast && sthis->remote_ep != sthis->last_remote_ep) {
257 sthis->remote_exists =
true;
258 sthis->last_remote_ep = sthis->remote_ep;
261 sthis->parse_buffer(
PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred);
262 sthis->do_recvfrom();
276 auto sthis = shared_from_this();
277 auto &buf_ref =
tx_q.front();
279 buffer(buf_ref.dpos(), buf_ref.nbytes()),
281 [sthis, &buf_ref] (error_code error,
size_t bytes_transferred) {
282 assert(bytes_transferred <= buf_ref.len);
284 if (error == boost::asio::error::network_unreachable) {
294 sthis->iostat_tx_add(bytes_transferred);
297 if (sthis->tx_q.empty()) {
298 sthis->tx_in_progress =
false;
302 buf_ref.pos += bytes_transferred;
303 if (buf_ref.nbytes() == 0) {
304 sthis->tx_q.pop_front();
307 if (!sthis->tx_q.empty())
308 sthis->do_sendto(
false);
310 sthis->tx_in_progress =
false;
void log_send(const char *pfx, const mavlink::mavlink_message_t *msg)
ClosedCb port_closed_cb
Port closed notification callback.
std::lock_guard< std::recursive_mutex > lock_guard
std::function< void(const mavlink::mavlink_message_t *message, const Framing framing)> ReceivedCb
#define CONSOLE_BRIDGE_logInform(fmt,...)
boost::asio::ip::udp::endpoint bind_ep
#define CONSOLE_BRIDGE_logWarn(fmt,...)
void connect(const ReceivedCb &cb_handle_message, const ClosedCb &cb_handle_closed_port=ClosedCb()) override
Establish connection, automatically called by open_url()
static constexpr size_t MAX_TXQ_SIZE
Maximum count of transmission buffers.
Common exception for communication error.
const std::string to_string_ss(T &obj)
Convert to string objects with operator <<.
MAVConnUDP(uint8_t system_id=1, uint8_t component_id=MAV_COMP_ID_UDP_BRIDGE, std::string bind_host=DEFAULT_BIND_HOST, unsigned short bind_port=DEFAULT_BIND_PORT, std::string remote_host=DEFAULT_REMOTE_HOST, unsigned short remote_port=DEFAULT_REMOTE_PORT)
void send_bytes(const uint8_t *bytes, size_t length) override
Send raw bytes (for some quirks)
#define CONSOLE_BRIDGE_logDebug(fmt,...)
static bool resolve_address_udp(io_service &io, size_t chan, std::string host, unsigned short port, udp::endpoint &ep)
void log_send_obj(const char *pfx, const mavlink::Message &msg)
ReceivedCb message_received_cb
Message receive callback.
boost::asio::ip::udp::endpoint remote_ep
std::string get_remote_endpoint() const
Generic mavlink interface.
std::deque< MsgBuffer > tx_q
std::array< uint8_t, MsgBuffer::MAX_SIZE > rx_buf
mavlink::mavlink_status_t * get_status_p()
std::function< void(void)> ClosedCb
size_t conn_id
Channel number used for logging.
#define CONSOLE_BRIDGE_logError(fmt,...)
static constexpr auto BROADCAST_REMOTE_HOST
Markers for broadcast modes. Not valid domain names.
void close() override
Close connection.
MAVConn console-bridge compatibility header.
static constexpr auto PERMANENT_BROADCAST_REMOTE_HOST
bool set_this_thread_name(const std::string &name, Args &&... args)
Set name to current thread, printf-like.
struct __mavlink_message mavlink_message_t
boost::asio::ip::udp::socket socket
std::atomic< bool > remote_exists
std::atomic< bool > tx_in_progress
void do_sendto(bool check_tx_state)
std::unique_ptr< boost::asio::io_service::work > io_work
boost::asio::io_service io_service
void send_message(const mavlink::mavlink_message_t *message) override
Send message (mavlink_message_t)
boost::asio::ip::udp::endpoint recv_ep
uint8_t sys_id
Connection System Id.
std::recursive_mutex mutex