From d29344cb9c59f5b2798a21849de3637d38d7fe28 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 22 Apr 2020 13:30:40 +0200 Subject: [PATCH] MCLandDetector: fix low thrust detection in stabilized mode HTE runs based on the position controller so, even if we whish to use the estimate, it is only available in altitude and position modes. Therefore, we need to always initialize the hoverThrottle using the hover thrust parameter in case we fly in stabilized --- src/modules/land_detector/MulticopterLandDetector.cpp | 9 ++++++++- src/modules/land_detector/MulticopterLandDetector.h | 2 ++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 754176825252..d6ac48a9a4e8 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -118,8 +118,15 @@ void MulticopterLandDetector::_update_params() param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); _params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1); - if (!_params.useHoverThrustEstimate) { + if (!_params.useHoverThrustEstimate || !_hover_thrust_initialized) { param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle); + + // HTE runs based on the position controller so, even if we wish to use + // the estimate, it is only available in altitude and position modes. + // Therefore, we need to always initialize the hoverThrottle using the hover + // thrust parameter in case we fly in stabilized + // TODO: this can be removed once HTE runs in all modes + _hover_thrust_initialized = true; } } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index a9d6abb650ef..a2498d0ef1c4 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -129,6 +129,8 @@ class MulticopterLandDetector : public LandDetector vehicle_control_mode_s _vehicle_control_mode {}; vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {}; + bool _hover_thrust_initialized{false}; + hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first hrt_abstime _landed_time{0};