serial.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * libmavconn
00011  * Copyright 2013,2014,2015 Vladimir Ermakov, All rights reserved.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 
00018 #include <cassert>
00019 #include <console_bridge/console.h>
00020 
00021 #include <mavconn/thread_utils.h>
00022 #include <mavconn/serial.h>
00023 
00024 namespace mavconn {
00025 using boost::system::error_code;
00026 using boost::asio::io_service;
00027 using boost::asio::serial_port_base;
00028 using boost::asio::buffer;
00029 typedef std::lock_guard<std::recursive_mutex> lock_guard;
00030 
00031 #define PFXd    "mavconn: serial%d: "
00032 
00033 
00034 MAVConnSerial::MAVConnSerial(uint8_t system_id, uint8_t component_id,
00035                 std::string device, unsigned baudrate) :
00036         MAVConnInterface(system_id, component_id),
00037         tx_in_progress(false),
00038         io_service(),
00039         serial_dev(io_service)
00040 {
00041         logInform(PFXd "device: %s @ %d bps", channel, device.c_str(), baudrate);
00042 
00043         try {
00044                 serial_dev.open(device);
00045 
00046                 // Sent baudrate, and 8N1 mode
00047                 serial_dev.set_option(serial_port_base::baud_rate(baudrate));
00048                 serial_dev.set_option(serial_port_base::character_size(8));
00049                 serial_dev.set_option(serial_port_base::parity(serial_port_base::parity::none));
00050                 serial_dev.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one));
00051                 serial_dev.set_option(serial_port_base::flow_control(serial_port_base::flow_control::none));
00052         }
00053         catch (boost::system::system_error &err) {
00054                 throw DeviceError("serial", err);
00055         }
00056 
00057         // give some work to io_service before start
00058         io_service.post(boost::bind(&MAVConnSerial::do_read, this));
00059 
00060         // run io_service for async io
00061         std::thread t(boost::bind(&io_service::run, &this->io_service));
00062         mavutils::set_thread_name(t, "MAVConnSerial%d", channel);
00063         io_thread.swap(t);
00064 }
00065 
00066 MAVConnSerial::~MAVConnSerial() {
00067         close();
00068 }
00069 
00070 void MAVConnSerial::close() {
00071         lock_guard lock(mutex);
00072         if (!is_open())
00073                 return;
00074 
00075         io_service.stop();
00076         serial_dev.close();
00077 
00078         // clear tx queue
00079         for (auto &p : tx_q)
00080                 delete p;
00081         tx_q.clear();
00082 
00083         if (io_thread.joinable())
00084                 io_thread.join();
00085 
00086         /* emit */ port_closed();
00087 }
00088 
00089 void MAVConnSerial::send_bytes(const uint8_t *bytes, size_t length)
00090 {
00091         if (!is_open()) {
00092                 logError(PFXd "send: channel closed!", channel);
00093                 return;
00094         }
00095 
00096         MsgBuffer *buf = new MsgBuffer(bytes, length);
00097         {
00098                 lock_guard lock(mutex);
00099                 tx_q.push_back(buf);
00100         }
00101         io_service.post(boost::bind(&MAVConnSerial::do_write, this, true));
00102 }
00103 
00104 void MAVConnSerial::send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid)
00105 {
00106         assert(message != nullptr);
00107 
00108         if (!is_open()) {
00109                 logError(PFXd "send: channel closed!", channel);
00110                 return;
00111         }
00112 
00113         logDebug(PFXd "send: Message-Id: %d [%d bytes] Sys-Id: %d Comp-Id: %d Seq: %d",
00114                         channel, message->msgid, message->len, sysid, compid, message->seq);
00115 
00116         MsgBuffer *buf = new_msgbuffer(message, sysid, compid);
00117         {
00118                 lock_guard lock(mutex);
00119                 tx_q.push_back(buf);
00120         }
00121         io_service.post(boost::bind(&MAVConnSerial::do_write, this, true));
00122 }
00123 
00124 void MAVConnSerial::do_read(void)
00125 {
00126         serial_dev.async_read_some(
00127                         buffer(rx_buf, sizeof(rx_buf)),
00128                         boost::bind(&MAVConnSerial::async_read_end,
00129                                 this,
00130                                 boost::asio::placeholders::error,
00131                                 boost::asio::placeholders::bytes_transferred));
00132 }
00133 
00134 void MAVConnSerial::async_read_end(error_code error, size_t bytes_transferred)
00135 {
00136         mavlink_message_t message;
00137         mavlink_status_t status;
00138 
00139         if (error) {
00140                 logError(PFXd "receive: %s", channel, error.message().c_str());
00141                 close();
00142                 return;
00143         }
00144 
00145         iostat_rx_add(bytes_transferred);
00146         for (size_t i = 0; i < bytes_transferred; i++) {
00147                 if (mavlink_parse_char(channel, rx_buf[i], &message, &status)) {
00148                         logDebug(PFXd "recv: Message-Id: %d [%d bytes] Sys-Id: %d Comp-Id: %d Seq: %d",
00149                                         channel, message.msgid, message.len, message.sysid, message.compid, message.seq);
00150 
00151                         /* emit */ message_received(&message, message.sysid, message.compid);
00152                 }
00153         }
00154 
00155         do_read();
00156 }
00157 
00158 void MAVConnSerial::do_write(bool check_tx_state)
00159 {
00160         if (check_tx_state && tx_in_progress)
00161                 return;
00162 
00163         lock_guard lock(mutex);
00164         if (tx_q.empty())
00165                 return;
00166 
00167         tx_in_progress = true;
00168         MsgBuffer *buf = tx_q.front();
00169         serial_dev.async_write_some(
00170                         buffer(buf->dpos(), buf->nbytes()),
00171                         boost::bind(&MAVConnSerial::async_write_end,
00172                                 this,
00173                                 boost::asio::placeholders::error,
00174                                 boost::asio::placeholders::bytes_transferred));
00175 }
00176 
00177 void MAVConnSerial::async_write_end(error_code error, size_t bytes_transferred)
00178 {
00179         if (error) {
00180                 logError(PFXd "write: %s", channel, error.message().c_str());
00181                 close();
00182                 return;
00183         }
00184 
00185         iostat_tx_add(bytes_transferred);
00186         lock_guard lock(mutex);
00187         if (tx_q.empty()) {
00188                 tx_in_progress = false;
00189                 return;
00190         }
00191 
00192         MsgBuffer *buf = tx_q.front();
00193         buf->pos += bytes_transferred;
00194         if (buf->nbytes() == 0) {
00195                 tx_q.pop_front();
00196                 delete buf;
00197         }
00198 
00199         if (!tx_q.empty())
00200                 do_write(false);
00201         else
00202                 tx_in_progress = false;
00203 }
00204 
00205 }; // namespace mavconn


libmavconn
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:13