interface.h

Go to the documentation of this file.

/*
 * libmavconn
 * Copyright 2013,2014,2015,2016 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
 */

#pragma once

#include <boost/system/system_error.hpp>

#include <deque>
#include <mutex>
#include <vector>
#include <atomic>
#include <chrono>
#include <thread>
#include <memory>
#include <sstream>
#include <cassert>
#include <stdexcept>
#include <unordered_map>
#include <mavconn/mavlink_dialect.h>


namespace mavconn {
using steady_clock = std::chrono::steady_clock;
using lock_guard = std::lock_guard<std::recursive_mutex>;

static constexpr auto MAV_COMP_ID_UDP_BRIDGE = 240;

enum class Framing : uint8_t {
        incomplete = mavlink::MAVLINK_FRAMING_INCOMPLETE,
        ok = mavlink::MAVLINK_FRAMING_OK,
        bad_crc = mavlink::MAVLINK_FRAMING_BAD_CRC,
        bad_signature = mavlink::MAVLINK_FRAMING_BAD_SIGNATURE,
};

enum class Protocol : uint8_t {
        V10 = 1,        
        V20 = 2         
};

class DeviceError : public std::runtime_error {
public:
        template <typename T>
        DeviceError(const char *module, T msg) :
                std::runtime_error(make_message(module, msg))
        { }

        template <typename T>
        static std::string make_message(const char *module, T msg) {
                std::ostringstream ss;
                ss << "DeviceError:" << module << ":" << msg_to_string(msg);
                return ss.str();
        }

        static std::string msg_to_string(const char *description) {
                return description;
        }

        static std::string msg_to_string(int errnum) {
                return ::strerror(errnum);
        }

        static std::string msg_to_string(boost::system::system_error &err) {
                return err.what();
        }
};

class MAVConnInterface {
private:
        MAVConnInterface(const MAVConnInterface&) = delete;

public:
        using ReceivedCb = std::function<void (const mavlink::mavlink_message_t *message, const Framing framing)>;
        using ClosedCb = std::function<void (void)>;
        using Ptr = std::shared_ptr<MAVConnInterface>;
        using ConstPtr = std::shared_ptr<MAVConnInterface const>;
        using WeakPtr = std::weak_ptr<MAVConnInterface>;

        struct IOStat {
                size_t tx_total_bytes;  
                size_t rx_total_bytes;  
                float tx_speed;         
                float rx_speed;         
        };

        MAVConnInterface(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE);

        virtual void connect(
                        const ReceivedCb &cb_handle_message,
                        const ClosedCb &cb_handle_closed_port = ClosedCb()) = 0;

        virtual void close() = 0;

        virtual void send_message(const mavlink::mavlink_message_t *message) = 0;

        virtual void send_message(const mavlink::Message &message) {
                send_message(message, this->comp_id);
        }

        virtual void send_message(const mavlink::Message &message, const uint8_t src_compid) = 0;

        virtual void send_bytes(const uint8_t *bytes, size_t length) = 0;

        void send_message_ignore_drop(const mavlink::mavlink_message_t *message);

        void send_message_ignore_drop(const mavlink::Message &message) {
                send_message_ignore_drop(message, this->comp_id);
        }

        void send_message_ignore_drop(const mavlink::Message &message, const uint8_t src_compid);

        ReceivedCb message_received_cb;
        ClosedCb port_closed_cb;

        virtual mavlink::mavlink_status_t get_status();
        virtual IOStat get_iostat();
        virtual bool is_open() = 0;

        inline uint8_t get_system_id() {
                return sys_id;
        }
        inline void set_system_id(uint8_t sysid) {
                sys_id = sysid;
        }
        inline uint8_t get_component_id() {
                return comp_id;
        }
        inline void set_component_id(uint8_t compid) {
                comp_id = compid;
        }

        void set_protocol_version(Protocol pver);
        Protocol get_protocol_version();

        static Ptr open_url(
                        std::string url,
                        uint8_t system_id = 1,
                        uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE,
                        const ReceivedCb &cb_handle_message = ReceivedCb(),
                        const ClosedCb &cb_handle_closed_port = ClosedCb());

        static Ptr open_url_no_connect(
                        std::string url,
                        uint8_t system_id = 1,
                        uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE);

        static std::vector<std::string> get_known_dialects();

protected:
        uint8_t sys_id;         
        uint8_t comp_id;        

        static constexpr size_t MAX_PACKET_SIZE = MAVLINK_MAX_PACKET_LEN + 16;
        static constexpr size_t MAX_TXQ_SIZE = 1000;

        static std::unordered_map<mavlink::msgid_t, const mavlink::mavlink_msg_entry_t*> message_entries;

        size_t conn_id;

        inline mavlink::mavlink_status_t *get_status_p() {
                return &m_parse_status;
        }

        inline mavlink::mavlink_message_t *get_buffer_p() {
                return &m_buffer;
        }

        void parse_buffer(const char *pfx, uint8_t *buf, const size_t bufsize, size_t bytes_received);

        void iostat_tx_add(size_t bytes);
        void iostat_rx_add(size_t bytes);

        void log_recv(const char *pfx, mavlink::mavlink_message_t &msg, Framing framing);
        void log_send(const char *pfx, const mavlink::mavlink_message_t *msg);
        void log_send_obj(const char *pfx, const mavlink::Message &msg);

private:
        friend const mavlink::mavlink_msg_entry_t* mavlink::mavlink_get_msg_entry(uint32_t msgid);

        mavlink::mavlink_status_t m_parse_status;
        mavlink::mavlink_message_t m_buffer;
        mavlink::mavlink_status_t m_mavlink_status;

        std::atomic<size_t> tx_total_bytes, rx_total_bytes;
        std::recursive_mutex iostat_mutex;
        size_t last_tx_total_bytes, last_rx_total_bytes;
        std::chrono::time_point<steady_clock> last_iostat;

        static std::atomic<size_t> conn_id_counter;

        static std::once_flag init_flag;

        static void init_msg_entry();
};
}       // namespace mavconn