00001
00009
00010
00011
00012
00013
00014
00015
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
00083
00084 if (message->sysid != sysid || message->compid != compid) {
00085 mavlink_message_t msg = *message;
00086
00087 #ifdef MAVLINK2_COMPAT
00088
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
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
00171 host_out = def_host;
00172 }
00173 else {
00174
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
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
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
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
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
00292
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
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
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
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
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 };