From 7691e3ff325f83e225660098b5167dbdcf0976f4 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 7 Nov 2022 10:25:58 +0100 Subject: [PATCH] FW Land Detector: force to landed if currently landed and in launch process Signed-off-by: Silvan Fuhrer --- .../launchdetection/LaunchDetector.h | 3 --- src/modules/land_detector/FixedwingLandDetector.cpp | 11 ++++++++++- src/modules/land_detector/FixedwingLandDetector.h | 2 ++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h index c22df933d8..d5912c3650 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h @@ -99,9 +99,6 @@ private: */ uint _state{launch_detection_status_s::STATE_WAITING_FOR_LAUNCH}; - // [us] logs the last time the launch detection notification was sent (used not to spam notifications during launch detection) - hrt_abstime _last_time_launch_detection_notified{0}; - DEFINE_PARAMETERS( (ParamBool) _param_laun_all_on, (ParamFloat) _param_laun_cat_a, diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index e27a4c592c..ec8157034b 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -60,7 +60,16 @@ bool FixedwingLandDetector::_get_landed_state() bool landDetected = false; - if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { + 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 not yet in the flying 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_FLYING) { + landDetected = true; + + } else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { // Horizontal velocity complimentary filter. float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 9c5fd865b5..981fdc2fb5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,6 +44,7 @@ #include #include +#include #include "LandDetector.h" @@ -68,6 +69,7 @@ private: static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us; uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; float _airspeed_filtered{0.0f}; float _velocity_xy_filtered{0.0f};