From cef5775305d47bc7d23a2e9677e6c823b802ef3d Mon Sep 17 00:00:00 2001 From: Javilinos Date: Thu, 6 Mar 2025 10:42:32 +0100 Subject: [PATCH 1/2] obtain control authority moved to set offboard mode --- src/as2_platform_dji_psdk.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/as2_platform_dji_psdk.cpp b/src/as2_platform_dji_psdk.cpp index 468cfef..f600e2c 100644 --- a/src/as2_platform_dji_psdk.cpp +++ b/src/as2_platform_dji_psdk.cpp @@ -172,9 +172,11 @@ bool DJIMatricePSDKPlatform::ownSetOffboardControl(bool offboard) { if (!offboard) { // Release control authority + RCLCPP_INFO(this->get_logger(), "Releasing control authority"); return setControlAuthority(false); } - return true; + RCLCPP_INFO(this->get_logger(), "Obtain control authority"); + return setControlAuthority(true); } bool DJIMatricePSDKPlatform::ownSetPlatformControlMode(const as2_msgs::msg::ControlMode & msg) @@ -196,7 +198,6 @@ bool DJIMatricePSDKPlatform::ownSetPlatformControlMode(const as2_msgs::msg::Cont { // Obtain control authority RCLCPP_INFO(this->get_logger(), "HOVER || SPEED MODE: Obtain control authority"); - success = setControlAuthority(true); break; } default: From 910451ff3e52cab7b45a447a7225e82c5ac9ac01 Mon Sep 17 00:00:00 2001 From: Javilinos Date: Thu, 6 Mar 2025 17:45:25 +0100 Subject: [PATCH 2/2] check control authority before go to, add sleep after takeoff and land --- src/as2_platform_dji_psdk.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/as2_platform_dji_psdk.cpp b/src/as2_platform_dji_psdk.cpp index f600e2c..5c66586 100644 --- a/src/as2_platform_dji_psdk.cpp +++ b/src/as2_platform_dji_psdk.cpp @@ -198,6 +198,7 @@ bool DJIMatricePSDKPlatform::ownSetPlatformControlMode(const as2_msgs::msg::Cont { // Obtain control authority RCLCPP_INFO(this->get_logger(), "HOVER || SPEED MODE: Obtain control authority"); + success = ctl_authority_; break; } default: @@ -276,6 +277,9 @@ bool DJIMatricePSDKPlatform::ownTakeoff() bool success = result && response->success; if (!success) { RCLCPP_INFO(this->get_logger(), "Could not takeoff due to '%s'", response->message.data()); + } else { + // sleep 10 seconds to wait for the takeoff + rclcpp::sleep_for(std::chrono::seconds(10)); } return success; } @@ -290,6 +294,9 @@ bool DJIMatricePSDKPlatform::ownLand() bool success = result && response->success; if (!success) { RCLCPP_INFO(this->get_logger(), "Could not land due to '%s'", response->message.data()); + } else { + // sleep 10 seconds to wait for the land + rclcpp::sleep_for(std::chrono::seconds(10)); } return success; }