From 6c1399b328ba2f2947eea46db19a60bf9f812cec Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Tue, 15 Jan 2019 17:34:56 +0100
Subject: [PATCH] multicopter land detector: make threshold for _has_low_thrust
configurable
Signed-off-by: Roman <bapstroman@gmail.com>
---
.../land_detector/MulticopterLandDetector.cpp | 6 +++++-
.../land_detector/MulticopterLandDetector.h | 2 ++
.../land_detector/land_detector_params_mc.c | 18 ++++++++++++++++++
3 files changed, 25 insertions(+), 1 deletion(-)
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index c5636d14f1..2a82fb4219 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 58444e5c77..78e9742ecc 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 4f85ce311a..e5e4449837 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);
--
GitLab