serial.cpp

Go to the documentation of this file.

 
/*
 * libmavconn
 * Copyright 2013,2014,2015,2016,2018 Vladimir Ermakov, All rights reserved.
 *
 * This file is part of the mavros package and subject to the license terms
 * in the top-level LICENSE file of the mavros repository.
 * https://github.com/mavlink/mavros/tree/master/LICENSE.md
 */
 
#include <cassert>
 
#include <mavconn/console_bridge_compat.h>
#include <mavconn/thread_utils.h>
#include <mavconn/serial.h>
 
#if defined(__linux__)
#include <linux/serial.h>
#endif
 
namespace mavconn {
 
using boost::system::error_code;
using boost::asio::io_service;
using boost::asio::buffer;
using mavlink::mavlink_message_t;
 
 
#define PFX     "mavconn: serial"
#define PFXd    PFX "%zu: "
 
 
MAVConnSerial::MAVConnSerial(uint8_t system_id, uint8_t component_id,
                std::string device, unsigned baudrate, bool hwflow) :
        MAVConnInterface(system_id, component_id),
        io_service(),
        serial_dev(io_service),
        tx_in_progress(false),
        tx_q {},
        rx_buf {}
{
        using SPB = boost::asio::serial_port_base;
 
        CONSOLE_BRIDGE_logInform(PFXd "device: %s @ %d bps", conn_id, device.c_str(), baudrate);
 
        try {
                serial_dev.open(device);
 
                // Set baudrate and 8N1 mode
                serial_dev.set_option(SPB::baud_rate(baudrate));
                serial_dev.set_option(SPB::character_size(8));
                serial_dev.set_option(SPB::parity(SPB::parity::none));
                serial_dev.set_option(SPB::stop_bits(SPB::stop_bits::one));
 
#if BOOST_ASIO_VERSION >= 101200 || !defined(__linux__)
                // Flow control setting in older versions of Boost.ASIO is broken, use workaround (below) for now.
                serial_dev.set_option(SPB::flow_control( (hwflow) ? SPB::flow_control::hardware : SPB::flow_control::none));
#elif BOOST_ASIO_VERSION < 101200 && defined(__linux__)
                // Workaround to set some options for the port manually. This is done in
                // Boost.ASIO, but until v1.12.0 (Boost 1.66) there was a bug which doesn't enable relevant
                // code. Fixed by commit: https://github.com/boostorg/asio/commit/619cea4356
                {
                        int fd = serial_dev.native_handle();
 
                        termios tio;
                        tcgetattr(fd, &tio);
 
                        // Set hardware flow control settings
                        if (hwflow) {
                                tio.c_iflag &= ~(IXOFF | IXON);
                                tio.c_cflag |= CRTSCTS;
                        } else {
                                tio.c_iflag &= ~(IXOFF | IXON);
                                tio.c_cflag &= ~CRTSCTS;
                        }
 
                        // Set serial port to "raw" mode to prevent EOF exit.
                        cfmakeraw(&tio);
 
                        // Commit settings
                        tcsetattr(fd, TCSANOW, &tio);
                }
#endif
 
#if defined(__linux__)
                // Enable low latency mode on Linux
                {
                        int fd = serial_dev.native_handle();
 
                        struct serial_struct ser_info;
                        ioctl(fd, TIOCGSERIAL, &ser_info);
 
                        ser_info.flags |= ASYNC_LOW_LATENCY;
 
                        ioctl(fd, TIOCSSERIAL, &ser_info);
                }
#endif
        }
        catch (boost::system::system_error &err) {
                throw DeviceError("serial", err);
        }
}
 
MAVConnSerial::~MAVConnSerial()
{
        close();
}
 
void MAVConnSerial::connect(
                const ReceivedCb &cb_handle_message,
                const ClosedCb &cb_handle_closed_port)
{
        message_received_cb = cb_handle_message;
        port_closed_cb = cb_handle_closed_port;
 
        // give some work to io_service before start
        io_service.post(std::bind(&MAVConnSerial::do_read, this));
 
        // run io_service for async io
        io_thread = std::thread([this] () {
                                utils::set_this_thread_name("mserial%zu", conn_id);
                                io_service.run();
                        });
}
 
void MAVConnSerial::close()
{
        lock_guard lock(mutex);
        if (!is_open())
                return;
 
        serial_dev.cancel();
        serial_dev.close();
 
        io_service.stop();
 
        if (io_thread.joinable())
                io_thread.join();
 
        io_service.reset();
 
        if (port_closed_cb)
                port_closed_cb();
}
 
void MAVConnSerial::send_bytes(const uint8_t *bytes, size_t length)
{
        if (!is_open()) {
                CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id);
                return;
        }
 
        {
                lock_guard lock(mutex);
 
                if (tx_q.size() >= MAX_TXQ_SIZE)
                        throw std::length_error("MAVConnSerial::send_bytes: TX queue overflow");
 
                tx_q.emplace_back(bytes, length);
        }
        io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
}
 
void MAVConnSerial::send_message(const mavlink_message_t *message)
{
        assert(message != nullptr);
 
        if (!is_open()) {
                CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id);
                return;
        }
 
        log_send(PFX, message);
 
        {
                lock_guard lock(mutex);
 
                if (tx_q.size() >= MAX_TXQ_SIZE)
                        throw std::length_error("MAVConnSerial::send_message: TX queue overflow");
 
                tx_q.emplace_back(message);
        }
        io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
}
 
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);
                return;
        }
 
        log_send_obj(PFX, message);
 
        {
                lock_guard lock(mutex);
 
                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, source_compid);
        }
        io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true));
}
 
void MAVConnSerial::do_read(void)
{
        auto sthis = shared_from_this();
        serial_dev.async_read_some(
                        buffer(rx_buf),
                        [sthis] (error_code error, size_t bytes_transferred) {
                                if (error) {
                                        CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str());
                                        sthis->close();
                                        return;
                                }
 
                                sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred);
                                sthis->do_read();
                        });
}
 
void MAVConnSerial::do_write(bool check_tx_state)
{
        if (check_tx_state && tx_in_progress)
                return;
 
        lock_guard lock(mutex);
        if (tx_q.empty())
                return;
 
        tx_in_progress = true;
        auto sthis = shared_from_this();
        auto &buf_ref = tx_q.front();
        serial_dev.async_write_some(
                        buffer(buf_ref.dpos(), buf_ref.nbytes()),
                        [sthis, &buf_ref] (error_code error, size_t bytes_transferred) {
                                assert(bytes_transferred <= buf_ref.len);
 
                                if (error) {
                                        CONSOLE_BRIDGE_logError(PFXd "write: %s", sthis->conn_id, error.message().c_str());
                                        sthis->close();
                                        return;
                                }
 
                                sthis->iostat_tx_add(bytes_transferred);
                                lock_guard lock(sthis->mutex);
 
                                if (sthis->tx_q.empty()) {
                                        sthis->tx_in_progress = false;
                                        return;
                                }
 
                                buf_ref.pos += bytes_transferred;
                                if (buf_ref.nbytes() == 0) {
                                        sthis->tx_q.pop_front();
                                }
 
                                if (!sthis->tx_q.empty())
                                        sthis->do_write(false);
                                else
                                        sthis->tx_in_progress = false;
                        });
}
}       // namespace mavconn