Go to the documentation of this file.00001
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #pragma once
00023
00024 #include <boost/bind.hpp>
00025 #include <boost/signals2.hpp>
00026 #include <boost/smart_ptr.hpp>
00027 #include <boost/make_shared.hpp>
00028 #include <boost/system/system_error.hpp>
00029
00030 #include <set>
00031 #include <mutex>
00032 #include <atomic>
00033 #include <chrono>
00034 #include <thread>
00035 #include <memory>
00036 #include <sstream>
00037 #include <stdexcept>
00038 #include <mavconn/mavlink_dialect.h>
00039
00040 namespace mavconn {
00041 namespace sig2 = boost::signals2;
00042
00043 class MsgBuffer;
00044
00045 #if __cplusplus == 201103L
00046 using steady_clock = std::chrono::steady_clock;
00047 #elif defined(__GXX_EXPERIMENTAL_CXX0X__)
00048 typedef std::chrono::monotonic_clock steady_clock;
00049 #else
00050 #error Unknown C++11 or C++0x wall clock class
00051 #endif
00052
00056 class DeviceError : public std::runtime_error {
00057 public:
00061 template <typename T>
00062 DeviceError(const char *module, T msg) :
00063 std::runtime_error(make_message(module, msg))
00064 { };
00065
00066 template <typename T>
00067 static std::string make_message(const char *module, T msg) {
00068 std::ostringstream ss;
00069 ss << "DeviceError:" << module << ":" << msg_to_string(msg);
00070 return ss.str();
00071 }
00072
00073 static std::string msg_to_string(const char *description) {
00074 return description;
00075 }
00076
00077 static std::string msg_to_string(int errnum) {
00078 return strerror(errnum);
00079 }
00080
00081 static std::string msg_to_string(boost::system::system_error &err) {
00082 return err.what();
00083 }
00084 };
00085
00089 class MAVConnInterface {
00090 private:
00091 MAVConnInterface(const MAVConnInterface&) = delete;
00092
00093 public:
00094 typedef sig2::signal<void(const mavlink_message_t *message, uint8_t system_id, uint8_t component_id)> MessageSig;
00095 typedef boost::shared_ptr<MAVConnInterface> Ptr;
00096 typedef boost::shared_ptr<MAVConnInterface const> ConstPtr;
00097 typedef boost::weak_ptr<MAVConnInterface> WeakPtr;
00098
00099 struct IOStat {
00100 size_t tx_total_bytes;
00101 size_t rx_total_bytes;
00102 float tx_speed;
00103 float rx_speed;
00104 };
00105
00110 MAVConnInterface(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE);
00111 virtual ~MAVConnInterface() {
00112 delete_channel(channel);
00113 };
00114
00118 virtual void close() = 0;
00119
00123 inline void send_message(const mavlink_message_t *message) {
00124 send_message(message, sys_id, comp_id);
00125 };
00126
00135 virtual void send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid) = 0;
00136
00140 virtual void send_bytes(const uint8_t *bytes, size_t length) = 0;
00141
00145 MessageSig message_received;
00146 sig2::signal<void()> port_closed;
00147
00148 virtual mavlink_status_t get_status();
00149 virtual IOStat get_iostat();
00150 virtual bool is_open() = 0;
00151
00152 inline int get_channel() { return channel; };
00153 inline uint8_t get_system_id() { return sys_id; };
00154 inline void set_system_id(uint8_t sysid) { sys_id = sysid; };
00155 inline uint8_t get_component_id() { return comp_id; };
00156 inline void set_component_id(uint8_t compid) { comp_id = compid; };
00157
00175 static Ptr open_url(std::string url,
00176 uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE);
00177
00178 protected:
00179 int channel;
00180 uint8_t sys_id;
00181 uint8_t comp_id;
00182
00183 #if MAVLINK_CRC_EXTRA
00184 static const uint8_t mavlink_crcs[];
00185 #endif
00186
00187 static int new_channel();
00188 static void delete_channel(int chan);
00189 static int channes_available();
00190
00194 MsgBuffer *new_msgbuffer(const mavlink_message_t *message, uint8_t sysid, uint8_t compid);
00195
00196 void iostat_tx_add(size_t bytes);
00197 void iostat_rx_add(size_t bytes);
00198
00199 private:
00200 static std::recursive_mutex channel_mutex;
00201 static std::set<int> allocated_channels;
00202
00203 std::atomic<size_t> tx_total_bytes, rx_total_bytes;
00204 std::recursive_mutex iostat_mutex;
00205 size_t last_tx_total_bytes, last_rx_total_bytes;
00206 std::chrono::time_point<steady_clock> last_iostat;
00207 };
00208
00209 };