Merge pull request #1953 from mavlink/pr-add-sniffer

examples: add simple sniffer

examples/CMakeLists.txt

@@ -29,6 +29,7 @@ add_subdirectory(multiple_drones)
 add_subdirectory(offboard)
 add_subdirectory(parachute)
 add_subdirectory(set_actuator)
+add_subdirectory(sniffer)
 add_subdirectory(system_info)
 add_subdirectory(takeoff_and_land)
 add_subdirectory(terminate)

examples/sniffer/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(sniffer)
+
+add_executable(sniffer
+    sniffer.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(sniffer
+    MAVSDK::mavsdk
+)
+
+if(NOT MSVC)
+    add_compile_options(sniffer PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(sniffer PRIVATE -WX -W2)
+endif()

examples/sniffer/sniffer.cpp

@@ -0,0 +1,86 @@
+//
+// Example how to print every message arriving.
+//
+
+#include <cstdint>
+#include <future>
+#include <mavsdk/mavsdk.h>
+#include <iostream>
+#include <thread>
+
+using namespace mavsdk;
+using namespace std::this_thread;
+using namespace std::chrono;
+
+void usage(const std::string& bin_name)
+{
+    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
+              << "Connection URL format should be :\n"
+              << " For TCP : tcp://[server_host][:server_port]\n"
+              << " For UDP : udp://[bind_host][:bind_port]\n"
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
+              << "For example, to connect to the simulator use URL: udp://:14540\n";
+}
+
+std::shared_ptr<System> get_system(Mavsdk& mavsdk)
+{
+    std::cout << "Waiting to discover system...\n";
+    auto prom = std::promise<std::shared_ptr<System>>{};
+    auto fut = prom.get_future();
+
+    // We wait for new systems to be discovered, once we find one that has an
+    // autopilot, we decide to use it.
+    Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&mavsdk, &prom, &handle]() {
+        auto system = mavsdk.systems().back();
+
+        if (system->has_autopilot()) {
+            std::cout << "Discovered autopilot\n";
+
+            // Unsubscribe again as we only want to find one system.
+            mavsdk.unsubscribe_on_new_system(handle);
+            prom.set_value(system);
+        }
+    });
+
+    // We usually receive heartbeats at 1Hz, therefore we should find a
+    // system after around 3 seconds max, surely.
+    if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
+        std::cerr << "No autopilot found.\n";
+        return {};
+    }
+
+    // Get discovered system now.
+    return fut.get();
+}
+
+int main(int argc, char** argv)
+{
+    if (argc != 2) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    Mavsdk mavsdk;
+    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    auto system = get_system(mavsdk);
+    if (!system) {
+        return 1;
+    }
+
+    mavsdk.intercept_incoming_messages_async([](mavlink_message_t& message) {
+        std::cout << "Got message " << (int)message.msgid << '\n';
+        return true;
+    });
+
+    while (true) {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+
+    return 0;
+}