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

@@ -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<std::recursive_mutex> 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

core/udp_connection.cpp

@@ -16,6 +16,7 @@
 #endif
 
 #include <cassert>
+#include <algorithm>
 
 #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<std::mutex> lock(_remote_mutex);
 
-    {
-        std::lock_guard<std::mutex> 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<char *>(buffer),
-                          buffer_len,
-                          0,
-                          reinterpret_cast<const sockaddr *>(&dest_addr),
-                          sizeof(dest_addr));
+        int send_len = sendto(_socket_fd,
+                              reinterpret_cast<char *>(buffer),
+                              buffer_len,
+                              0,
+                              reinterpret_cast<const sockaddr *>(&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<std::mutex> 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);
             }
         }
 

core/udp_connection.h

@@ -4,6 +4,7 @@
 #include <mutex>
 #include <thread>
 #include <atomic>
+#include <vector>
 #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<Remote> _remotes{};
 
     int _socket_fd{-1};
     std::thread *_recv_thread{nullptr};