Merge 135ee9a into a8ba9c1

core/udp_connection.cpp

@@ -131,9 +131,19 @@ bool UdpConnection::send_message(const mavlink_message_t &message)
         return false;
     }
 
-    bool send_successful = true;
+    // Some messages have a target system set which allows to send it only
+    // on the matching link.
+    const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(message.msgid);
+    const uint8_t target_system_id =
+        (entry ? reinterpret_cast<const uint8_t *>(message.payload64)[entry->target_system_ofs] :
+                 0);
 
+    bool send_successful = true;
     for (auto &remote : _remotes) {
+        if (target_system_id != 0 && remote.system_id != target_system_id) {
+            continue;
+        }
+
         struct sockaddr_in dest_addr {};
         dest_addr.sin_family = AF_INET;
 
@@ -188,24 +198,41 @@ void UdpConnection::receive()
             continue;
         }
 
-        {
-            std::lock_guard<std::mutex> lock(_remote_mutex);
-
-            Remote new_remote;
-            new_remote.ip = inet_ntoa(src_addr.sin_addr);
-            new_remote.port_number = ntohs(src_addr.sin_port);
-
-            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);
-            }
-        }
-
         _mavlink_receiver->set_new_datagram(buffer, recv_len);
 
+        bool saved_remote = false;
+
         // Parse all mavlink messages in one datagram. Once exhausted, we'll exit while.
         while (_mavlink_receiver->parse_message()) {
+            const uint8_t sysid = _mavlink_receiver->get_last_message().sysid;
+
+            if (!saved_remote && sysid != 0) {
+                saved_remote = true;
+
+                std::lock_guard<std::mutex> lock(_remote_mutex);
+                Remote new_remote;
+                new_remote.ip = inet_ntoa(src_addr.sin_addr);
+                new_remote.port_number = ntohs(src_addr.sin_port);
+                new_remote.system_id = sysid;
+
+                auto existing_remote =
+                    std::find_if(_remotes.begin(), _remotes.end(), [&new_remote](Remote &remote) {
+                        return (remote.ip == new_remote.ip &&
+                                remote.port_number == new_remote.port_number);
+                    });
+
+                if (existing_remote == _remotes.end()) {
+                    LogInfo() << "New system on: " << new_remote.ip << ":"
+                              << new_remote.port_number;
+                    _remotes.push_back(new_remote);
+                } else if (existing_remote->system_id != new_remote.system_id) {
+                    LogWarn() << "System on: " << new_remote.ip << ":" << new_remote.port_number
+                              << " changed system ID (" << int(existing_remote->system_id) << " to "
+                              << int(new_remote.system_id) << ")";
+                    existing_remote->system_id = new_remote.system_id;
+                }
+            }
+
             receive_message(_mavlink_receiver->get_last_message());
         }
     }

core/udp_connection.h

@@ -5,6 +5,7 @@
 #include <thread>
 #include <atomic>
 #include <vector>
+#include <cstdint>
 #include "connection.h"
 
 namespace dronecode_sdk {
@@ -42,6 +43,8 @@ class UdpConnection : public Connection {
         {
             return ip == other.ip && port_number == other.port_number;
         }
+
+        uint8_t system_id{0};
     };
     std::vector<Remote> _remotes{};
 

example/CMakeLists.txt

@@ -10,3 +10,4 @@ add_subdirectory(offboard_velocity)
 add_subdirectory(takeoff_land)
 add_subdirectory(transition_vtol_fixed_wing)
 add_subdirectory(battery)
+add_subdirectory(multiple_drones)

example/multiple_drones/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 2.8.12)
+
+project(multiple_drones)
+
+find_package(Threads REQUIRED)
+
+if(NOT MSVC)
+    add_definitions("-std=c++11 -Wall -Wextra -Werror")
+    # Line below required if /usr/local/include is not in your default includes
+    #include_directories(/usr/local/include)
+    # Line below required if /usr/local/lib is not in your default linker path
+    #link_directories(/usr/local/lib)
+else()
+    add_definitions("-std=c++11 -WX -W2")
+    include_directories(${CMAKE_SOURCE_DIR}/../../install/include)
+    link_directories(${CMAKE_SOURCE_DIR}/../../install/lib)
+endif()
+
+add_executable(multiple_drones
+    multiple_drones.cpp
+)
+
+target_link_libraries(multiple_drones
+    dronecode_sdk
+    dronecode_sdk_telemetry
+    dronecode_sdk_action
+    ${CMAKE_THREAD_LIBS_INIT}
+)
+

example/multiple_drones/multiple_drones.cpp

