Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

Land detector: Code cleanup

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-14 15:17:50 +01:00
parent 515d81b8d6
commit de46d8e872
2 changed files with 6 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+5 -15
View File
@@ -61,9 +61,7 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_accel_x_integral(0.0f),
_lastTime(0),
_lastXAccel(0.0f),
_accel_horz_lp(0.0f),
_landDetectTrigger(0)
{
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
@@ -93,9 +91,6 @@ void FixedwingLandDetector::updateSubscriptions()
bool FixedwingLandDetector::update()
{
_lastXAccel = _controlState.x_acc;
_lastTime = _controlState.timestamp;
// First poll for new data from our subscriptions
updateSubscriptions();
@@ -123,12 +118,9 @@ bool FixedwingLandDetector::update()
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
if (_lastTime != 0) {
/* a leaking integrator prevents biases from building up, but
* gives a mostly correct response for short impulses
*/
_accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f;
}
// a leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses
_accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f;
}
@@ -136,7 +128,7 @@ bool FixedwingLandDetector::update()
if (_velocity_xy_filtered < _params.maxVelocity
&& _velocity_z_filtered < _params.maxClimbRate
&& _airspeed_filtered < _params.maxAirSpeed
&& _accel_x_integral < _params.maxIntVelocity) {
&& _accel_horz_lp < _params.maxIntVelocity) {
// these conditions need to be stable for a period of time before we trust them
if (now > _landDetectTrigger) {
@@ -148,8 +140,6 @@ bool FixedwingLandDetector::update()
_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
}
_lastTime = _controlState.timestamp;
return landDetected;
}
src/modules/land_detector/FixedwingLandDetector.h
+1 -3
View File
@@ -107,9 +107,7 @@ private:
float _velocity_xy_filtered;
float _velocity_z_filtered;
float _airspeed_filtered;
float _accel_x_integral;
uint64_t _lastTime;
float _lastXAccel;
float _accel_horz_lp;
uint64_t _landDetectTrigger;
};