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

Clean initialiseFilter function (#687)

Browse Source 
* Clean initialiseFilter

* Add const qualifiers
This commit is contained in:
kritz 2019-12-17 13:36:28 +01:00 committed by Mathieu Bresciani
parent 532c9abd4a
commit 01495ede97
2 changed files with 18 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
EKF/ekf.cpp
View File

@ -155,56 +155,42 @@ bool Ekf::initialiseFilter()
// Sum the magnetometer measurements
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if ((_mag_counter == 0) && (_mag_sample_delayed.time_us != 0)) {
// initialise the counter when we start getting data from the buffer
_mag_counter = 1;
} else if ((_mag_counter != 0) && (_mag_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement
if(_mag_sample_delayed.time_us != 0)
{
_mag_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed
if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
// wait for all bad initial data to be flushed
if (_mag_counter <= uint8_t(_obs_buffer_length + 1)) {
_mag_lpf.reset(_mag_sample_delayed.mag);
} else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
} else {
_mag_lpf.update(_mag_sample_delayed.mag);
}
}
}
// accumulate enough height measurements to be confident in the quality of the data
// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if ((_hgt_counter == 0) && (_baro_sample_delayed.time_us != 0)) {
// initialise the counter and height fusion method when we start getting data from the buffer
setControlBaroHeight();
_hgt_counter = 1;
} else if ((_hgt_counter != 0) && (_baro_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement
_hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed
if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states
if (_baro_sample_delayed.time_us != 0)
{
_baro_counter ++;
// wait for all bad initial data to be flushed
if (_baro_counter <= uint8_t(_obs_buffer_length + 1)) {
_baro_hgt_offset = _baro_sample_delayed.hgt;
} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data
} else if (_baro_counter > (uint8_t)(_obs_buffer_length + 1)) {
_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
}
}
}
// check to see if we have enough measurements and return false if not
bool hgt_count_fail = _hgt_counter <= 2u * _obs_buffer_length;
bool mag_count_fail = _mag_counter <= 2u * _obs_buffer_length;
const bool not_enough_baro_samples_accumulated = _baro_counter <= 2u * _obs_buffer_length;
const bool not_enough_mag_samples_accumulated = _mag_counter <= 2u * _obs_buffer_length;
if (hgt_count_fail || mag_count_fail) {
if (not_enough_baro_samples_accumulated || not_enough_mag_samples_accumulated) {
return false;
} else {
// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
setControlBaroHeight();
// reset variables that are shared with post alignment GPS checks
_gps_drift_velD = 0.0f;
_gps_alt_ref = 0.0f;

3
EKF/ekf.h
View File

@ -462,9 +462,8 @@ private:
float _gps_alt_ref{0.0f}; ///< WGS-84 height (m)
// Variables used to initialise the filter states
uint32_t _hgt_counter{0}; ///< number of height samples read during initialisation
float _rng_filt_state{0.0f}; ///< filtered height measurement (m)
bool _is_first_imu_sample{true};
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec)
AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement for instant reset(Gauss)