diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b4abfd8245..58888052ca 100644 --- a/examples/CMakeLists.txt +++ b/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) diff --git a/examples/sniffer/CMakeLists.txt b/examples/sniffer/CMakeLists.txt new file mode 100644 index 0000000000..4f3bd9bcc3 --- /dev/null +++ b/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() diff --git a/examples/sniffer/sniffer.cpp b/examples/sniffer/sniffer.cpp new file mode 100644 index 0000000000..311ee75c68 --- /dev/null +++ b/examples/sniffer/sniffer.cpp @@ -0,0 +1,86 @@ +// +// Example how to print every message arriving. +// + +#include +#include +#include +#include +#include + +using namespace mavsdk; +using namespace std::this_thread; +using namespace std::chrono; + +void usage(const std::string& bin_name) +{ + std::cerr << "Usage : " << bin_name << " \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 get_system(Mavsdk& mavsdk) +{ + std::cout << "Waiting to discover system...\n"; + auto prom = std::promise>{}; + 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; +}