diff --git a/example/transition_vtol_fixed_wing/CMakeLists.txt b/example/transition_vtol_fixed_wing/CMakeLists.txt new file mode 100644 index 0000000000..319d8a9ba0 --- /dev/null +++ b/example/transition_vtol_fixed_wing/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 2.8.12) + +project(transition_vtol_fixed_wing) + +if(NOT MSVC) + add_definitions("-std=c++11 -Wall -Wextra -Werror") +else() + add_definitions("-std=c++11 -WX -W2") + set(platform_libs "Ws2_32.lib") +endif() + +# Add DEBUG define for Debug target +set(CMAKE_CXX_FLAGS_DEBUG "-DDEBUG") + +# This finds thread libs on Linux, Mac, and Windows. +find_package(Threads REQUIRED) + +# Not needed if DroneCore installed system-wide +include_directories( + ${CMAKE_SOURCE_DIR}/../../install/include +) + +add_executable(transition_vtol_fixed_wing + transition_vtol_fixed_wing.cpp +) + +# Not needed if DroneCore installed system-wide +if(MSVC) + set(dronecore_lib "${CMAKE_SOURCE_DIR}/../../install/lib/dronecore.lib") +else() + set(dronecore_lib "${CMAKE_SOURCE_DIR}/../../install/lib/libdronecore.so") +endif() + +target_link_libraries(transition_vtol_fixed_wing + ${dronecore_lib} # Remove/comment out this line if DroneCore used locally + # dronecore # Uncomment/add this line if DroneCore installed system-wide + ${CMAKE_THREAD_LIBS_INIT} + ${platform_libs} +) diff --git a/example/transition_vtol_fixed_wing/transition_vtol_fixed_wing.cpp b/example/transition_vtol_fixed_wing/transition_vtol_fixed_wing.cpp new file mode 100644 index 0000000000..856cce6fe9 --- /dev/null +++ b/example/transition_vtol_fixed_wing/transition_vtol_fixed_wing.cpp @@ -0,0 +1,143 @@ + +#include +#include +#include +#include +#include + +using std::this_thread::sleep_for; +using std::chrono::milliseconds; +using namespace dronecore; + +#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*/) +{ + DroneCore dc; + + bool discovered_device = false; + + DroneCore::ConnectionResult connection_result = dc.add_udp_connection(); + + if (connection_result != DroneCore::ConnectionResult::SUCCESS) { + std::cout << ERROR_CONSOLE_TEXT << "Connection failed: " + << DroneCore::connection_result_str(connection_result) + << NORMAL_CONSOLE_TEXT << std::endl; + return 1; + } + + std::cout << "Waiting to discover device..." << std::endl; + dc.register_on_discover([&discovered_device](uint64_t uuid) { + std::cout << "Discovered device with UUID: " << uuid << std::endl; + discovered_device = true; + }); + + // We usually receive heartbeats at 1Hz, therefore we should find a device after around 2 seconds. + std::this_thread::sleep_for(std::chrono::seconds(2)); + + if (!discovered_device) { + std::cout << ERROR_CONSOLE_TEXT << "No device found, exiting." << NORMAL_CONSOLE_TEXT << std::endl; + return 1; + } + + // We don't need to specify the UUID if it's only one device anyway. + // If there were multiple, we could specify it with: + // dc.device(uint64_t uuid); + Device &device = dc.device(); + + // We want to listen to the altitude of the drone at 1 Hz. + const Telemetry::Result set_rate_result = dc.device().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 1; + } + + + // Set up callback to monitor altitude while the vehicle is in flight + device.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 + if (device.telemetry().health_all_ok() != true) { + std::cout << ERROR_CONSOLE_TEXT << "Vehicle not ready to arm" << NORMAL_CONSOLE_TEXT << std::endl; + return 1; + } + + // Arm vehicle + std::cout << "Arming..." << std::endl; + const Action::Result arm_result = device.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 1; + } + + // Take off + std::cout << "Taking off..." << std::endl; + const Action::Result takeoff_result = device.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 1; + } + + // Wait + std::this_thread::sleep_for(std::chrono::seconds(10)); + + // Change to FW + const Action::Result fw_result = device.action().transition_to_fixedwing(); + + if (fw_result != Action::Result::SUCCESS) { + std::cout << ERROR_CONSOLE_TEXT << "Transition to fixed wing failed: " << Action::result_str( + fw_result) << NORMAL_CONSOLE_TEXT << std::endl; + //return 1; + } + + // Wait + std::this_thread::sleep_for(std::chrono::seconds(10)); + + // Change to VTOL + const Action::Result mc_result = device.action().transition_to_multicopter(); + if (mc_result != Action::Result::SUCCESS) { + std::cout << ERROR_CONSOLE_TEXT << "Transition to multi copter failed:" << Action::result_str( + mc_result) << NORMAL_CONSOLE_TEXT << std::endl; + // return 1; + } + + // Wait + std::this_thread::sleep_for(std::chrono::seconds(5)); + + // Return to launch + const Action::Result rtl_result = device.action().return_to_launch(); + if (rtl_result != Action::Result::SUCCESS) { + std::cout << ERROR_CONSOLE_TEXT << "Returning to launch failed:" << Action::result_str( + rtl_result) << NORMAL_CONSOLE_TEXT << std::endl; + // return 1; + } + + // Wait + std::this_thread::sleep_for(std::chrono::seconds(20)); + + // Land + std::cout << "Landing..." << std::endl; + const Action::Result land_result = device.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 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; + return 0; +} diff --git a/integration_tests/CMakeLists.txt b/integration_tests/CMakeLists.txt index b77066e9f6..1ec4c32a0a 100644 --- a/integration_tests/CMakeLists.txt +++ b/integration_tests/CMakeLists.txt @@ -27,6 +27,7 @@ list(APPEND integration_tests mission_survey gimbal curl + transition_multicopter_fixedwing ) diff --git a/integration_tests/transition_multicopter_fixedwing.cpp b/integration_tests/transition_multicopter_fixedwing.cpp new file mode 100644 index 0000000000..428362f402 --- /dev/null +++ b/integration_tests/transition_multicopter_fixedwing.cpp @@ -0,0 +1,87 @@ +#include +#include "integration_test_helper.h" +#include "dronecore.h" + +using namespace dronecore; + + +//static void connect(DroneCore); +static void takeoff(Device &); +static void takeoff_and_transition_to_fixedwing(); +static void land_and_disarm(Device &); + + +TEST_F(SitlTest, ActionSimpleTransition) +{ + takeoff_and_transition_to_fixedwing(); +} + +void takeoff_and_transition_to_fixedwing() +{ + // Init & connect + DroneCore dc; + + DroneCore::ConnectionResult ret = dc.add_udp_connection(); + ASSERT_EQ(ret, DroneCore::ConnectionResult::SUCCESS); + + // Wait for device to connect via heartbeat. + std::this_thread::sleep_for(std::chrono::seconds(2)); + ASSERT_TRUE(dc.is_connected()); + + Device &device = dc.device(); + + // We need to takeoff first, otherwise we can't actually transition + LogInfo() << "Taking off"; + takeoff(device); + + LogInfo() << "Transitioning to fixedwing"; + device.action().transition_to_fixedwing(); + + // Wait a little before the transition back to multicopter, + // so we can actually see it fly + std::this_thread::sleep_for(std::chrono::seconds(5)); + + LogInfo() << "Transitioning to multicopter"; + device.action().transition_to_multicopter(); + std::this_thread::sleep_for(std::chrono::seconds(5)); + + // Return safely to launch position so the next test + // can start with a clean slate + land_and_disarm(device); +} + +void land_and_disarm(Device &device) +{ + device.action().return_to_launch(); + + // Wait until the vtol is disarmed. + while (device.telemetry().armed()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + LogInfo() << "Disarmed, exiting."; +} + +void takeoff(Device &device) +{ + while (!device.telemetry().health_all_ok()) { + std::cout << "waiting for device to be ready" << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + Action::Result action_ret = device.action().arm(); + EXPECT_EQ(action_ret, Action::Result::SUCCESS); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + float altitude_m = 10.0f; + device.action().set_takeoff_altitude(altitude_m); + + action_ret = device.action().takeoff(); + EXPECT_EQ(action_ret, Action::Result::SUCCESS); + const int wait_time_s = 10; + std::this_thread::sleep_for(std::chrono::seconds(wait_time_s)); + + + EXPECT_GT(device.telemetry().position().relative_altitude_m, altitude_m - 0.25f); + EXPECT_LT(device.telemetry().position().relative_altitude_m, altitude_m + 0.25f); +} + diff --git a/plugins/action/action.cpp b/plugins/action/action.cpp index 97bf6371a9..c88b98ed2c 100644 --- a/plugins/action/action.cpp +++ b/plugins/action/action.cpp @@ -42,6 +42,16 @@ Action::Result Action::return_to_launch() const return _impl->return_to_launch(); } +Action::Result Action::transition_to_fixedwing() const +{ + return _impl->transition_to_fixedwing(); +} + +Action::Result Action::transition_to_multicopter() const +{ + return _impl->transition_to_multicopter(); +} + void Action::arm_async(result_callback_t callback) { _impl->arm_async(callback); @@ -72,6 +82,16 @@ void Action::return_to_launch_async(result_callback_t callback) _impl->return_to_launch_async(callback); } +void Action::transition_to_multicopter_async(result_callback_t callback) +{ + _impl->transition_to_multicopter_async(callback); +} + +void Action::transition_to_fixedwing_async(result_callback_t callback) +{ + _impl->transition_to_fixedwing_async(callback); +} + void Action::set_takeoff_altitude(float relative_altitude_m) { _impl->set_takeoff_altitude(relative_altitude_m); diff --git a/plugins/action/action.h b/plugins/action/action.h index 9e910301ab..92147cdbd2 100644 --- a/plugins/action/action.h +++ b/plugins/action/action.h @@ -118,6 +118,26 @@ class Action */ Result return_to_launch() const; + /** + * @brief Send command to transition the drone to fixedwing. + * + * The associated action will only be executed for VTOL vehicles in multicopter mode. + * On other vehicles/modes the command will fail with a Result. + * + * @return Result of request. + */ + Result transition_to_fixedwing() const; + + /** + * @brief Send command to transition the drone to multicopter. + * + * The associated action will only be executed for VTOL vehicles in fixedwing mode. + * On other vehicles/modes the command will fail with a Result. + * + * @return Result of request. + */ + Result transition_to_multicopter() const; + /** * @brief Callback type for asynchronous Action calls. */ @@ -186,6 +206,26 @@ class Action */ void return_to_launch_async(result_callback_t callback); + /** + * @brief Send command to transition the drone to fixedwing (asynchronous). + * + * Note that this is only for the vtol type. + * Also, transition to fixedwing is only allowed from multicopter. + * + * @param callback Function to call with result of request. + */ + void transition_to_fixedwing_async(result_callback_t callback); + + /** + * @brief Send command to transition the drone to multicopter (asynchronous). + * + * Note that this is only for the vtol type. + * Also, transition to fixedwing is only allowed from multicopter. + * + * @param callback Function to call with result of request. + */ + void transition_to_multicopter_async(result_callback_t callback); + /** * @brief Set takeoff altitude above ground. * diff --git a/plugins/action/action_impl.cpp b/plugins/action/action_impl.cpp index bec007bf57..ae765a678c 100644 --- a/plugins/action/action_impl.cpp +++ b/plugins/action/action_impl.cpp @@ -157,6 +157,46 @@ Action::Result ActionImpl::return_to_launch() const MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); } +Action::Result ActionImpl::transition_to_fixedwing() const +{ + return action_result_from_command_result( + _parent->send_command_with_ack( + MAV_CMD_DO_VTOL_TRANSITION, + MavlinkCommands::Params {float(MAV_VTOL_STATE_FW)}, + MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); +} + +void ActionImpl::transition_to_fixedwing_async(const Action::result_callback_t &callback) +{ + _parent->send_command_with_ack_async( + MAV_CMD_DO_VTOL_TRANSITION, + MavlinkCommands::Params {float(MAV_VTOL_STATE_FW)}, + std::bind(&ActionImpl::command_result_callback, + _1, + callback), + MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); +} + +Action::Result ActionImpl::transition_to_multicopter() const +{ + return action_result_from_command_result( + _parent->send_command_with_ack( + MAV_CMD_DO_VTOL_TRANSITION, + MavlinkCommands::Params {float(MAV_VTOL_STATE_MC)}, + MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT)); +} + +void ActionImpl::transition_to_multicopter_async(const Action::result_callback_t &callback) +{ + _parent->send_command_with_ack_async( + MAV_CMD_DO_VTOL_TRANSITION, + MavlinkCommands::Params {float(MAV_VTOL_STATE_MC)}, + std::bind(&ActionImpl::command_result_callback, + _1, + callback), + MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); +} + void ActionImpl::arm_async(const Action::result_callback_t &callback) { Action::Result ret = arming_allowed(); diff --git a/plugins/action/action_impl.h b/plugins/action/action_impl.h index a09e5a7a2f..10131d22d9 100644 --- a/plugins/action/action_impl.h +++ b/plugins/action/action_impl.h @@ -23,6 +23,8 @@ class ActionImpl : public PluginImplBase Action::Result takeoff() const; Action::Result land() const; Action::Result return_to_launch() const; + Action::Result transition_to_fixedwing() const; + Action::Result transition_to_multicopter() const; void arm_async(const Action::result_callback_t &callback); void disarm_async(const Action::result_callback_t &callback); @@ -30,6 +32,8 @@ class ActionImpl : public PluginImplBase void takeoff_async(const Action::result_callback_t &callback); void land_async(const Action::result_callback_t &callback); void return_to_launch_async(const Action::result_callback_t &callback); + void transition_to_fixedwing_async(const Action::result_callback_t &callback); + void transition_to_multicopter_async(const Action::result_callback_t &callback); void set_takeoff_altitude(float relative_altitude_m); float get_takeoff_altitude_m() const;