From 95b8328162a20cd5cdf884b495c68eb7780893a4 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Fri, 9 Jan 2026 17:34:46 +0100 Subject: [PATCH] FixedWingLandDetector: force to landed during runway takeoff --- .../land_detector/FixedwingLandDetector.cpp | 33 +++++++++++++------ .../land_detector/FixedwingLandDetector.h | 2 ++ 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5de7f5cb0a..df7016f097 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,18 +57,31 @@ bool FixedwingLandDetector::_get_landed_state() return true; } + // Force the landed state to stay landed if we're currently in an early state of the takeoff state machines. + // This prevents premature transitions to in-air during the early takeoff phase. + if (_landed_hysteresis.get_state()) { + launch_detection_status_s launch_detection_status{}; + _launch_detection_status_sub.copy(&launch_detection_status); + + fixed_wing_runway_control_s fixed_wing_runway_control{}; + _fixed_wing_runway_control_sub.copy(&fixed_wing_runway_control); + + // Check if we're in catapult/hand-launch waiting state + const bool waiting_for_catapult_launch = hrt_elapsed_time(&launch_detection_status.timestamp) < 500_ms + && launch_detection_status.launch_detection_state == launch_detection_status_s::STATE_WAITING_FOR_LAUNCH; + + // Check if we're in runway takeoff early phase (throttle ramp or clamped to runway) + const bool waiting_for_auto_runway_climbout = hrt_elapsed_time(&fixed_wing_runway_control.timestamp) < 500_ms + && fixed_wing_runway_control.runway_takeoff_state < fixed_wing_runway_control_s::STATE_CLIMBOUT; + + if (waiting_for_catapult_launch || waiting_for_auto_runway_climbout) { + return true; + } + } + bool landDetected = false; - launch_detection_status_s launch_detection_status{}; - _launch_detection_status_sub.copy(&launch_detection_status); - - // force the landed state to stay landed if we're currently in the catapult/hand-launch launch process. Detect that we are in this state - // by checking if the last publication of launch_detection_status is less than 0.5s old, and we're still in the wait for launch state. - if (_landed_hysteresis.get_state() && hrt_elapsed_time(&launch_detection_status.timestamp) < 500_ms - && launch_detection_status.launch_detection_state == launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { - landDetected = true; - - } else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { + if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { float val = 0.0f; diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 435ecc1fae..2454b83f43 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,6 +44,7 @@ #include #include +#include #include #include "LandDetector.h" @@ -67,6 +68,7 @@ protected: private: uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; + uORB::Subscription _fixed_wing_runway_control_sub{ORB_ID(fixed_wing_runway_control)}; float _airspeed_filtered{0.0f}; float _velocity_xy_filtered{0.0f};