fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 10:59:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

added rng_stuck to filter control status flags (#392)

Browse Source
This commit is contained in:
Carl Olsson 2018-03-21 01:46:27 +01:00 committed by Paul Riseborough
parent 7964820412
commit f6d23cc621
4 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/common.h
View File

@ -433,6 +433,7 @@ union filter_control_status_u {
uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used
uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused
uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
} flags;
uint32_t value;
};

4
EKF/control.cpp
View File

@ -1033,14 +1033,14 @@ void Ekf::checkForStuckRange()
{
if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 &&
_control_status.flags.in_air) {
_rng_stuck = true;
_control_status.flags.rng_stuck = true;
//require a variance of rangefinder values to check for "stuck" measurements
if (_rng_check_max_val - _rng_check_min_val > 1.0f) {
_time_last_rng_ready = _range_sample_delayed.time_us;
_rng_check_min_val = 0.0f;
_rng_check_max_val = 0.0f;
_rng_stuck = false;
_control_status.flags.rng_stuck = false;
} else {
if (_range_sample_delayed.rng > _rng_check_max_val) {

1
EKF/ekf.h
View File

@ -428,7 +428,6 @@ private:
bool _in_range_aid_mode{false}; ///< true when range finder is to be used as the height reference instead of the primary height sensor
// variables used to check for "stuck" rng data
bool _rng_stuck{false}; ///< true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
float _rng_check_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck
float _rng_check_max_val{0.0f}; ///< maximum value for new rng measurement when being stuck

4
EKF/terrain_estimator.cpp
View File

@ -93,7 +93,7 @@ void Ekf::runTerrainEstimator()
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
// Fuse range finder data if available
if (_range_data_ready && !_rng_stuck) {
if (_range_data_ready && !_control_status.flags.rng_stuck) {
fuseHagl();
// update range sensor angle parameters in case they have changed
@ -158,7 +158,7 @@ void Ekf::fuseHagl()
// return true if the terrain estimate is valid
bool Ekf::get_terrain_valid()
{
if (_terrain_initialised && _range_data_continuous && !_rng_stuck &&
if (_terrain_initialised && _range_data_continuous && !_control_status.flags.rng_stuck &&
(_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) {
return true;