00001
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include <set>
00028 #include <cassert>
00029 #include <console_bridge/console.h>
00030
00031 #include <mavconn/interface.h>
00032 #include <mavconn/msgbuffer.h>
00033 #include <mavconn/serial.h>
00034 #include <mavconn/udp.h>
00035 #include <mavconn/tcp.h>
00036
00037 namespace mavconn {
00038
00039 #if MAVLINK_CRC_EXTRA
00040 const uint8_t MAVConnInterface::mavlink_crcs[] = MAVLINK_MESSAGE_CRCS;
00041 #endif
00042 std::set<int> MAVConnInterface::allocated_channels;
00043 std::recursive_mutex MAVConnInterface::channel_mutex;
00044
00045
00046 MAVConnInterface::MAVConnInterface(uint8_t system_id, uint8_t component_id) :
00047 sys_id(system_id),
00048 comp_id(component_id)
00049 {
00050 channel = new_channel();
00051 assert(channel >= 0);
00052 }
00053
00054 int MAVConnInterface::new_channel() {
00055 std::lock_guard<std::recursive_mutex> lock(channel_mutex);
00056 int chan = 0;
00057
00058 for (chan = 0; chan < MAVLINK_COMM_NUM_BUFFERS; chan++) {
00059 if (allocated_channels.count(chan) == 0) {
00060 logDebug("Allocate new channel: %d", chan);
00061 allocated_channels.insert(chan);
00062 return chan;
00063 }
00064 }
00065
00066 logError("channel overrun");
00067 return -1;
00068 }
00069
00070 void MAVConnInterface::delete_channel(int chan) {
00071 std::lock_guard<std::recursive_mutex> lock(channel_mutex);
00072 logDebug("Freeing channel: %d", chan);
00073 allocated_channels.erase(allocated_channels.find(chan));
00074 }
00075
00076 int MAVConnInterface::channes_available() {
00077 std::lock_guard<std::recursive_mutex> lock(channel_mutex);
00078 return MAVLINK_COMM_NUM_BUFFERS - allocated_channels.size();
00079 }
00080
00081 MsgBuffer *MAVConnInterface::new_msgbuffer(const mavlink_message_t *message,
00082 uint8_t sysid, uint8_t compid)
00083 {
00084
00085
00086 if (message->sysid != sysid || message->compid != compid) {
00087 mavlink_message_t msg = *message;
00088
00089 #if MAVLINK_CRC_EXTRA
00090 mavlink_finalize_message_chan(&msg, sysid, compid, channel, message->len,
00091 mavlink_crcs[msg.msgid]);
00092 #else
00093 mavlink_finalize_message_chan(&msg, sysid, compid, channel, message->len);
00094 #endif
00095
00096 return new MsgBuffer(&msg);
00097 }
00098 else
00099 return new MsgBuffer(message);
00100 }
00101
00105 static void url_parse_host(std::string host,
00106 std::string &host_out, int &port_out,
00107 const std::string def_host, const int def_port)
00108 {
00109 std::string port;
00110
00111 auto sep_it = std::find(host.begin(), host.end(), ':');
00112 if (sep_it == host.end()) {
00113
00114 if (!host.empty()) {
00115 host_out = host;
00116 port_out = def_port;
00117 }
00118 else {
00119 host_out = def_host;
00120 port_out = def_port;
00121 }
00122 return;
00123 }
00124
00125 if (sep_it == host.begin()) {
00126
00127 host_out = def_host;
00128 }
00129 else {
00130
00131 host_out.assign(host.begin(), sep_it);
00132 }
00133
00134 port.assign(sep_it + 1, host.end());
00135 port_out = std::stoi(port);
00136 }
00137
00141 static void url_parse_query(std::string query, uint8_t &sysid, uint8_t &compid)
00142 {
00143 const std::string ids_end("ids=");
00144 std::string sys, comp;
00145
00146 if (query.empty())
00147 return;
00148
00149 auto ids_it = std::search(query.begin(), query.end(),
00150 ids_end.begin(), ids_end.end());
00151 if (ids_it == query.end()) {
00152 logWarn("URL: unknown query arguments");
00153 return;
00154 }
00155
00156 std::advance(ids_it, ids_end.length());
00157 auto comma_it = std::find(ids_it, query.end(), ',');
00158 if (comma_it == query.end()) {
00159 logError("URL: no comma in ids= query");
00160 return;
00161 }
00162
00163 sys.assign(ids_it, comma_it);
00164 comp.assign(comma_it + 1, query.end());
00165
00166 sysid = std::stoi(sys);
00167 compid = std::stoi(comp);
00168
00169 logDebug("URL: found system/component id = [%u, %u]", sysid, compid);
00170 }
00171
00172 static MAVConnInterface::Ptr url_parse_serial(
00173 std::string path, std::string query,
00174 uint8_t system_id, uint8_t component_id)
00175 {
00176 std::string file_path;
00177 int baudrate;
00178
00179
00180 url_parse_host(path, file_path, baudrate, "/dev/ttyACM0", 57600);
00181 url_parse_query(query, system_id, component_id);
00182
00183 return boost::make_shared<MAVConnSerial>(system_id, component_id,
00184 file_path, baudrate);
00185 }
00186
00187 static MAVConnInterface::Ptr url_parse_udp(
00188 std::string hosts, std::string query,
00189 uint8_t system_id, uint8_t component_id)
00190 {
00191 std::string bind_pair, remote_pair;
00192 std::string bind_host, remote_host;
00193 int bind_port, remote_port;
00194
00195 auto sep_it = std::find(hosts.begin(), hosts.end(), '@');
00196 if (sep_it == hosts.end()) {
00197 logError("UDP URL should contain @!");
00198 throw DeviceError("url", "UDP separator not found");
00199 }
00200
00201 bind_pair.assign(hosts.begin(), sep_it);
00202 remote_pair.assign(sep_it + 1, hosts.end());
00203
00204
00205 url_parse_host(bind_pair, bind_host, bind_port, "0.0.0.0", 14555);
00206 url_parse_host(remote_pair, remote_host, remote_port, "", 14550);
00207 url_parse_query(query, system_id, component_id);
00208
00209 return boost::make_shared<MAVConnUDP>(system_id, component_id,
00210 bind_host, bind_port,
00211 remote_host, remote_port);
00212 }
00213
00214 static MAVConnInterface::Ptr url_parse_tcp_client(
00215 std::string host, std::string query,
00216 uint8_t system_id, uint8_t component_id)
00217 {
00218 std::string server_host;
00219 int server_port;
00220
00221
00222 url_parse_host(host, server_host, server_port, "localhost", 5760);
00223 url_parse_query(query, system_id, component_id);
00224
00225 return boost::make_shared<MAVConnTCPClient>(system_id, component_id,
00226 server_host, server_port);
00227 }
00228
00229 static MAVConnInterface::Ptr url_parse_tcp_server(
00230 std::string host, std::string query,
00231 uint8_t system_id, uint8_t component_id)
00232 {
00233 std::string bind_host;
00234 int bind_port;
00235
00236
00237 url_parse_host(host, bind_host, bind_port, "0.0.0.0", 5760);
00238 url_parse_query(query, system_id, component_id);
00239
00240 return boost::make_shared<MAVConnTCPServer>(system_id, component_id,
00241 bind_host, bind_port);
00242 }
00243
00244 MAVConnInterface::Ptr MAVConnInterface::open_url(std::string url,
00245 uint8_t system_id, uint8_t component_id) {
00246
00247
00248
00249
00250
00251 const std::string proto_end("://");
00252 std::string proto;
00253 std::string host;
00254 std::string path;
00255 std::string query;
00256
00257 auto proto_it = std::search(
00258 url.begin(), url.end(),
00259 proto_end.begin(), proto_end.end());
00260 if (proto_it == url.end()) {
00261
00262 logDebug("URL: %s: looks like file path", url.c_str());
00263 return url_parse_serial(url, "", system_id, component_id);
00264 }
00265
00266
00267 proto.reserve(std::distance(url.begin(), proto_it));
00268 std::transform(url.begin(), proto_it,
00269 std::back_inserter(proto),
00270 std::ref(tolower));
00271
00272
00273 std::advance(proto_it, proto_end.length());
00274 auto path_it = std::find(proto_it, url.end(), '/');
00275 std::transform(proto_it, path_it,
00276 std::back_inserter(host),
00277 std::ref(tolower));
00278
00279
00280 auto query_it = std::find(path_it, url.end(), '?');
00281 path.assign(path_it, query_it);
00282 if (query_it != url.end())
00283 ++query_it;
00284 query.assign(query_it, url.end());
00285
00286 logDebug("URL: %s: proto: %s, host: %s, path: %s, query: %s",
00287 url.c_str(), proto.c_str(), host.c_str(),
00288 path.c_str(), query.c_str());
00289
00290 if (proto == "udp")
00291 return url_parse_udp(host, query, system_id, component_id);
00292 else if (proto == "tcp")
00293 return url_parse_tcp_client(host, query, system_id, component_id);
00294 else if (proto == "tcp-l")
00295 return url_parse_tcp_server(host, query, system_id, component_id);
00296 else if (proto == "serial")
00297 return url_parse_serial(path, query, system_id, component_id);
00298 else
00299 throw DeviceError("url", "Unknown URL type");
00300 }
00301
00302 };