From 98f8b5300554645f2fe560f56e09a1e920b80d86 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 6 Nov 2018 13:47:48 +0100 Subject: [PATCH 1/2] core: remove confusing warning We don't need to warn for this case because it's ok to just get the dummy system. --- core/dronecode_sdk_impl.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/core/dronecode_sdk_impl.cpp b/core/dronecode_sdk_impl.cpp index 4c2a84d342..d561293872 100644 --- a/core/dronecode_sdk_impl.cpp +++ b/core/dronecode_sdk_impl.cpp @@ -219,7 +219,6 @@ System &DronecodeSDKImpl::get_system() // Just return first system instead of failing. return *_systems.begin()->second; } else { - LogWarn() << "No system found."; uint8_t system_id = 0, comp_id = 0; make_system_with_component(system_id, comp_id); return *_systems[system_id]; From 77ec60d386dfe1c00b3e3b7a96dbd84f1b0c499b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 6 Nov 2018 13:48:21 +0100 Subject: [PATCH 2/2] example: improve VTOL transition example, add goto This improves the readability of the VTOL example, and also adds some more action to it by sending it South using `goto_location`. --- .../transition_vtol_fixed_wing.cpp | 162 +++++++++--------- 1 file changed, 77 insertions(+), 85 deletions(-) diff --git a/example/transition_vtol_fixed_wing/transition_vtol_fixed_wing.cpp b/example/transition_vtol_fixed_wing/transition_vtol_fixed_wing.cpp index bf89918f88..9403947e67 100644 --- a/example/transition_vtol_fixed_wing/transition_vtol_fixed_wing.cpp +++ b/example/transition_vtol_fixed_wing/transition_vtol_fixed_wing.cpp @@ -1,46 +1,36 @@ - #include #include +#include +#include +#include #include #include #include -#include -#include using std::this_thread::sleep_for; +using std::chrono::seconds; using std::chrono::milliseconds; using namespace dronecode_sdk; -#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 +static constexpr auto ERROR_CONSOLE_TEXT = "\033[31m"; +static constexpr auto TELEMETRY_CONSOLE_TEXT = "\033[34m"; +static constexpr auto NORMAL_CONSOLE_TEXT = "\033[0m"; -void usage(std::string bin_name) -{ - std::cout << NORMAL_CONSOLE_TEXT << "Usage : " << bin_name << " " << std::endl - << "Connection URL format should be :" << std::endl - << " For TCP : tcp://[server_host][:server_port]" << std::endl - << " For UDP : udp://[bind_host][:bind_port]" << std::endl - << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl - << "For example, to connect to the simulator use URL: udp://:14540" << std::endl; -} +void usage(const std::string &bin_name); int main(int argc, char **argv) { - DronecodeSDK dc; - std::string connection_url; - ConnectionResult connection_result; - - bool discovered_system = false; - - if (argc == 2) { - connection_url = argv[1]; - connection_result = dc.add_any_connection(connection_url); - } else { + if (argc != 2) { usage(argv[0]); return 1; } + const std::string connection_url = argv[1]; + + DronecodeSDK dc; + + // Add connection specified by CLI argument. + const ConnectionResult connection_result = dc.add_any_connection(connection_url); if (connection_result != ConnectionResult::SUCCESS) { std::cout << ERROR_CONSOLE_TEXT << "Connection failed: " << connection_result_str(connection_result) @@ -48,25 +38,13 @@ int main(int argc, char **argv) return 1; } - 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(std::chrono::seconds(2)); - - if (!discovered_system) { - std::cout << ERROR_CONSOLE_TEXT << "No system found, exiting." << NORMAL_CONSOLE_TEXT - << std::endl; - return 1; + // We need an autopilot connected to start. + while (!dc.system().has_autopilot()) { + sleep_for(seconds(1)); + std::cout << "Waiting for system to connect." << std::endl; } - // We don't need to specify the UUID if it's only one system anyway. - // If there were multiple, we could specify it with: - // dc.system(uint64_t uuid); + // Get system and plugins. System &system = dc.system(); auto telemetry = std::make_shared(system); auto action = std::make_shared(system); @@ -75,97 +53,111 @@ int main(int argc, char **argv) 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) + << "Setting rate failed: " << Telemetry::result_str(set_rate_result) << NORMAL_CONSOLE_TEXT << std::endl; return 1; } - // Set up callback to monitor altitude while the vehicle is in flight + // Set up callback to monitor altitude. 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; + std::cout << TELEMETRY_CONSOLE_TEXT << "Altitude: " << position.relative_altitude_m << " m" + << NORMAL_CONSOLE_TEXT << std::endl; }); - // Wait until vehicle is ready to arm. - while (telemetry->health_all_ok() != true) { - std::cout << ERROR_CONSOLE_TEXT << "Vehicle not ready to arm" << NORMAL_CONSOLE_TEXT - << std::endl; - sleep_for(std::chrono::seconds(1)); + // Wait until we are ready to arm. + while (!telemetry->health_all_ok()) { + std::cout << "Waiting for vehicle to be ready to arm..." << std::endl; + sleep_for(seconds(1)); } // Arm vehicle - std::cout << "Arming..." << std::endl; + std::cout << "Arming." << std::endl; const ActionResult arm_result = action->arm(); if (arm_result != ActionResult::SUCCESS) { - std::cout << ERROR_CONSOLE_TEXT << "Arming failed:" << action_result_str(arm_result) + std::cout << ERROR_CONSOLE_TEXT << "Arming failed: " << action_result_str(arm_result) << NORMAL_CONSOLE_TEXT << std::endl; return 1; } // Take off - std::cout << "Taking off..." << std::endl; + std::cout << "Taking off." << std::endl; const ActionResult takeoff_result = action->takeoff(); if (takeoff_result != ActionResult::SUCCESS) { - std::cout << ERROR_CONSOLE_TEXT << "Takeoff failed:" << action_result_str(takeoff_result) + std::cout << ERROR_CONSOLE_TEXT << "Takeoff failed:n" << action_result_str(takeoff_result) << NORMAL_CONSOLE_TEXT << std::endl; return 1; } - // Wait - std::this_thread::sleep_for(std::chrono::seconds(10)); + // Wait while it takes off. + sleep_for(seconds(10)); - std::cout << "Transition to fixedwing..." << std::endl; + std::cout << "Transition to fixedwing." << std::endl; const ActionResult fw_result = action->transition_to_fixedwing(); if (fw_result != ActionResult::SUCCESS) { std::cout << ERROR_CONSOLE_TEXT << "Transition to fixed wing failed: " << action_result_str(fw_result) << NORMAL_CONSOLE_TEXT << std::endl; - // return 1; + return 1; } - // Wait - std::this_thread::sleep_for(std::chrono::seconds(10)); + // Let it transition and start loitering. + sleep_for(seconds(30)); - std::cout << "Transition back to multicopter..." << std::endl; - const ActionResult mc_result = action->transition_to_multicopter(); - if (mc_result != ActionResult::SUCCESS) { - std::cout << ERROR_CONSOLE_TEXT - << "Transition to multi copter failed:" << action_result_str(mc_result) + // Send it South. + std::cout << "Sending it to location." << std::endl; + // We pass latitude and longitude but leave altitude and yaw unset by passing NAN. + const ActionResult goto_result = action->goto_location(47.3633001, 8.5428515, NAN, NAN); + if (goto_result != ActionResult::SUCCESS) { + std::cout << ERROR_CONSOLE_TEXT << "Goto command failed: " << action_result_str(goto_result) << NORMAL_CONSOLE_TEXT << std::endl; - // return 1; + return 1; } - // Wait - std::this_thread::sleep_for(std::chrono::seconds(5)); + // Let it fly South for a bit. + sleep_for(seconds(20)); - // Return to launch - std::cout << "Return to launch..." << std::endl; - const ActionResult rtl_result = action->return_to_launch(); - if (rtl_result != ActionResult::SUCCESS) { + // Let's stop before reaching the goto point and go back to hover. + std::cout << "Transition back to multicopter..." << std::endl; + const ActionResult mc_result = action->transition_to_multicopter(); + if (mc_result != ActionResult::SUCCESS) { std::cout << ERROR_CONSOLE_TEXT - << "Returning to launch failed:" << action_result_str(rtl_result) + << "Transition to multi copter failed: " << action_result_str(mc_result) << NORMAL_CONSOLE_TEXT << std::endl; - // return 1; + return 1; } - // Wait - std::this_thread::sleep_for(std::chrono::seconds(20)); + // Wait for the transition to be carried out. + sleep_for(seconds(5)); - // Land + // Now just land here. std::cout << "Landing..." << std::endl; const ActionResult land_result = action->land(); if (land_result != ActionResult::SUCCESS) { - std::cout << ERROR_CONSOLE_TEXT << "Land failed:" << action_result_str(land_result) + std::cout << ERROR_CONSOLE_TEXT << "Land failed: " << action_result_str(land_result) << NORMAL_CONSOLE_TEXT << std::endl; - // return 1; + return 1; } - // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer. - std::this_thread::sleep_for(std::chrono::seconds(5)); - std::cout << "Finished..." << std::endl; + // Wait until disarmed. + while (telemetry->armed()) { + std::cout << "Waiting for vehicle to land and disarm." << std::endl; + sleep_for(seconds(1)); + } + + std::cout << "Disarmed, exiting." << std::endl; + return 0; } + +void usage(const std::string &bin_name) +{ + std::cout << NORMAL_CONSOLE_TEXT << "Usage : " << bin_name << " " << std::endl + << "Connection URL format should be :" << std::endl + << " For TCP : tcp://[server_host][:server_port]" << std::endl + << " For UDP : udp://[bind_host][:bind_port]" << std::endl + << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl + << std::endl + << "For example, to connect to the simulator use URL: udp://:14540" << std::endl; +}