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  // NOTE: shared_from_this() should not be used in constructors
111 
112  // give some work to io_service before start
113  io_service.post(std::bind(&MAVConnSerial::do_read, this));
114 
115  // run io_service for async io
116  io_thread = std::thread([this] () {
117  utils::set_this_thread_name("mserial%zu", conn_id);
118  io_service.run();
119  });
120 }
121 
123 {
124  close();
125 }
126 
128 {
129  lock_guard lock(mutex);
130  if (!is_open())
131  return;
132 
133  serial_dev.cancel();
134  serial_dev.close();
135 
136  io_service.stop();
137 
138  if (io_thread.joinable())
139  io_thread.join();
140 
141  io_service.reset();
142 
143  if (port_closed_cb)
144  port_closed_cb();
145 }
146 
147 void MAVConnSerial::send_bytes(const uint8_t *bytes, size_t length)
148 {
149  if (!is_open()) {
150  CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id);
151  return;
152  }
153 
154  {
155  lock_guard lock(mutex);
156 
157  if (tx_q.size() >= MAX_TXQ_SIZE)
158  throw std::length_error("MAVConnSerial::send_bytes: TX queue overflow");
159 
160  tx_q.emplace_back(bytes, length);
161  }
162  io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
163 }
164 
166 {
167  assert(message != nullptr);
168 
169  if (!is_open()) {
170  CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id);
171  return;
172  }
173 
174  log_send(PFX, message);
175 
176  {
177  lock_guard lock(mutex);
178 
179  if (tx_q.size() >= MAX_TXQ_SIZE)
180  throw std::length_error("MAVConnSerial::send_message: TX queue overflow");
181 
182  tx_q.emplace_back(message);
183  }
184  io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
185 }
186 
187 void MAVConnSerial::send_message(const mavlink::Message &message, const uint8_t source_compid)
188 {
189  if (!is_open()) {
190  CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id);
191  return;
192  }
193 
194  log_send_obj(PFX, message);
195 
196  {
197  lock_guard lock(mutex);
198 
199  if (tx_q.size() >= MAX_TXQ_SIZE)
200  throw std::length_error("MAVConnSerial::send_message: TX queue overflow");
201 
202  tx_q.emplace_back(message, get_status_p(), sys_id, source_compid);
203  }
204  io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
205 }
206 
208 {
209  auto sthis = shared_from_this();
210  serial_dev.async_read_some(
211  buffer(rx_buf),
212  [sthis] (error_code error, size_t bytes_transferred) {
213  if (error) {
214  CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str());
215  sthis->close();
216  return;
217  }
218 
219  sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred);
220  sthis->do_read();
221  });
222 }
223 
224 void MAVConnSerial::do_write(bool check_tx_state)
225 {
226  if (check_tx_state && tx_in_progress)
227  return;
228 
229  lock_guard lock(mutex);
230  if (tx_q.empty())
231  return;
232 
233  tx_in_progress = true;
234  auto sthis = shared_from_this();
235  auto &buf_ref = tx_q.front();
236  serial_dev.async_write_some(
237  buffer(buf_ref.dpos(), buf_ref.nbytes()),
238  [sthis, &buf_ref] (error_code error, size_t bytes_transferred) {
239  assert(bytes_transferred <= buf_ref.len);
240 
241  if (error) {
242  CONSOLE_BRIDGE_logError(PFXd "write: %s", sthis->conn_id, error.message().c_str());
243  sthis->close();
244  return;
245  }
246 
247  sthis->iostat_tx_add(bytes_transferred);
248  lock_guard lock(sthis->mutex);
249 
250  if (sthis->tx_q.empty()) {
251  sthis->tx_in_progress = false;
252  return;
253  }
254 
255  buf_ref.pos += bytes_transferred;
256  if (buf_ref.nbytes() == 0) {
257  sthis->tx_q.pop_front();
258  }
259 
260  if (!sthis->tx_q.empty())
261  sthis->do_write(false);
262  else
263  sthis->tx_in_progress = false;
264  });
265 }
266 } // 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:195
std::lock_guard< std::recursive_mutex > lock_guard
Definition: interface.h:42
virtual ~MAVConnSerial()
Definition: serial.cpp:122
#define CONSOLE_BRIDGE_logInform(fmt,...)
static constexpr size_t MAX_TXQ_SIZE
Maximum count of transmission buffers.
Definition: interface.h:249
boost::asio::io_service io_service
Definition: serial.h:56
Common exception for communication error.
Definition: interface.h:64
std::thread io_thread
Definition: serial.h:57
bool set_this_thread_name(const std::string &name, Args &&...args)
Set name to current thread, printf-like.
Definition: thread_utils.h:55
void log_send_obj(const char *pfx, const mavlink::Message &msg)
Definition: interface.cpp:142
std::array< uint8_t, MsgBuffer::MAX_SIZE > rx_buf
Definition: serial.h:62
void close() override
Close connection.
Definition: serial.cpp:127
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:147
mavlink::mavlink_status_t * get_status_p()
Definition: interface.h:257
size_t conn_id
Channel number used for logging.
Definition: interface.h:255
#define CONSOLE_BRIDGE_logError(fmt,...)
MAVConn Serial link class.
boost::asio::serial_port serial_dev
Definition: serial.h:58
some useful utils
MAVConn console-bridge compatibility header.
std::deque< MsgBuffer > tx_q
Definition: serial.h:61
#define PFX
Definition: serial.cpp:36
bool is_open() override
Definition: serial.h:51
std::atomic< bool > tx_in_progress
Definition: serial.h:60
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:243
void do_write(bool check_tx_state)
Definition: serial.cpp:224
std::recursive_mutex mutex
Definition: serial.h:63


libmavconn
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:21