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