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

EKF: don't initialise filter without EV data if we are relying on it

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-18 19:18:07 +10:00
parent 50ba26a434
commit 22fba0fc6e
2 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
EKF/ekf.cpp
View File

@ -92,6 +92,7 @@ Ekf::Ekf():
_hgt_counter(0),
_rng_filt_state(0.0f),
_mag_counter(0),
_ev_counter(0),
_time_last_mag(0),
_hgt_sensor_offset(0.0f),
_terrain_vpos(0.0f),
@ -387,7 +388,7 @@ bool Ekf::update()
bool Ekf::initialiseFilter(void)
{
// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
// Sum the IMU delta angle measurements
imuSample imu_init = _imu_buffer.get_newest();
@ -406,6 +407,17 @@ bool Ekf::initialiseFilter(void)
}
}
// 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 (_ev_counter == 0 && _ev_sample_delayed.time_us !=0) {
// initialise the counter
_ev_counter = 1;
} else if (_ev_counter != 0) {
// increment the sample count
_ev_counter ++;
}
}
// set the default height source from the adjustable parameter
if (_hgt_counter == 0) {
if (_params.fusion_mode & MASK_USE_EVPOS) {
@ -454,7 +466,10 @@ bool Ekf::initialiseFilter(void)
}
// check to see if we have enough measurements and return false if not
if (_hgt_counter <= 10 || _mag_counter <= 10) {
bool hgt_count_fail = _hgt_counter <= 10;
bool mag_count_fail = _mag_counter <= 10;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 10);
if (hgt_count_fail || mag_count_fail || ev_count_fail) {
return false;
} else {

5
EKF/ekf.h
View File

@ -209,9 +209,10 @@ private:
float _gps_alt_ref; // WGS-84 height (m)
// Variables used to initialise the filter states
uint32_t _hgt_counter; // number of height samples taken
uint32_t _hgt_counter; // number of height samples read during initialisation
float _rng_filt_state; // filtered height measurement
uint32_t _mag_counter; // number of magnetometer samples taken
uint32_t _mag_counter; // number of magnetometer samples read during initialisation
uint32_t _ev_counter; // number of exgernal vision samples read during initialisation
uint64_t _time_last_mag; // measurement time of last magnetomter sample
Vector3f _mag_filt_state; // filtered magnetometer measurement
Vector3f _delVel_sum; // summed delta velocity