@@ -0,0 +1,153 @@
+//
+// Example to connect multiple vehicles and make them take off and land in parallel.
+//./multiple_drones udp://:14540 udp://:14541
+//
+// Author: Julian Oes <julian@oes.ch>
+// Author: Shayaan Haider (via Slack)
+
+#include <dronecode_sdk/dronecode_sdk.h>
+#include <dronecode_sdk/plugins/action/action.h>
+#include <dronecode_sdk/plugins/telemetry/telemetry.h>
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <thread>
+
+using namespace dronecode_sdk;
+using namespace std::this_thread;
+using namespace std::chrono;
+
+static void takeoff_land(System &system);
+
+#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
+#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue
+#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour
+
+int main(int argc,
+         char **argv) // The input looks like this ./multiple_drones udp://:14540 udp://:14541
+{
+    if (argc == 1) {
+        std::cout << ERROR_CONSOLE_TEXT << "Please specify connection" << NORMAL_CONSOLE_TEXT
+                  << std::endl;
+        ;
+        return 1;
+    }
+
+    DronecodeSDK dc;
+
+    // the loop below adds the number of ports the sdk monitors.
+    for (int i = 1; i < argc; ++i) {
+        ConnectionResult connection_result = dc.add_any_connection(argv[i]);
+        if (connection_result != ConnectionResult::SUCCESS) {
+            std::cout << ERROR_CONSOLE_TEXT
+                      << "Connection error: " << connection_result_str(connection_result)
+                      << NORMAL_CONSOLE_TEXT << std::endl;
+            ;
+            return 1;
+        }
+    }
+
+    bool discovered_system = false;
+
+    std::cout << "Waiting to discover system..." << std::endl;
+    dc.register_on_discover([&discovered_system](uint64_t uuid) {
+        std::cout << "Discovered system with UUID: " << uuid << std::endl;
+        discovered_system = true;
+    });
+
+    // We usually receive heartbeats at 1Hz, therefore we should find a system after around 2
+    // seconds.
+    sleep_for(seconds(2));
+
+    if (!discovered_system) {
+        std::cout << ERROR_CONSOLE_TEXT << "No system found, exiting." << NORMAL_CONSOLE_TEXT
+                  << std::endl;
+        return 1;
+    }
+
+    std::vector<std::thread> threads;
+
+    for (auto uuid : dc.system_uuids()) {
+        System &system = dc.system(uuid);
+        std::thread t(&takeoff_land, std::ref(system));
+        threads.push_back(std::move(t));
+    }
+
+    for (auto &t : threads) {
+        t.join();
+    }
+    return 0;
+}
+
+void takeoff_land(System &system)
+{
+    auto telemetry = std::make_shared<Telemetry>(system);
+    auto action = std::make_shared<Action>(system);
+
+    // We want to listen to the altitude of the drone at 1 Hz.
+    const Telemetry::Result set_rate_result = telemetry->set_rate_position(1.0);
+
+    if (set_rate_result != Telemetry::Result::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT
+                  << "Setting rate failed:" << Telemetry::result_str(set_rate_result)
+                  << NORMAL_CONSOLE_TEXT << std::endl;
+        return;
+    }
+
+    // Set up callback to monitor altitude while the vehicle is in flight
+    telemetry->position_async([](Telemetry::Position position) {
+        std::cout << TELEMETRY_CONSOLE_TEXT // set to blue
+                  << "Altitude: " << position.relative_altitude_m << " m"
+                  << NORMAL_CONSOLE_TEXT // set to default color again
+                  << std::endl;
+    });
+
+    // Check if vehicle is ready to arm
+    while (telemetry->health_all_ok() != true) {
+        std::cout << "Vehicle is getting ready to arm" << std::endl;
+        sleep_for(seconds(1));
+    }
+
+    // Arm vehicle
+    std::cout << "Arming..." << std::endl;
+    const Action::Result arm_result = action->arm();
+
+    if (arm_result != Action::Result::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT << "Arming failed:" << Action::result_str(arm_result)
+                  << NORMAL_CONSOLE_TEXT << std::endl;
+        // return ;
+    }
+
+    // Take off
+    std::cout << "Taking off..." << std::endl;
+    const Action::Result takeoff_result = action->takeoff();
+    if (takeoff_result != Action::Result::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT << "Takeoff failed:" << Action::result_str(takeoff_result)
+                  << NORMAL_CONSOLE_TEXT << std::endl;
+        // return ;
+    }
+
+    // Let it hover for a bit before landing again.
+    sleep_for(seconds(20));
+
+    std::cout << "Landing..." << std::endl;
+    const Action::Result land_result = action->land();
+    if (land_result != Action::Result::SUCCESS) {
+        std::cout << ERROR_CONSOLE_TEXT << "Land failed:" << Action::result_str(land_result)
+                  << NORMAL_CONSOLE_TEXT << std::endl;
+        // return ;
+    }
+
+    // Check if vehicle is still in air
+    while (telemetry->in_air()) {
+        std::cout << "Vehicle is landing..." << std::endl;
+        sleep_for(seconds(1));
+    }
+    std::cout << "Landed!" << std::endl;
+
+    // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
+
+    sleep_for(seconds(5));
+    std::cout << "Finished..." << std::endl;
+    return;
+}