Program Listing for File interface.hpp

↰ Return to documentation for file (/tmp/ws/src/mavros/libmavconn/include/mavconn/interface.hpp)

//
// libmavconn
// Copyright 2013,2014,2015,2016,2021 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
#ifndef MAVCONN__INTERFACE_HPP_
#define MAVCONN__INTERFACE_HPP_

#include <atomic>
#include <cassert>
#include <chrono>
#include <deque>
#include <functional>
#include <memory>
#include <mutex>
#include <sstream>
#include <stdexcept>
#include <string>
#include <system_error>
#include <thread>
#include <unordered_map>
#include <vector>

#include <mavconn/mavlink_dialect.hpp>

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(std::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;
  };

  explicit 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

#endif  // MAVCONN__INTERFACE_HPP_