serial.cpp
Go to the documentation of this file.
1 
9 /*
10  * libmavconn
11  * Copyright 2013,2014,2015,2016,2018 Vladimir Ermakov, All rights reserved.
12  *
13  * This file is part of the mavros package and subject to the license terms
14  * in the top-level LICENSE file of the mavros repository.
15  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
16  */
17 
18 #include <cassert>
19 
21 #include <mavconn/thread_utils.h>
22 #include <mavconn/serial.h>
23 
24 #if defined(__linux__)
25 #include <linux/serial.h>
26 #endif
27 
28 namespace mavconn {
29 
30 using boost::system::error_code;
31 using boost::asio::io_service;
32 using boost::asio::buffer;
33 using mavlink::mavlink_message_t;
34 
35 
36 #define PFX "mavconn: serial"
37 #define PFXd PFX "%zu: "
38 
39 
40 MAVConnSerial::MAVConnSerial(uint8_t system_id, uint8_t component_id,
41  std::string device, unsigned baudrate, bool hwflow) :
42  MAVConnInterface(system_id, component_id),
43  io_service(),
44  serial_dev(io_service),
45  tx_in_progress(false),
46  tx_q {},
47  rx_buf {}
48 {
49  using SPB = boost::asio::serial_port_base;
50 
51  CONSOLE_BRIDGE_logInform(PFXd "device: %s @ %d bps", conn_id, device.c_str(), baudrate);
52 
53  try {
54  serial_dev.open(device);
55 
56  // Set baudrate and 8N1 mode
57  serial_dev.set_option(SPB::baud_rate(baudrate));
58  serial_dev.set_option(SPB::character_size(8));
59  serial_dev.set_option(SPB::parity(SPB::parity::none));
60  serial_dev.set_option(SPB::stop_bits(SPB::stop_bits::one));
61 
62 #if BOOST_ASIO_VERSION >= 101200 || !defined(__linux__)
63  // Flow control setting in older versions of Boost.ASIO is broken, use workaround (below) for now.
64  serial_dev.set_option(SPB::flow_control( (hwflow) ? SPB::flow_control::hardware : SPB::flow_control::none));
65 #elif BOOST_ASIO_VERSION < 101200 && defined(__linux__)
66  // Workaround to set some options for the port manually. This is done in
67  // Boost.ASIO, but until v1.12.0 (Boost 1.66) there was a bug which doesn't enable relevant
68  // code. Fixed by commit: https://github.com/boostorg/asio/commit/619cea4356
69  {
70  int fd = serial_dev.native_handle();
71 
72  termios tio;
73  tcgetattr(fd, &tio);
74 
75  // Set hardware flow control settings
76  if (hwflow) {
77  tio.c_iflag &= ~(IXOFF | IXON);
78  tio.c_cflag |= CRTSCTS;
79  } else {
80  tio.c_iflag &= ~(IXOFF | IXON);
81  tio.c_cflag &= ~CRTSCTS;
82  }
83 
84  // Set serial port to "raw" mode to prevent EOF exit.
85  cfmakeraw(&tio);
86 
87  // Commit settings
88  tcsetattr(fd, TCSANOW, &tio);
89  }
90 #endif
91 
92 #if defined(__linux__)
93  // Enable low latency mode on Linux
94  {
95  int fd = serial_dev.native_handle();
96 
97  struct serial_struct ser_info;
98  ioctl(fd, TIOCGSERIAL, &ser_info);
99 
100  ser_info.flags |= ASYNC_LOW_LATENCY;
101 
102  ioctl(fd, TIOCSSERIAL, &ser_info);
103  }
104 #endif
105  }
106  catch (boost::system::system_error &err) {
107  throw DeviceError("serial", err);
108  }
109 }
110 
112 {
113  close();
114 }
115 
117  const ReceivedCb &cb_handle_message,
118  const ClosedCb &cb_handle_closed_port)
119 {
120  message_received_cb = cb_handle_message;
121  port_closed_cb = cb_handle_closed_port;
122 
123  // give some work to io_service before start
124  io_service.post(std::bind(&MAVConnSerial::do_read, this));
125 
126  // run io_service for async io
127  io_thread = std::thread([this] () {
128  utils::set_this_thread_name("mserial%zu", conn_id);
129  io_service.run();
130  });
131 }
132 
134 {
135  lock_guard lock(mutex);
136  if (!is_open())
137  return;
138 
139  serial_dev.cancel();
140  serial_dev.close();
141 
142  io_service.stop();
143 
144  if (io_thread.joinable())
145  io_thread.join();
146 
147  io_service.reset();
148 
149  if (port_closed_cb)
150  port_closed_cb();
151 }
152 
153 void MAVConnSerial::send_bytes(const uint8_t *bytes, size_t length)
154 {
155  if (!is_open()) {
156  CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id);
157  return;
158  }
159 
160  {
161  lock_guard lock(mutex);
162 
163  if (tx_q.size() >= MAX_TXQ_SIZE)
164  throw std::length_error("MAVConnSerial::send_bytes: TX queue overflow");
165 
166  tx_q.emplace_back(bytes, length);
167  }
168  io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
169 }
170 
172 {
173  assert(message != nullptr);
174 
175  if (!is_open()) {
176  CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id);
177  return;
178  }
179 
180  log_send(PFX, message);
181 
182  {
183  lock_guard lock(mutex);
184 
185  if (tx_q.size() >= MAX_TXQ_SIZE)
186  throw std::length_error("MAVConnSerial::send_message: TX queue overflow");
187 
188  tx_q.emplace_back(message);
189  }
190  io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
191 }
192 
193 void MAVConnSerial::send_message(const mavlink::Message &message, const uint8_t source_compid)
194 {
195  if (!is_open()) {
196  CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id);
197  return;
198  }
199 
200  log_send_obj(PFX, message);
201 
202  {
203  lock_guard lock(mutex);
204 
205  if (tx_q.size() >= MAX_TXQ_SIZE)
206  throw std::length_error("MAVConnSerial::send_message: TX queue overflow");
207 
208  tx_q.emplace_back(message, get_status_p(), sys_id, source_compid);
209  }
210  io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
211 }
212 
214 {
215  auto sthis = shared_from_this();
216  serial_dev.async_read_some(
217  buffer(rx_buf),
218  [sthis] (error_code error, size_t bytes_transferred) {
219  if (error) {
220  CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str());
221  sthis->close();
222  return;
223  }
224 
225  sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred);
226  sthis->do_read();
227  });
228 }
229 
230 void MAVConnSerial::do_write(bool check_tx_state)
231 {
232  if (check_tx_state && tx_in_progress)
233  return;
234 
235  lock_guard lock(mutex);
236  if (tx_q.empty())
237  return;
238 
239  tx_in_progress = true;
240  auto sthis = shared_from_this();
241  auto &buf_ref = tx_q.front();
242  serial_dev.async_write_some(
243  buffer(buf_ref.dpos(), buf_ref.nbytes()),
244  [sthis, &buf_ref] (error_code error, size_t bytes_transferred) {
245  assert(bytes_transferred <= buf_ref.len);
246 
247  if (error) {
248  CONSOLE_BRIDGE_logError(PFXd "write: %s", sthis->conn_id, error.message().c_str());
249  sthis->close();
250  return;
251  }
252 
253  sthis->iostat_tx_add(bytes_transferred);
254  lock_guard lock(sthis->mutex);
255 
256  if (sthis->tx_q.empty()) {
257  sthis->tx_in_progress = false;
258  return;
259  }
260 
261  buf_ref.pos += bytes_transferred;
262  if (buf_ref.nbytes() == 0) {
263  sthis->tx_q.pop_front();
264  }
265 
266  if (!sthis->tx_q.empty())
267  sthis->do_write(false);
268  else
269  sthis->tx_in_progress = false;
270  });
271 }
272 } // namespace mavconn
void log_send(const char *pfx, const mavlink::mavlink_message_t *msg)
Definition: interface.cpp:132
ClosedCb port_closed_cb
Port closed notification callback.
Definition: interface.h:202
std::lock_guard< std::recursive_mutex > lock_guard
Definition: interface.h:42
virtual ~MAVConnSerial()
Definition: serial.cpp:111
void connect(const ReceivedCb &cb_handle_message, const ClosedCb &cb_handle_closed_port=ClosedCb()) override
Establish connection, automatically called by open_url()
Definition: serial.cpp:116
std::function< void(const mavlink::mavlink_message_t *message, const Framing framing)> ReceivedCb
Definition: interface.h:102
#define CONSOLE_BRIDGE_logInform(fmt,...)
static constexpr size_t MAX_TXQ_SIZE
Maximum count of transmission buffers.
Definition: interface.h:268
boost::asio::io_service io_service
Definition: serial.h:59
Common exception for communication error.
Definition: interface.h:64
std::thread io_thread
Definition: serial.h:60
void log_send_obj(const char *pfx, const mavlink::Message &msg)
Definition: interface.cpp:142
ReceivedCb message_received_cb
Message receive callback.
Definition: interface.h:200
std::array< uint8_t, MsgBuffer::MAX_SIZE > rx_buf
Definition: serial.h:65
void close() override
Close connection.
Definition: serial.cpp:133
Generic mavlink interface.
Definition: interface.h:97
#define PFXd
Definition: serial.cpp:37
void send_bytes(const uint8_t *bytes, size_t length) override
Send raw bytes (for some quirks)
Definition: serial.cpp:153
mavlink::mavlink_status_t * get_status_p()
Definition: interface.h:276
std::function< void(void)> ClosedCb
Definition: interface.h:103
size_t conn_id
Channel number used for logging.
Definition: interface.h:274
#define CONSOLE_BRIDGE_logError(fmt,...)
MAVConn Serial link class.
boost::asio::serial_port serial_dev
Definition: serial.h:61
some useful utils
MAVConn console-bridge compatibility header.
std::deque< MsgBuffer > tx_q
Definition: serial.h:64
bool set_this_thread_name(const std::string &name, Args &&... args)
Set name to current thread, printf-like.
Definition: thread_utils.h:55
#define PFX
Definition: serial.cpp:36
bool is_open() override
Definition: serial.h:54
std::atomic< bool > tx_in_progress
Definition: serial.h:63
MAVConnSerial(uint8_t system_id=1, uint8_t component_id=MAV_COMP_ID_UDP_BRIDGE, std::string device=DEFAULT_DEVICE, unsigned baudrate=DEFAULT_BAUDRATE, bool hwflow=false)
Definition: serial.cpp:40
void send_message(const mavlink::mavlink_message_t *message) override
Send message (mavlink_message_t)
uint8_t sys_id
Connection System Id.
Definition: interface.h:262
void do_write(bool check_tx_state)
Definition: serial.cpp:230
std::recursive_mutex mutex
Definition: serial.h:66


libmavconn
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 13 2023 02:17:45