yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

land detector: small refactor while studying

Browse Source
This commit is contained in:
Matthias Grob
2016-12-21 17:49:37 +01:00
committed by Lorenz Meier
parent 494d2c9ecf
commit 808dedf441
3 changed files with 17 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/land_detector/LandDetector.h
View File

@@ -127,10 +127,10 @@ protected:
static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
/* Time in us that landing conditions have to hold before triggering a land. */
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2000000;
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2e6;
/* Time interval in us in which wider acceptance thresholds are used after arming. */
static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000;
static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2e6;
orb_advert_t _landDetectedPub;
struct vehicle_land_detected_s _landDetected;

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

@@ -72,9 +72,9 @@ 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("MPC_THR_MIN");
_paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
_paramHandle.ff_acc_threshold = param_find("LNDMC_FFALL_THR");
_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
}
@@ -108,9 +108,9 @@ void MulticopterLandDetector::_update_params()
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
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.minThrottle, &_params.minThrottle);
param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
param_get(_paramHandle.ff_acc_threshold, &_params.ff_acc_threshold);
param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time);
}
@@ -118,8 +118,8 @@ void MulticopterLandDetector::_update_params()
bool MulticopterLandDetector::_get_freefall_state()
{
if (_params.acc_threshold_m_s2 < 0.1f
|| _params.acc_threshold_m_s2 > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection.
if (_params.ff_acc_threshold < 0.1f
|| _params.ff_acc_threshold > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection.
return false;
}
@@ -133,7 +133,7 @@ bool MulticopterLandDetector::_get_freefall_state()
+ _ctrl_state.z_acc * _ctrl_state.z_acc;
acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed.
return (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling
return (acc_norm < _params.ff_acc_threshold); //true if we are currently falling
}
bool MulticopterLandDetector::_get_landed_state()
@@ -141,7 +141,7 @@ bool MulticopterLandDetector::_get_landed_state()
// Time base for this function
const uint64_t now = hrt_absolute_time();
float sys_min_throttle = (_params.maxThrottle + 0.01f);
float sys_min_throttle = (_params.minThrottle + 0.01f);
// Determine the system min throttle based on flight mode
if (!_ctrl_mode.flag_control_altitude_enabled) {
@@ -154,6 +154,7 @@ bool MulticopterLandDetector::_get_landed_state()
if (minimalThrust && _min_trust_start == 0) {
_min_trust_start = now;
} else if (!minimalThrust) {
_min_trust_start = 0;
}
@@ -178,7 +179,7 @@ bool MulticopterLandDetector::_get_landed_state()
// Return status based on armed state and throttle if no position lock is available.
if (_vehicleLocalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 0.5e6 ||
!_vehicleLocalPosition.xy_valid ||
!_vehicleLocalPosition.z_valid) {
@@ -187,7 +188,7 @@ bool MulticopterLandDetector::_get_landed_state()
// falling consider it to be landed. This should even sustain
// quite acrobatic flight.
if ((_min_trust_start > 0) &&
(hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) {
(hrt_elapsed_time(&_min_trust_start) > 8e6)) {
return true;

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

@@ -81,9 +81,9 @@ private:
param_t maxClimbRate;
param_t maxVelocity;
param_t maxRotation;
param_t maxThrottle;
param_t minThrottle;
param_t minManThrottle;
param_t acc_threshold_m_s2;
param_t ff_acc_threshold;
param_t ff_trigger_time;
} _paramHandle;
@@ -91,9 +91,9 @@ private:
float maxClimbRate;
float maxVelocity;
float maxRotation_rad_s;
float maxThrottle;
float minThrottle;
float minManThrottle;
float acc_threshold_m_s2;
float ff_acc_threshold;
float ff_trigger_time;
} _params;