diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index c5636d14f17076244c453df6ca535479018b7659..2a82fb42192ba578c6c4ac25306d99d82015127e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -82,6 +82,7 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI"); _paramHandle.altitude_max = param_find("LNDMC_ALT_MAX"); _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); + _paramHandle.low_thrust_threshold = param_find("LNDMC_LOW_T_THR"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); @@ -126,6 +127,8 @@ void MulticopterLandDetector::_update_params() _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time)); param_get(_paramHandle.altitude_max, &_params.altitude_max); param_get(_paramHandle.landSpeed, &_params.landSpeed); + param_get(_paramHandle.low_thrust_threshold, &_params.low_thrust_threshold); + } @@ -314,7 +317,8 @@ bool MulticopterLandDetector::_is_climb_rate_enabled() bool MulticopterLandDetector::_has_low_thrust() { // 30% of throttle range between min and hover - float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f; + float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * + _params.low_thrust_threshold; // Check if thrust output is less than the minimum auto throttle param. return _actuators.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 58444e5c77f1c6a4984ae4a49c1fb06d12f57b72..78e9742ecce55020c63c0a684d53764a8027af2a 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -107,6 +107,7 @@ private: param_t freefall_trigger_time; param_t altitude_max; param_t landSpeed; + param_t low_thrust_threshold; } _paramHandle{}; struct { @@ -120,6 +121,7 @@ private: float freefall_trigger_time; float altitude_max; float landSpeed; + float low_thrust_threshold; } _params{}; int _vehicleLocalPositionSub{ -1}; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 4f85ce311a685130bb41be4c1a9effa2dae6a764..e5e4449837b571d6e1062e0e1b6b3019780043bb 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -113,3 +113,21 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3); * */ PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f); + +/** + * Low throttle detection threshold. + * + * Defines the commanded throttle value below which the land detector + * considers the vehicle to have "low thrust". This is one condition that + * is used to detect the ground contact state. The value is calculated as + * val = (MPC_THR_HOVER - MPC_THR_MIN) * LNDMC_LOW_T_THR + MPC_THR_MIN + * Increase this value if the system takes long time to detect landing. + * + * @unit norm + * @min 0.1 + * @max 0.9 + * @decimal 2 + * @group Land Detector + * + */ +PARAM_DEFINE_FLOAT(LNDMC_LOW_T_THR, 0.3);