From 218ecae35e0c8f9fe142a4d3838f5ec2c6fc9178 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 17 Aug 2018 14:11:30 +0200 Subject: [PATCH] core: more than one vehicle on one UDP port This adds support to command multiple drones that connect e.g. on the UDP port 14540. --- core/dronecode_sdk_impl.cpp | 15 +------ core/udp_connection.cpp | 81 +++++++++++++++---------------------- core/udp_connection.h | 13 +++++- 3 files changed, 44 insertions(+), 65 deletions(-) diff --git a/core/dronecode_sdk_impl.cpp b/core/dronecode_sdk_impl.cpp index 395846f70e..6bea251d8b 100644 --- a/core/dronecode_sdk_impl.cpp +++ b/core/dronecode_sdk_impl.cpp @@ -74,10 +74,6 @@ void DronecodeSDKImpl::receive_message(const mavlink_message_t &message) return; } - if (message.sysid != 1) { - LogDebug() << "sysid: " << int(message.sysid); - } - if (_systems.find(message.sysid) != _systems.end()) { _systems.at(message.sysid)->system_impl()->process_mavlink_message(message); } @@ -218,12 +214,6 @@ System &DronecodeSDKImpl::get_system() if (_systems.size() > 1) { LogErr() << "Error: more than one system found:"; - int i = 0; - for (auto it = _systems.begin(); it != _systems.end(); ++it) { - LogWarn() << "strange: " << i << ": " << int(it->first) << ", " << it->second; - ++i; - } - // Just return first system instead of failing. return *_systems.begin()->second; } else { @@ -262,10 +252,7 @@ bool DronecodeSDKImpl::is_connected() const { std::lock_guard lock(_systems_mutex); - if (_systems.size() == 1) { - return _systems.begin()->second->is_connected(); - } - return false; + return _systems.begin()->second->is_connected(); } bool DronecodeSDKImpl::is_connected(const uint64_t uuid) const diff --git a/core/udp_connection.cpp b/core/udp_connection.cpp index b7ac1e2bdb..9d407b3fad 100644 --- a/core/udp_connection.cpp +++ b/core/udp_connection.cpp @@ -16,6 +16,7 @@ #endif #include +#include #ifndef WINDOWS #define GET_ERROR(_x) strerror(_x) @@ -123,47 +124,40 @@ ConnectionResult UdpConnection::stop() bool UdpConnection::send_message(const mavlink_message_t &message) { - struct sockaddr_in dest_addr {}; + std::lock_guard lock(_remote_mutex); - { - std::lock_guard lock(_remote_mutex); + if (_remotes.size() == 0) { + LogErr() << "No known remotes"; + return false; + } - if (_remote_ip.empty()) { - LogErr() << "Remote IP unknown"; - return false; - } - - if (_remote_port_number == 0) { - LogErr() << "Remote port unknown"; - return false; - } + bool send_successful = true; + for (auto &remote : _remotes) { + struct sockaddr_in dest_addr {}; dest_addr.sin_family = AF_INET; - inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr); - - dest_addr.sin_port = htons(_remote_port_number); - } - - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message); + inet_pton(AF_INET, remote.ip.c_str(), &dest_addr.sin_addr.s_addr); + dest_addr.sin_port = htons(remote.port_number); - // TODO: remove this assert again - assert(buffer_len <= MAVLINK_MAX_PACKET_LEN); + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message); - int send_len = sendto(_socket_fd, - reinterpret_cast(buffer), - buffer_len, - 0, - reinterpret_cast(&dest_addr), - sizeof(dest_addr)); + int send_len = sendto(_socket_fd, + reinterpret_cast(buffer), + buffer_len, + 0, + reinterpret_cast(&dest_addr), + sizeof(dest_addr)); - if (send_len != buffer_len) { - LogErr() << "sendto failure: " << GET_ERROR(errno); - return false; + if (send_len != buffer_len) { + LogErr() << "sendto failure: " << GET_ERROR(errno); + send_successful = false; + continue; + } } - return true; + return send_successful; } void UdpConnection::receive() @@ -197,25 +191,14 @@ void UdpConnection::receive() { std::lock_guard lock(_remote_mutex); - int new_remote_port_number = ntohs(src_addr.sin_port); - std::string new_remote_ip(inet_ntoa(src_addr.sin_addr)); - - if (_remote_ip.empty() || _remote_port_number == 0) { - // Set IP if we don't know it yet. - _remote_ip = new_remote_ip; - _remote_port_number = new_remote_port_number; - - LogInfo() << "New device on: " << _remote_ip << ":" << _remote_port_number; - - } else if (_remote_ip.compare(new_remote_ip) != 0 || - _remote_port_number != new_remote_port_number) { - // It is possible that wifi disconnects and a device might get a new - // IP and/or UDP port. - _remote_ip = new_remote_ip; - _remote_port_number = new_remote_port_number; + Remote new_remote; + new_remote.ip = inet_ntoa(src_addr.sin_addr); + new_remote.port_number = ntohs(src_addr.sin_port); - LogInfo() << "Device changed to: " << new_remote_ip << ":" - << new_remote_port_number; + auto found = std::find(_remotes.begin(), _remotes.end(), new_remote); + if (found == _remotes.end()) { + LogInfo() << "New device on: " << new_remote.ip << ":" << new_remote.port_number; + _remotes.push_back(new_remote); } } diff --git a/core/udp_connection.h b/core/udp_connection.h index e610c92dc0..3650ea9c54 100644 --- a/core/udp_connection.h +++ b/core/udp_connection.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "connection.h" namespace dronecode_sdk { @@ -33,8 +34,16 @@ class UdpConnection : public Connection { int _local_port_number; std::mutex _remote_mutex{}; - std::string _remote_ip{}; - int _remote_port_number{0}; + struct Remote { + std::string ip{}; + int port_number{0}; + + bool operator==(const UdpConnection::Remote &other) + { + return ip == other.ip && port_number == other.port_number; + } + }; + std::vector _remotes{}; int _socket_fd{-1}; std::thread *_recv_thread{nullptr};