dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

multicopter land detector: make threshold for _has_low_thrust 

configurable

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2019-01-15 17:34:56 +01:00 committed by Lorenz Meier
parent ec3bc4ee5b
commit 6c1399b328
3 changed files with 25 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -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;

2
src/modules/land_detector/MulticopterLandDetector.h
View File

@ -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};

18
src/modules/land_detector/land_detector_params_mc.c
View File

@ -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);