fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 23:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Multicopter land detector: Enforce sync between system and detector

Browse Source
This commit is contained in:
Lorenz Meier
2016-04-29 12:34:58 +02:00
parent 109f88564d
commit 3ccd9988d3
4 changed files with 35 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+27 -14
View File
@@ -68,7 +68,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
_paramHandle.maxThrottle = param_find("MPC_THR_MIN");
_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
}
@@ -144,34 +145,45 @@ bool MulticopterLandDetector::get_freefall_state()
bool MulticopterLandDetector::get_landed_state()
{
// Time base for this function
const uint64_t now = hrt_absolute_time();
// Check if thrust output is less than max throttle param.
bool minimalThrust = (_actuators.control[3] <= (_params.maxThrottle + 0.05f));
if (minimalThrust && _min_trust_start == 0) {
_min_trust_start = now;
} else if (!minimalThrust) {
_min_trust_start = 0;
}
// only trigger flight conditions if we are armed
if (!_arming.armed) {
_arming_time = 0;
return true;
} else if (_arming_time == 0) {
_arming_time = hrt_absolute_time();
_arming_time = now;
}
// Check if user commands throttle and if so, report not landed
if (_manual.z > 0.3f) {
return false;
}
// Check if thrust output is less than max throttle param.
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
// Return status based on armed state and throttle if no position lock is available.
if (_vehicleLocalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
!_vehicleLocalPosition.xy_valid ||
!_vehicleLocalPosition.z_valid) {
// Minimal thrust means landed.
return minimalThrust;
}
// Check if user commands throttle and if so, report not landed
if (_manual.z > _params.minManThrottle + 0.01f) {
return false;
}
const uint64_t now = hrt_absolute_time();
if ((_min_trust_start > 0) &&
(hrt_elapsed_time(&_min_trust_start) > 5 * 1000 * 1000)) {
return !get_freefall_state();
} else {
return false;
}
}
float armThresholdFactor = 1.0f;
@@ -224,6 +236,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
}