fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 02:41:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

land detector: refactoring ff to freefall

Browse Source
This commit is contained in:
Matthias Grob
2016-12-26 15:36:43 +01:00
committed by Lorenz Meier
parent 808dedf441
commit 03d7b65299
2 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+8 -8
View File
@@ -74,8 +74,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
_paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
_paramHandle.ff_acc_threshold = param_find("LNDMC_FFALL_THR"); _paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR");
_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI"); _paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI");
} }
void MulticopterLandDetector::_initialize_topics() void MulticopterLandDetector::_initialize_topics()
@@ -110,16 +110,16 @@ void MulticopterLandDetector::_update_params()
_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
param_get(_paramHandle.minThrottle, &_params.minThrottle); param_get(_paramHandle.minThrottle, &_params.minThrottle);
param_get(_paramHandle.minManThrottle, &_params.minManThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
param_get(_paramHandle.ff_acc_threshold, &_params.ff_acc_threshold); param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold);
param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time); param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time);
_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time); _freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.freefall_trigger_time);
} }
bool MulticopterLandDetector::_get_freefall_state() bool MulticopterLandDetector::_get_freefall_state()
{ {
if (_params.ff_acc_threshold < 0.1f if (_params.freefall_acc_threshold < 0.1f
|| _params.ff_acc_threshold > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. || _params.freefall_acc_threshold > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection.
return false; return false;
} }
@@ -133,7 +133,7 @@ bool MulticopterLandDetector::_get_freefall_state()
+ _ctrl_state.z_acc * _ctrl_state.z_acc; + _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. acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed.
return (acc_norm < _params.ff_acc_threshold); //true if we are currently falling return (acc_norm < _params.freefall_acc_threshold); //true if we are currently falling
} }
bool MulticopterLandDetector::_get_landed_state() bool MulticopterLandDetector::_get_landed_state()
src/modules/land_detector/MulticopterLandDetector.h
+4 -4
View File
@@ -83,8 +83,8 @@ private:
param_t maxRotation; param_t maxRotation;
param_t minThrottle; param_t minThrottle;
param_t minManThrottle; param_t minManThrottle;
param_t ff_acc_threshold; param_t freefall_acc_threshold;
param_t ff_trigger_time; param_t freefall_trigger_time;
} _paramHandle; } _paramHandle;
struct { struct {
@@ -93,8 +93,8 @@ private:
float maxRotation_rad_s; float maxRotation_rad_s;
float minThrottle; float minThrottle;
float minManThrottle; float minManThrottle;
float ff_acc_threshold; float freefall_acc_threshold;
float ff_trigger_time; float freefall_trigger_time;
} _params; } _params;
int _vehicleLocalPositionSub; int _vehicleLocalPositionSub;