diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index edfeb74d7..183a27865 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -36,6 +36,7 @@ add_subdirectory(mavshell) add_subdirectory(multiple_drones) add_subdirectory(offboard) add_subdirectory(parachute) +add_subdirectory(params) add_subdirectory(set_actuator) add_subdirectory(sniffer) add_subdirectory(system_info) diff --git a/examples/params/CMakeLists.txt b/examples/params/CMakeLists.txt new file mode 100644 index 000000000..895859299 --- /dev/null +++ b/examples/params/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(params) + +add_executable(params + params.cpp +) + +find_package(MAVSDK REQUIRED) + +target_link_libraries(params + MAVSDK::mavsdk +) + +if(NOT MSVC) + add_compile_options(params PRIVATE -Wall -Wextra) +else() + add_compile_options(params PRIVATE -W2) +endif() diff --git a/examples/params/params.cpp b/examples/params/params.cpp new file mode 100644 index 000000000..9f4c7731a --- /dev/null +++ b/examples/params/params.cpp @@ -0,0 +1,86 @@ +// +// Simple example to demonstrate how to get and set params. +// + +#include +#include +#include +#include +#include +#include + +using namespace mavsdk; + +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: udpin://0.0.0.0:14540\n"; +} + +int main(int argc, char** argv) +{ + if (argc != 2) { + usage(argv[0]); + return 1; + } + + const std::string connection_url = argv[1]; + + Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; + const ConnectionResult connection_result = mavsdk.add_any_connection(connection_url); + + if (connection_result != ConnectionResult::Success) { + std::cerr << "Connection failed: " << connection_result << '\n'; + return 1; + } + + 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(std::chrono::seconds(3)) == std::future_status::timeout) { + std::cerr << "No autopilot found, exiting.\n"; + return 1; + } + + // Get discovered system now. + auto system = fut.get(); + + // Instantiate plugins. + auto param = Param{system}; + + // Print params once. + const auto get_all_result = param.get_all_params(); + + std::cout << "Int params: \n"; + for (auto p : get_all_result.int_params) { + std::cout << p.name << ": " << p.value << '\n'; + } + + std::cout << "Float params: \n"; + for (auto p : get_all_result.float_params) { + std::cout << p.name << ": " << p.value << '\n'; + } + + return 0; +}