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

use baro for the ekf alignment if range finder is primary height source

Browse Source
This commit is contained in:
ChristophTobler 2017-06-30 10:28:57 +02:00
parent 73ef95b37f
commit ed8fa16678
1 changed files with 3 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
EKF/ekf.cpp
View File

@ -204,34 +204,9 @@ bool Ekf::initialiseFilter()
}
// accumulate enough height measurements to be confident in the qulaity of the data
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
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)) {
// initialise the counter height fusion method when we start getting data from the buffer
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
_hgt_counter = 1;
} else if ((_hgt_counter != 0) && (_range_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
_rng_filt_state = _range_sample_delayed.rng;
} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data
_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
}
}
}
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS ||
_primary_hgt_source == VDIST_SENSOR_RANGE) {
// if the user parameter specifies use of GPS/range finder for height we use baro height initially and switch to GPS/range 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)) {