diff options
| author | Julian Oes <julian@oes.ch> | 2014-04-21 17:35:42 +0200 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2014-04-21 17:35:42 +0200 |
| commit | 488785250f4f1fa3c2f6d1e3283fd8eabb6b3144 (patch) | |
| tree | 49e9d7da7de6aec94f365a3e71368a62f80fb162 | |
| parent | 654ab4635ad0fb70d6c0cb601d56e3d97691bdac (diff) | |
| download | px4-firmware-488785250f4f1fa3c2f6d1e3283fd8eabb6b3144.tar.gz px4-firmware-488785250f4f1fa3c2f6d1e3283fd8eabb6b3144.tar.bz2 px4-firmware-488785250f4f1fa3c2f6d1e3283fd8eabb6b3144.zip | |
fw_att_pos_estimator: added simple in-air/on-ground detector
| -rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 22 |
1 files changed, 21 insertions, 1 deletions
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 840cd585e..a61fff942 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -211,6 +211,9 @@ private: AttPosEKF *_ekf; + float _velocity_xy_filtered; + float _velocity_z_filtered; + /** * Update our local parameter cache. */ @@ -292,7 +295,9 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _gps_initialized(false), _mavlink_fd(-1), - _ekf(nullptr) + _ekf(nullptr), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f) { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1030,6 +1035,21 @@ 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); + + + /* 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) { + _local_pos.landed = true; + } else { + _local_pos.landed = false; + } + /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { /* publish the attitude setpoint */ |