zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Prevent use of non time-stamped invalid data during initialisation 

Fixes bad height initialisation seen intermittently with snapdragon
master
Paul Riseborough 9 years ago
parent
3c84d37175
commit
34ffffa021
1 changed files with 8 additions and 8 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      EKF/ekf.cpp

16
EKF/ekf.cpp
Unescape View File

@ -399,10 +399,10 @@ bool Ekf::initialiseFilter(void)
// Sum the magnetometer measurements // Sum the magnetometer measurements
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { 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) { if ((_mag_counter == 0) && (_mag_sample_delayed.time_us != 0)) {
// initialise the counter when we start getting data from the buffer // initialise the counter when we start getting data from the buffer
_mag_counter = 1; _mag_counter = 1;
} else if (_mag_counter != 0) { } else if ((_mag_counter != 0) && (_mag_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement // increment the sample count and apply a LPF to the measurement
_mag_counter ++; _mag_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed // don't start using data until we can be certain all bad initial data has been flushed
@ -418,7 +418,7 @@ bool Ekf::initialiseFilter(void)
// Count the number of external vision measurements received // Count the number of external vision measurements received
if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) { if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
if (_ev_counter == 0 && _ev_sample_delayed.time_us !=0) { if ((_ev_counter == 0) && (_ev_sample_delayed.time_us != 0)) {
// initialise the counter // initialise the counter
_ev_counter = 1; _ev_counter = 1;
// set the height fusion mode to use external vision data when we start getting valid data from the buffer // set the height fusion mode to use external vision data when we start getting valid data from the buffer
@ -428,7 +428,7 @@ bool Ekf::initialiseFilter(void)
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true; _control_status.flags.ev_hgt = true;
} }
} else if (_ev_counter != 0) { } else if ((_ev_counter != 0) && (_ev_sample_delayed.time_us != 0)) {
// increment the sample count // increment the sample count
_ev_counter ++; _ev_counter ++;
} }
@ -442,14 +442,14 @@ bool Ekf::initialiseFilter(void)
// accumulate enough height measurements to be confident in the qulaity of the data // accumulate enough height measurements to be confident in the qulaity of the data
if (_primary_hgt_source == VDIST_SENSOR_RANGE) { if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
if (_hgt_counter == 0 && _range_sample_delayed.time_us != 0) { if ((_hgt_counter == 0) && (_range_sample_delayed.time_us != 0)) {
// initialise the counter height fusion method when we start getting data from the buffer // initialise the counter height fusion method when we start getting data from the buffer
_control_status.flags.baro_hgt = false; _control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true; _control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
_hgt_counter = 1; _hgt_counter = 1;
} else if (_hgt_counter != 0) { } else if ((_hgt_counter != 0) && (_range_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement // increment the sample count and apply a LPF to the measurement
_hgt_counter ++; _hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed // don't start using data until we can be certain all bad initial data has been flushed
@ -467,13 +467,13 @@ bool Ekf::initialiseFilter(void)
// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS // if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
// later when it passes checks. // later when it passes checks.
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { 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) { 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 // initialise the counter and height fusion method when we start getting data from the buffer
_control_status.flags.baro_hgt = true; _control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_hgt_counter = 1; _hgt_counter = 1;
} else if (_hgt_counter != 0) { } else if ((_hgt_counter != 0) && (_baro_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement // increment the sample count and apply a LPF to the measurement
_hgt_counter ++; _hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed // don't start using data until we can be certain all bad initial data has been flushed

Write Preview
Loading…
Cancel
Save