From b9ea8b4c24faa6c9e63bb0cad57557da10e2a9e3 Mon Sep 17 00:00:00 2001 From: Magnus Berdal Date: Tue, 24 Mar 2026 13:39:38 +0100 Subject: [PATCH 1/8] Ready for test --- dji_sdk/include/dji_sdk/dji_sdk_node.h | 15 ++- dji_sdk/src/modules/dji_sdk_node.cpp | 100 ++++++++++++++++-- .../src/modules/dji_sdk_node_publisher.cpp | 2 + dji_sdk/src/modules/dji_sdk_node_services.cpp | 3 + 4 files changed, 109 insertions(+), 11 deletions(-) diff --git a/dji_sdk/include/dji_sdk/dji_sdk_node.h b/dji_sdk/include/dji_sdk/dji_sdk_node.h index db54eabb..2f993cce 100644 --- a/dji_sdk/include/dji_sdk/dji_sdk_node.h +++ b/dji_sdk/include/dji_sdk/dji_sdk_node.h @@ -129,14 +129,15 @@ class DJISDKNode }; private: - bool initVehicle(ros::NodeHandle& nh_private); + void fcCommunicationWatchdogCallback(const ros::TimerEvent& event); + bool initVehicle(void); bool initServices(ros::NodeHandle& nh); bool initFlightControl(ros::NodeHandle& nh); bool initSubscriber(ros::NodeHandle& nh); bool initPublisher(ros::NodeHandle& nh); bool initActions(ros::NodeHandle& nh); bool initDataSubscribeFromFC(ros::NodeHandle& nh); - void cleanUpSubscribeFromFC(); + void cleanUpSubscribeFromFC(int timeout_sec); bool validateSerialDevice(LinuxSerialDevice* serialDevice); bool isM100(); @@ -315,6 +316,7 @@ class DJISDKNode #endif private: + ros::NodeHandle nh_; //! OSDK core Vehicle* vehicle; //! general service servers @@ -440,6 +442,14 @@ class DJISDKNode ros::Publisher main_camera_stream_publisher; ros::Publisher fpv_camera_stream_publisher; #endif + + const ros::Duration FC_COMMUNICATION_WATCHDOG_NOMINAL_PERIOD{1.0}; + const ros::Duration FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD{0.5}; + ros::Timer fc_communication_watchdog_timer_; + std::atomic_bool has_recieved_data_since_prev_check_{false}; + + bool rerequest_sdk_ctrl_on_fc_coms_reestablished_{false}; + //! constant const int WAIT_TIMEOUT = 10; const int MAX_SUBSCRIBE_PACKAGES = 5; @@ -528,6 +538,7 @@ class DJISDKNode ros::Time& data_time_of_measurement_out ); + void setDataRecieved(void); }; #endif // DJI_SDK_NODE_MAIN_H diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index 53d1fbaf..ff786165 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -14,7 +14,8 @@ using namespace DJI::OSDK; DJISDKNode::DJISDKNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private) - : telemetry_from_fc(USE_BROADCAST), + : nh_{nh}, + telemetry_from_fc(USE_BROADCAST), R_FLU2FRD(tf::Matrix3x3(1, 0, 0, 0, -1, 0, 0, 0, -1)), R_ENU2NED(tf::Matrix3x3(0, 1, 0, 1, 0, 0, 0, 0, -1)), curr_align_state(UNALIGNED) @@ -50,7 +51,7 @@ DJISDKNode::DJISDKNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private) // @todo need some error handling for init functions //! @note parsing launch file to get environment parameters - if (!initVehicle(nh_private)) + if (!initVehicle()) { ROS_ERROR("Vehicle initialization failed"); ros::shutdown(); @@ -126,13 +127,18 @@ DJISDKNode::DJISDKNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private) ROS_ERROR("initPublisher failed"); ros::shutdown(); } + + fc_communication_watchdog_timer_ = nh_.createTimer( + FC_COMMUNICATION_WATCHDOG_NOMINAL_PERIOD, + &DJISDKNode::fcCommunicationWatchdogCallback, this + ); } DJISDKNode::~DJISDKNode() { if(!isM100()) { - cleanUpSubscribeFromFC(); + cleanUpSubscribeFromFC(WAIT_TIMEOUT); } if (vehicle) { @@ -140,8 +146,79 @@ DJISDKNode::~DJISDKNode() } } +void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) +{ + const bool data_recieved{has_recieved_data_since_prev_check_}; + has_recieved_data_since_prev_check_ = false; + if (data_recieved) + return; + + ROS_ERROR_STREAM("No data recieved during the last " << (event.current_real - event.last_real).toSec() << " seconds."); + + if (vehicle) + { + cleanUpSubscribeFromFC(1u); + delete vehicle; + } + + if (!initVehicle()) + { + ROS_ERROR_STREAM("Re-initing vehicle failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); + fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, false); + return; + } + + if (!initDataSubscribeFromFC(nh_)) + { + ROS_ERROR_STREAM("Restarting data subscription from FC failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); + fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, false); + } + else + { + ROS_INFO_STREAM("Data subscription from FC restarted successfully"); + if (pps_sync_) + { + vehicle->hardSync->setSyncFreq(1ul); + vehicle->hardSync->subscribeNMEAMsgs(NMEACallback, this); + vehicle->hardSync->subscribeUTCTime(GPSUTCTimeCallback, this); + vehicle->hardSync->subscribeFCTimeInUTCRef(FCTimeInUTCCallback, this); + vehicle->hardSync->subscribePPSSource(PPSSourceCallback, this); + } + fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_NOMINAL_PERIOD, false); + } + + if (rerequest_sdk_ctrl_on_fc_coms_reestablished_) + { + //Attempt to obtain control + dji_sdk::SDKControlAuthority::Request request; + request.control_enable = dji_sdk::SDKControlAuthorityRequest::REQUEST_CONTROL; + dji_sdk::SDKControlAuthority::Response response; + sdkCtrlAuthorityCallback(request, response); + if (!response.result) + { + // Everything else is up and running. Should the request be attempted again? + ROS_ERROR_STREAM("Failed to obtain SDK control after re-initiating FC coms."); + return; + } + // Send control command to change the display mode to SDK control mode + constexpr uint8_t CONTROL_FLAG{ + Control::HORIZONTAL_POSITION | + Control::VERTICAL_VELOCITY | + Control::YAW_RATE | + Control::HORIZONTAL_BODY | + Control::STABLE_DISABLE + }; + flightControl(CONTROL_FLAG, 0.0, 0.0, 0.0, 0.0); + ROS_ERROR_STREAM("SDK control regained after re-initiating FC coms."); + + // TODO: will display mode change instantly, or will a few messages containing + // a display mode other than MODE_NAVI_SDK_CTRL be published before? If so, ensure that + // the rest of the system handles it correctly + } +} + bool -DJISDKNode::initVehicle(ros::NodeHandle& nh_private) +DJISDKNode::initVehicle(void) { bool threadSupport = true; bool enable_advanced_sensing = false; @@ -700,12 +777,12 @@ DJISDKNode::initDataSubscribeFromFC(ros::NodeHandle& nh) } void -DJISDKNode::cleanUpSubscribeFromFC() +DJISDKNode::cleanUpSubscribeFromFC(int timeout_sec) { - vehicle->subscribe->removePackage(0, WAIT_TIMEOUT); - vehicle->subscribe->removePackage(1, WAIT_TIMEOUT); - vehicle->subscribe->removePackage(2, WAIT_TIMEOUT); - vehicle->subscribe->removePackage(3, WAIT_TIMEOUT); + vehicle->subscribe->removePackage(0, timeout_sec); + vehicle->subscribe->removePackage(1, timeout_sec); + vehicle->subscribe->removePackage(2, timeout_sec); + vehicle->subscribe->removePackage(3, timeout_sec); if (vehicle->hardSync) { vehicle->hardSync->unsubscribeNMEAMsgs(); @@ -809,3 +886,8 @@ std::string DJISDKNode::controlAuthorityErrorString(const uint32_t error_code) else return ""; } + +void DJISDKNode::setDataRecieved(void) +{ + has_recieved_data_since_prev_check_ = true; +} \ No newline at end of file diff --git a/dji_sdk/src/modules/dji_sdk_node_publisher.cpp b/dji_sdk/src/modules/dji_sdk_node_publisher.cpp index 5652ff46..d8f92950 100644 --- a/dji_sdk/src/modules/dji_sdk_node_publisher.cpp +++ b/dji_sdk/src/modules/dji_sdk_node_publisher.cpp @@ -851,6 +851,8 @@ DJISDKNode::publish400HzData(Vehicle *vehicle, RecvContainer recvFrame, p->imu_publisher.publish(synced_imu); publishStampDiff(msg_time, p->stamp_diff_400hz_pub); + + p->setDataRecieved(); } bool DJISDKNode::get400HzTimestamp diff --git a/dji_sdk/src/modules/dji_sdk_node_services.cpp b/dji_sdk/src/modules/dji_sdk_node_services.cpp index 7ae648bc..27bbb6e8 100644 --- a/dji_sdk/src/modules/dji_sdk_node_services.cpp +++ b/dji_sdk/src/modules/dji_sdk_node_services.cpp @@ -127,6 +127,9 @@ DJISDKNode::sdkCtrlAuthorityCallback( response.result = true; } + if (response.result) + rerequest_sdk_ctrl_on_fc_coms_reestablished_ = (request.control_enable == dji_sdk::SDKControlAuthorityRequest::REQUEST_CONTROL); + return true; } From b3e39c848bba6739c4eaa3b66723bf5bdf0877bc Mon Sep 17 00:00:00 2001 From: Magnus Berdal Date: Tue, 24 Mar 2026 13:40:37 +0100 Subject: [PATCH 2/8] Newline --- dji_sdk/src/modules/dji_sdk_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index ff786165..3da97f6e 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -890,4 +890,4 @@ std::string DJISDKNode::controlAuthorityErrorString(const uint32_t error_code) void DJISDKNode::setDataRecieved(void) { has_recieved_data_since_prev_check_ = true; -} \ No newline at end of file +} From e89c54d889163bba4fd72c212b3d8b43f3965185 Mon Sep 17 00:00:00 2001 From: Magnus Berdal Date: Tue, 31 Mar 2026 10:34:18 +0200 Subject: [PATCH 3/8] Reset timer as drone activation can block for a long time --- dji_sdk/src/modules/dji_sdk_node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index 3da97f6e..7957512b 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -164,14 +164,14 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) if (!initVehicle()) { ROS_ERROR_STREAM("Re-initing vehicle failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); - fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, false); + fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, true); return; } if (!initDataSubscribeFromFC(nh_)) { ROS_ERROR_STREAM("Restarting data subscription from FC failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); - fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, false); + fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, true); } else { @@ -184,7 +184,7 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) vehicle->hardSync->subscribeFCTimeInUTCRef(FCTimeInUTCCallback, this); vehicle->hardSync->subscribePPSSource(PPSSourceCallback, this); } - fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_NOMINAL_PERIOD, false); + fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_NOMINAL_PERIOD, true); } if (rerequest_sdk_ctrl_on_fc_coms_reestablished_) From 370cd811a5fa46e490c051d7594d512b4e81b35f Mon Sep 17 00:00:00 2001 From: Magnus Berdal Date: Tue, 31 Mar 2026 10:39:03 +0200 Subject: [PATCH 4/8] Assign to nullptr --- dji_sdk/src/modules/dji_sdk_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index 7957512b..e8090ca1 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -159,6 +159,7 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) { cleanUpSubscribeFromFC(1u); delete vehicle; + vehicle = nullptr; } if (!initVehicle()) From bd87363115b8a9cd112e3d10a80f8fa48ba6eb72 Mon Sep 17 00:00:00 2001 From: Magnus Berdal Date: Tue, 31 Mar 2026 10:54:30 +0200 Subject: [PATCH 5/8] Delete vehicle if activation fails --- dji_sdk/src/modules/dji_sdk_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index e8090ca1..95572bcb 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -166,6 +166,8 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) { ROS_ERROR_STREAM("Re-initing vehicle failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, true); + delete vehicle; + vehicle = nullptr; return; } From f57d18dab2a4b0e2326911a3b712bb44ef8eaac9 Mon Sep 17 00:00:00 2001 From: Magnus Berdal Date: Tue, 31 Mar 2026 11:42:11 +0200 Subject: [PATCH 6/8] reorder --- dji_sdk/src/modules/dji_sdk_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index 95572bcb..d63ada2d 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -164,10 +164,10 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) if (!initVehicle()) { - ROS_ERROR_STREAM("Re-initing vehicle failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); - fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, true); delete vehicle; vehicle = nullptr; + ROS_ERROR_STREAM("Re-initing vehicle failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); + fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, true); return; } From c857b59a32eb94ee42f23e2472f937b1b94e9c72 Mon Sep 17 00:00:00 2001 From: Magnus Berdal Date: Tue, 31 Mar 2026 11:56:31 +0200 Subject: [PATCH 7/8] No usleep before actual activate call & configurable timeout --- dji_sdk/include/dji_sdk/dji_sdk_node.h | 10 +++++----- dji_sdk/src/modules/dji_sdk_node.cpp | 13 ++++++------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/dji_sdk/include/dji_sdk/dji_sdk_node.h b/dji_sdk/include/dji_sdk/dji_sdk_node.h index 2f993cce..9ba04bdf 100644 --- a/dji_sdk/include/dji_sdk/dji_sdk_node.h +++ b/dji_sdk/include/dji_sdk/dji_sdk_node.h @@ -130,7 +130,7 @@ class DJISDKNode private: void fcCommunicationWatchdogCallback(const ros::TimerEvent& event); - bool initVehicle(void); + bool initVehicle(int activation_timeout_sec=WAIT_TIMEOUT); bool initServices(ros::NodeHandle& nh); bool initFlightControl(ros::NodeHandle& nh); bool initSubscriber(ros::NodeHandle& nh); @@ -148,7 +148,7 @@ class DJISDKNode * we cannot call a service without serviceClient, which is in another * node */ - ACK::ErrorCode activate(int l_app_id, std::string l_enc_key); + ACK::ErrorCode activate(int l_app_id, std::string l_enc_key, int timeout_sec=WAIT_TIMEOUT); //! flight control subscriber callbacks void flightControlSetpointCallback( @@ -451,9 +451,9 @@ class DJISDKNode bool rerequest_sdk_ctrl_on_fc_coms_reestablished_{false}; //! constant - const int WAIT_TIMEOUT = 10; - const int MAX_SUBSCRIBE_PACKAGES = 5; - const int INVALID_VERSION = 0; + static constexpr int WAIT_TIMEOUT{10}; + static constexpr int MAX_SUBSCRIBE_PACKAGES{5}; + static constexpr int INVALID_VERSION{0}; //! configurations int app_id; diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index d63ada2d..b67ea628 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -162,7 +162,7 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) vehicle = nullptr; } - if (!initVehicle()) + if (!initVehicle(1)) { delete vehicle; vehicle = nullptr; @@ -221,7 +221,7 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) } bool -DJISDKNode::initVehicle(void) +DJISDKNode::initVehicle(int activation_timeout_sec) { bool threadSupport = true; bool enable_advanced_sensing = false; @@ -239,7 +239,7 @@ DJISDKNode::initVehicle(void) * user can also call it as a service * this has been tested by giving wrong appID in launch file */ - if (ACK::getError(this->activate(this->app_id, this->enc_key))) + if (ACK::getError(this->activate(this->app_id, this->enc_key, activation_timeout_sec))) { ROS_ERROR("drone activation error"); return false; @@ -339,17 +339,16 @@ bool DJISDKNode::isM100() ACK::ErrorCode -DJISDKNode::activate(int l_app_id, std::string l_enc_key) +DJISDKNode::activate(int l_app_id, std::string l_enc_key, int timeout_sec) { - usleep(1000000); Vehicle::ActivateData testActivateData; char app_key[65]; testActivateData.encKey = app_key; strcpy(testActivateData.encKey, l_enc_key.c_str()); testActivateData.ID = l_app_id; - ROS_DEBUG("called vehicle->activate(&testActivateData, WAIT_TIMEOUT)"); - return vehicle->activate(&testActivateData, WAIT_TIMEOUT); + ROS_DEBUG_STREAM("called vehicle->activate(&testActivateData, " << timeout_sec << ")"); + return vehicle->activate(&testActivateData, timeout_sec); } bool From 4da09f2ed056eb0e523f3ae4bf643902762ac228 Mon Sep 17 00:00:00 2001 From: Magnus Berdal Date: Tue, 31 Mar 2026 12:13:43 +0200 Subject: [PATCH 8/8] Avoid concurrent calls --- dji_sdk/src/modules/dji_sdk_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index b67ea628..5cf19287 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -153,6 +153,7 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) if (data_recieved) return; + fc_communication_watchdog_timer_.stop(); // Avoid concurrent calls due to AsyncSpinner ROS_ERROR_STREAM("No data recieved during the last " << (event.current_real - event.last_real).toSec() << " seconds."); if (vehicle) @@ -168,6 +169,7 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) vehicle = nullptr; ROS_ERROR_STREAM("Re-initing vehicle failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, true); + fc_communication_watchdog_timer_.start(); return; } @@ -175,6 +177,7 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) { ROS_ERROR_STREAM("Restarting data subscription from FC failed. Retrying in " << FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD.toSec() << " seconds."); fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_RESTART_SUB_PERIOD, true); + fc_communication_watchdog_timer_.start(); } else { @@ -188,6 +191,7 @@ void DJISDKNode::fcCommunicationWatchdogCallback(const ros::TimerEvent& event) vehicle->hardSync->subscribePPSSource(PPSSourceCallback, this); } fc_communication_watchdog_timer_.setPeriod(FC_COMMUNICATION_WATCHDOG_NOMINAL_PERIOD, true); + fc_communication_watchdog_timer_.start(); } if (rerequest_sdk_ctrl_on_fc_coms_reestablished_)