interface.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * libmavconn
00011  * Copyright 2013,2014,2015 Vladimir Ermakov, All rights reserved.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 
00018 #include <set>
00019 #include <cassert>
00020 #include <console_bridge/console.h>
00021 
00022 #include <mavconn/interface.h>
00023 #include <mavconn/msgbuffer.h>
00024 #include <mavconn/serial.h>
00025 #include <mavconn/udp.h>
00026 #include <mavconn/tcp.h>
00027 
00028 namespace mavconn {
00029 
00030 #define PFX     "mavconn: "
00031 
00032 #if MAVLINK_CRC_EXTRA
00033 const uint8_t MAVConnInterface::mavlink_crcs[] = MAVLINK_MESSAGE_CRCS;
00034 #endif
00035 std::set<int> MAVConnInterface::allocated_channels;
00036 std::recursive_mutex MAVConnInterface::channel_mutex;
00037 
00038 
00039 MAVConnInterface::MAVConnInterface(uint8_t system_id, uint8_t component_id) :
00040         sys_id(system_id),
00041         comp_id(component_id),
00042         tx_total_bytes(0),
00043         rx_total_bytes(0),
00044         last_tx_total_bytes(0),
00045         last_rx_total_bytes(0),
00046         last_iostat(steady_clock::now())
00047 {
00048         channel = new_channel();
00049         assert(channel >= 0);
00050 }
00051 
00052 int MAVConnInterface::new_channel() {
00053         std::lock_guard<std::recursive_mutex> lock(channel_mutex);
00054         int chan = 0;
00055 
00056         for (chan = 0; chan < MAVLINK_COMM_NUM_BUFFERS; chan++) {
00057                 if (allocated_channels.count(chan) == 0) {
00058                         logDebug(PFX "Allocate new channel: %d", chan);
00059                         allocated_channels.insert(chan);
00060                         return chan;
00061                 }
00062         }
00063 
00064         logError(PFX "channel overrun");
00065         return -1;
00066 }
00067 
00068 void MAVConnInterface::delete_channel(int chan) {
00069         std::lock_guard<std::recursive_mutex> lock(channel_mutex);
00070         logDebug(PFX "Freeing channel: %d", chan);
00071         allocated_channels.erase(allocated_channels.find(chan));
00072 }
00073 
00074 int MAVConnInterface::channes_available() {
00075         std::lock_guard<std::recursive_mutex> lock(channel_mutex);
00076         return MAVLINK_COMM_NUM_BUFFERS - allocated_channels.size();
00077 }
00078 
00079 MsgBuffer *MAVConnInterface::new_msgbuffer(const mavlink_message_t *message,
00080                 uint8_t sysid, uint8_t compid)
00081 {
00082         /* if sysid/compid pair not match we need explicit finalize
00083          * else just copy to buffer */
00084         if (message->sysid != sysid || message->compid != compid) {
00085                 mavlink_message_t msg = *message;
00086 
00087 #ifdef MAVLINK2_COMPAT
00088                 // for mavlink 1.0 len == min_len
00089                 mavlink_finalize_message_chan(&msg, sysid, compid, channel, message->len, message->len,
00090                                 mavlink_crcs[msg.msgid]);
00091 #else
00092 # if MAVLINK_CRC_EXTRA
00093                 mavlink_finalize_message_chan(&msg, sysid, compid, channel, message->len,
00094                                 mavlink_crcs[msg.msgid]);
00095 # else
00096                 mavlink_finalize_message_chan(&msg, sysid, compid, channel, message->len);
00097 # endif
00098 #endif
00099 
00100                 return new MsgBuffer(&msg);
00101         }
00102         else
00103                 return new MsgBuffer(message);
00104 }
00105 
00106 mavlink_status_t MAVConnInterface::get_status()
00107 {
00108         return *mavlink_get_channel_status(channel);
00109 }
00110 
00111 MAVConnInterface::IOStat MAVConnInterface::get_iostat()
00112 {
00113         std::lock_guard<std::recursive_mutex> lock(iostat_mutex);
00114         IOStat stat;
00115 
00116         stat.tx_total_bytes = tx_total_bytes;
00117         stat.rx_total_bytes = rx_total_bytes;
00118 
00119         auto d_tx = stat.tx_total_bytes - last_tx_total_bytes;
00120         auto d_rx = stat.rx_total_bytes - last_rx_total_bytes;
00121         last_tx_total_bytes = stat.tx_total_bytes;
00122         last_rx_total_bytes = stat.rx_total_bytes;
00123 
00124         auto now = steady_clock::now();
00125         auto dt = now - last_iostat;
00126         last_iostat = now;
00127 
00128         float dt_s = std::chrono::duration_cast<std::chrono::seconds>(dt).count();
00129 
00130         stat.tx_speed = d_tx / dt_s;
00131         stat.rx_speed = d_rx / dt_s;
00132 
00133         return stat;
00134 }
00135 
00136 void MAVConnInterface::iostat_tx_add(size_t bytes)
00137 {
00138         tx_total_bytes += bytes;
00139 }
00140 
00141 void MAVConnInterface::iostat_rx_add(size_t bytes)
00142 {
00143         rx_total_bytes += bytes;
00144 }
00145 
00149 static void url_parse_host(std::string host,
00150                 std::string &host_out, int &port_out,
00151                 const std::string def_host, const int def_port)
00152 {
00153         std::string port;
00154 
00155         auto sep_it = std::find(host.begin(), host.end(), ':');
00156         if (sep_it == host.end()) {
00157                 // host
00158                 if (!host.empty()) {
00159                         host_out = host;
00160                         port_out = def_port;
00161                 }
00162                 else {
00163                         host_out = def_host;
00164                         port_out = def_port;
00165                 }
00166                 return;
00167         }
00168 
00169         if (sep_it == host.begin()) {
00170                 // :port
00171                 host_out = def_host;
00172         }
00173         else {
00174                 // host:port
00175                 host_out.assign(host.begin(), sep_it);
00176         }
00177 
00178         port.assign(sep_it + 1, host.end());
00179         port_out = std::stoi(port);
00180 }
00181 
00185 static void url_parse_query(std::string query, uint8_t &sysid, uint8_t &compid)
00186 {
00187         const std::string ids_end("ids=");
00188         std::string sys, comp;
00189 
00190         if (query.empty())
00191                 return;
00192 
00193         auto ids_it = std::search(query.begin(), query.end(),
00194                         ids_end.begin(), ids_end.end());
00195         if (ids_it == query.end()) {
00196                 logWarn(PFX "URL: unknown query arguments");
00197                 return;
00198         }
00199 
00200         std::advance(ids_it, ids_end.length());
00201         auto comma_it = std::find(ids_it, query.end(), ',');
00202         if (comma_it == query.end()) {
00203                 logError(PFX "URL: no comma in ids= query");
00204                 return;
00205         }
00206 
00207         sys.assign(ids_it, comma_it);
00208         comp.assign(comma_it + 1, query.end());
00209 
00210         sysid = std::stoi(sys);
00211         compid = std::stoi(comp);
00212 
00213         logDebug(PFX "URL: found system/component id = [%u, %u]", sysid, compid);
00214 }
00215 
00216 static MAVConnInterface::Ptr url_parse_serial(
00217                 std::string path, std::string query,
00218                 uint8_t system_id, uint8_t component_id)
00219 {
00220         std::string file_path;
00221         int baudrate;
00222 
00223         // /dev/ttyACM0:57600
00224         url_parse_host(path, file_path, baudrate, "/dev/ttyACM0", 57600);
00225         url_parse_query(query, system_id, component_id);
00226 
00227         return boost::make_shared<MAVConnSerial>(system_id, component_id,
00228                         file_path, baudrate);
00229 }
00230 
00231 static MAVConnInterface::Ptr url_parse_udp(
00232                 std::string hosts, std::string query,
00233                 uint8_t system_id, uint8_t component_id)
00234 {
00235         std::string bind_pair, remote_pair;
00236         std::string bind_host, remote_host;
00237         int bind_port, remote_port;
00238 
00239         auto sep_it = std::find(hosts.begin(), hosts.end(), '@');
00240         if (sep_it == hosts.end()) {
00241                 logError(PFX "UDP URL should contain @!");
00242                 throw DeviceError("url", "UDP separator not found");
00243         }
00244 
00245         bind_pair.assign(hosts.begin(), sep_it);
00246         remote_pair.assign(sep_it + 1, hosts.end());
00247 
00248         // udp://0.0.0.0:14555@:14550
00249         url_parse_host(bind_pair, bind_host, bind_port, "0.0.0.0", 14555);
00250         url_parse_host(remote_pair, remote_host, remote_port, "", 14550);
00251         url_parse_query(query, system_id, component_id);
00252 
00253         return boost::make_shared<MAVConnUDP>(system_id, component_id,
00254                         bind_host, bind_port,
00255                         remote_host, remote_port);
00256 }
00257 
00258 static MAVConnInterface::Ptr url_parse_tcp_client(
00259                 std::string host, std::string query,
00260                 uint8_t system_id, uint8_t component_id)
00261 {
00262         std::string server_host;
00263         int server_port;
00264 
00265         // tcp://localhost:5760
00266         url_parse_host(host, server_host, server_port, "localhost", 5760);
00267         url_parse_query(query, system_id, component_id);
00268 
00269         return boost::make_shared<MAVConnTCPClient>(system_id, component_id,
00270                         server_host, server_port);
00271 }
00272 
00273 static MAVConnInterface::Ptr url_parse_tcp_server(
00274                 std::string host, std::string query,
00275                 uint8_t system_id, uint8_t component_id)
00276 {
00277         std::string bind_host;
00278         int bind_port;
00279 
00280         // tcp-l://0.0.0.0:5760
00281         url_parse_host(host, bind_host, bind_port, "0.0.0.0", 5760);
00282         url_parse_query(query, system_id, component_id);
00283 
00284         return boost::make_shared<MAVConnTCPServer>(system_id, component_id,
00285                         bind_host, bind_port);
00286 }
00287 
00288 MAVConnInterface::Ptr MAVConnInterface::open_url(std::string url,
00289                 uint8_t system_id, uint8_t component_id) {
00290 
00291         /* Based on code found here:
00292          * http://stackoverflow.com/questions/2616011/easy-way-to-parse-a-url-in-c-cross-platform
00293          */
00294 
00295         const std::string proto_end("://");
00296         std::string proto;
00297         std::string host;
00298         std::string path;
00299         std::string query;
00300 
00301         auto proto_it = std::search(
00302                         url.begin(), url.end(),
00303                         proto_end.begin(), proto_end.end());
00304         if (proto_it == url.end()) {
00305                 // looks like file path
00306                 logDebug(PFX "URL: %s: looks like file path", url.c_str());
00307                 return url_parse_serial(url, "", system_id, component_id);
00308         }
00309 
00310         // copy protocol
00311         proto.reserve(std::distance(url.begin(), proto_it));
00312         std::transform(url.begin(), proto_it,
00313                         std::back_inserter(proto),
00314                         std::ref(tolower));
00315 
00316         // copy host
00317         std::advance(proto_it, proto_end.length());
00318         auto path_it = std::find(proto_it, url.end(), '/');
00319         std::transform(proto_it, path_it,
00320                         std::back_inserter(host),
00321                         std::ref(tolower));
00322 
00323         // copy path, and query if exists
00324         auto query_it = std::find(path_it, url.end(), '?');
00325         path.assign(path_it, query_it);
00326         if (query_it != url.end())
00327                 ++query_it;
00328         query.assign(query_it, url.end());
00329 
00330         logDebug(PFX "URL: %s: proto: %s, host: %s, path: %s, query: %s",
00331                         url.c_str(), proto.c_str(), host.c_str(),
00332                         path.c_str(), query.c_str());
00333 
00334         if (proto == "udp")
00335                 return url_parse_udp(host, query, system_id, component_id);
00336         else if (proto == "tcp")
00337                 return url_parse_tcp_client(host, query, system_id, component_id);
00338         else if (proto == "tcp-l")
00339                 return url_parse_tcp_server(host, query, system_id, component_id);
00340         else if (proto == "serial")
00341                 return url_parse_serial(path, query, system_id, component_id);
00342         else
00343                 throw DeviceError("url", "Unknown URL type");
00344 }
00345 
00346 }; // namespace mavconn


libmavconn
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:13