diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index e15cb646a5..68d709483f 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -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 { diff --git a/EKF/ekf.h b/EKF/ekf.h index 66691d5b46..55ffa5c363 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -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