diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 4cc480be82..6b20d6ce44 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -86,9 +86,8 @@ void Ekf::controlFusionModes() } if (_baro_buffer) { - // check for intermittent data (before pop_first_older_than) - const baroSample &baro_init = _baro_buffer->get_newest(); - _baro_hgt_faulty = !isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL); + // check for intermittent data + _baro_hgt_faulty = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL); const uint64_t baro_time_prev = _baro_sample_delayed.time_us; _baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); @@ -102,8 +101,7 @@ void Ekf::controlFusionModes() if (_gps_buffer) { - const gpsSample &gps_init = _gps_buffer->get_newest(); - _gps_hgt_intermittent = !isRecent(gps_init.time_us, 2 * GPS_MAX_INTERVAL); + _gps_intermittent = !isRecent(_time_last_gps, 2 * GPS_MAX_INTERVAL); // check for arrival of new sensor data at the fusion time horizon _time_prev_gps_us = _gps_sample_delayed.time_us; @@ -542,7 +540,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) && _control_status.flags.tilt_align && gps_checks_passing && !is_gps_yaw_data_intermittent - && !_gps_hgt_intermittent; + && !_gps_intermittent; _time_last_gps_yaw_data = _time_last_imu; @@ -645,7 +643,7 @@ void Ekf::controlHeightSensorTimeouts() const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION); // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data - const bool reset_to_gps = !_gps_hgt_intermittent && + const bool reset_to_gps = !_gps_intermittent && ((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty); if (reset_to_gps) { @@ -684,7 +682,7 @@ void Ekf::controlHeightSensorTimeouts() // if baro data is acceptable and GPS data is inaccurate, reset height to baro const bool reset_to_baro = !_baro_hgt_faulty && - ((baro_data_consistent && !gps_hgt_accurate) || _gps_hgt_intermittent); + ((baro_data_consistent && !gps_hgt_accurate) || _gps_intermittent); if (reset_to_baro) { startBaroHgtFusion(); @@ -693,7 +691,7 @@ void Ekf::controlHeightSensorTimeouts() failing_height_source = "gps"; new_height_source = "baro"; - } else if (!_gps_hgt_intermittent) { + } else if (!_gps_intermittent) { request_height_reset = true; failing_height_source = "gps"; new_height_source = "gps"; @@ -878,7 +876,7 @@ void Ekf::controlHeightFusion() } else { if (!_control_status.flags.gps_hgt) { - if (!_gps_hgt_intermittent && _gps_checks_passed) { + if (!_gps_intermittent && _gps_checks_passed) { // In fallback mode and GPS has recovered so start using it startGpsHgtFusion(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a895c43227..386db11826 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -543,7 +543,7 @@ private: // height sensor status bool _baro_hgt_faulty{true}; ///< true if valid baro data is unavailable for use - bool _gps_hgt_intermittent{true}; ///< true if gps height into the buffer is intermittent + bool _gps_intermittent{true}; ///< true if into the buffer is intermittent // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9867fdb5e8..01156f15af 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -296,7 +296,7 @@ void Ekf::resetHeight() } else if (_control_status.flags.gps_hgt) { // initialize vertical position and velocity with newest gps measurement - if (!_gps_hgt_intermittent && _gps_buffer) { + if (!_gps_intermittent && _gps_buffer) { const gpsSample &gps_newest = _gps_buffer->get_newest(); resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref); @@ -329,7 +329,7 @@ void Ekf::resetHeight() } // reset the vertical velocity state - if (_control_status.flags.gps && !_gps_hgt_intermittent && _gps_buffer) { + if (_control_status.flags.gps && !_gps_intermittent && _gps_buffer) { // If we are using GPS, then use it to reset the vertical velocity const gpsSample &gps_newest = _gps_buffer->get_newest(); resetVerticalVelocityTo(gps_newest.vel(2)); @@ -1363,7 +1363,7 @@ void Ekf::updateBaroHgtBias() _baro_b_est.predict(dt); } - if (_gps_data_ready && !_gps_hgt_intermittent + if (_gps_data_ready && !_gps_intermittent && _gps_checks_passed && _NED_origin_initialised && !_baro_hgt_faulty) { // Use GPS altitude as a reference to compute the baro bias measurement