diff options
| author | Julian Oes <julian@oes.ch> | 2014-04-27 02:24:33 +0200 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2014-04-27 02:24:33 +0200 |
| commit | a6f71f10d0664754e0b3c8ad2161dc81befb2ca9 (patch) | |
| tree | 393deadb6e7bac1c77b6cc78319711f43c3d47d4 /src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
| parent | e882824ee15e0c5fff58c7f223ec7be181c7af8f (diff) | |
| download | px4-firmware-a6f71f10d0664754e0b3c8ad2161dc81befb2ca9.tar.gz px4-firmware-a6f71f10d0664754e0b3c8ad2161dc81befb2ca9.tar.bz2 px4-firmware-a6f71f10d0664754e0b3c8ad2161dc81befb2ca9.zip | |
fw_att_pos_estimator: some tuning for the land/in-air detector
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 15 |
1 files changed, 9 insertions, 6 deletions
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index f47531b26..fcbd90405 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -240,6 +240,7 @@ private: float _velocity_xy_filtered; float _velocity_z_filtered; + float _airspeed_filtered; /** * Update our local parameter cache. @@ -343,7 +344,8 @@ FixedwingEstimator::FixedwingEstimator() : _mavlink_fd(-1), _ekf(nullptr), _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f) + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f) { last_run = hrt_absolute_time(); @@ -1200,16 +1202,17 @@ FixedwingEstimator::task_main() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; - _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); - _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz); + _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); + _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); + _airspeed_filtered = 0.95*_airspeed_filtered + + 0.05*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, * TODO: adapt so that it works for both, maybe move to another location */ - if (_velocity_xy_filtered < 2 - && _velocity_z_filtered < 2 - && _airspeed.true_airspeed_m_s < 10) { + if (_velocity_xy_filtered < 5 + && _velocity_z_filtered < 10 + && _airspeed_filtered < 10) { _local_pos.landed = true; } else { _local_pos.landed = false; |