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

Add check for stuck range finder measurements

Browse Source
This commit is contained in:
ChristophTobler 2017-07-31 17:59:16 +02:00
parent 41f4b62cdb
commit 7252284628
2 changed files with 39 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
EKF/control.cpp
View File

@ -105,6 +105,8 @@ void Ekf::controlFusionModes()
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_rng_to_earth_2_2 > 0.7071f);
checkForStuckRange();
_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f);
_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
@ -947,6 +949,34 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
}
}
void Ekf::checkForStuckRange()
{
if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > 10e6 &&
_control_status.flags.in_air) {
_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;
} else {
if (_range_sample_delayed.rng > _rng_check_max_val)
_rng_check_max_val = _range_sample_delayed.rng;
if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val)
_rng_check_min_val = _range_sample_delayed.rng;
_range_data_ready = false;
}
} else if (_range_data_ready) {
_time_last_rng_ready = _range_sample_delayed.time_us;
}
}
void Ekf::controlAirDataFusion()
{
// control activation and initialisation/reset of wind states required for airspeed fusion

9
EKF/ekf.h
View File

@ -261,6 +261,7 @@ private:
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
uint64_t _time_last_rng_ready{0}; ///< time the last range finder measurement was ready (uSec)
Vector2f _last_known_posNE; ///< last known local NE position vector (m)
float _last_disarmed_posD{0.0f}; ///< vertical position recorded at arming (m)
float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
@ -382,6 +383,11 @@ private:
// variables used to control range aid functionality
bool _in_range_aid_mode; ///< 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
// update the real time complementary filter states. This includes the prediction
// and the correction step
void calculateOutputStates();
@ -518,6 +524,9 @@ private:
bool rangeAidConditionsMet(bool in_range_aid_mode);
// check for "stuck" range finder measurements when rng was not valid for certain period
void checkForStuckRange();
// return the square of two floating point numbers - used in auto coded sections
inline float sq(float var)
{