From ae1e12a6b55dd475da4f7a64a7bbe10b5bd81afa Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 8 Sep 2022 16:04:51 +0200 Subject: [PATCH] ekf2: remove baro accumulation from initializeFilter --- src/modules/ekf2/EKF/baro_height_control.cpp | 41 +++++++++++++++----- src/modules/ekf2/EKF/control.cpp | 12 ------ src/modules/ekf2/EKF/ekf.cpp | 27 ------------- src/modules/ekf2/EKF/ekf.h | 6 +-- src/modules/ekf2/EKF/height_fusion.cpp | 2 +- 5 files changed, 34 insertions(+), 54 deletions(-) diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index 91870fb0a9..e014da5458 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -40,6 +40,28 @@ void Ekf::controlBaroHeightFusion() { + if (!_baro_buffer) { + return; + } + + baroSample baro_sample; + const bool baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample); + + if (baro_data_ready) { + if (_baro_counter == 0) { + _baro_lpf.reset(baro_sample.hgt); + + } else { + _baro_lpf.update(baro_sample.hgt); + } + + if (_baro_counter < _obs_buffer_length) { + // Initialize the pressure offset (included in the baro bias) + _baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState()); + _baro_counter++; + } + } + if (!(_params.baro_ctrl == 1)) { stopBaroHgtFusion(); return; @@ -50,12 +72,11 @@ void Ekf::controlBaroHeightFusion() // check for intermittent data const bool baro_hgt_intermittent = !isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); - if (_baro_data_ready) { - _baro_lpf.update(_baro_sample_delayed.hgt); - updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt); + if (baro_data_ready) { + updateBaroHgt(baro_sample, _aid_src_baro_hgt); const bool continuing_conditions_passing = !_baro_hgt_faulty && !baro_hgt_intermittent; - const bool starting_conditions_passing = continuing_conditions_passing; + const bool starting_conditions_passing = continuing_conditions_passing && (_baro_counter >= _obs_buffer_length); if (_control_status.flags.baro_hgt) { if (continuing_conditions_passing) { @@ -65,7 +86,7 @@ void Ekf::controlBaroHeightFusion() if (isHeightResetRequired()) { // All height sources are failing - resetHeightToBaro(); + resetHeightToBaro(baro_sample); resetVerticalVelocityToZero(); } else if (is_fusion_failing) { @@ -79,7 +100,7 @@ void Ekf::controlBaroHeightFusion() } } else { if (starting_conditions_passing) { - startBaroHgtFusion(); + startBaroHgtFusion(baro_sample); } } @@ -89,12 +110,12 @@ void Ekf::controlBaroHeightFusion() } } -void Ekf::startBaroHgtFusion() +void Ekf::startBaroHgtFusion(const baroSample &baro_sample) { if (!_control_status.flags.baro_hgt) { if (_params.height_sensor_ref == HeightSensor::BARO) { _height_sensor_ref = HeightSensor::BARO; - resetHeightToBaro(); + resetHeightToBaro(baro_sample); } else { _baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState()); @@ -106,12 +127,12 @@ void Ekf::startBaroHgtFusion() } } -void Ekf::resetHeightToBaro() +void Ekf::resetHeightToBaro(const baroSample &baro_sample) { ECL_INFO("reset height to baro"); _information_events.flags.reset_hgt_to_baro = true; - resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_b_est.getBias())); + resetVerticalPositionTo(-(baro_sample.hgt - _baro_b_est.getBias())); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise)); diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index f2bde288c8..c1ad433db6 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -85,18 +85,6 @@ void Ekf::controlFusionModes() } } - if (_baro_buffer) { - 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); - - // if we have a new baro sample save the delta time between this sample and the last sample which is - // used below for baro offset calculations - if (_baro_data_ready && baro_time_prev != 0) { - _delta_time_baro_us = _baro_sample_delayed.time_us - baro_time_prev; - } - } - - if (_gps_buffer) { _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index ca00fb794e..5442e9f151 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -160,33 +160,6 @@ bool Ekf::initialiseFilter() } } - // accumulate enough height measurements to be confident in the quality of the data - if (_baro_buffer) { - baroSample baro_sample; - - if (_baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) { - if (baro_sample.time_us != 0) { - if (_baro_counter == 0) { - _baro_lpf.reset(baro_sample.hgt); - - } else { - _baro_lpf.update(baro_sample.hgt); - - _baro_b_est.setBias(_baro_lpf.getState()); - } - - _baro_counter++; - } - } - } - - if (_params.baro_ctrl != 0) { - if (_baro_counter < _obs_buffer_length) { - // not enough baro samples accumulated - return false; - } - } - if (!initialiseTilt()) { return false; } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 71883f1a09..ff8a39c87c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -457,7 +457,6 @@ private: // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused - bool _baro_data_ready{false}; ///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused bool _rng_data_ready{false}; bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused @@ -483,7 +482,6 @@ private: Vector3f _last_known_pos{}; ///< last known local position vector (m) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) - uint64_t _delta_time_baro_us{0}; ///< delta time between two consecutive delayed baro samples from the buffer (uSec) Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s @@ -708,7 +706,7 @@ private: void resetVerticalPositionTo(float new_vert_pos); - void resetHeightToBaro(); + void resetHeightToBaro(const baroSample &baro_sample); void resetHeightToGps(const gpsSample &gps_sample); void resetHeightToRng(); void resetHeightToEv(); @@ -967,7 +965,7 @@ private: void startMagHdgFusion(); void startMag3DFusion(); - void startBaroHgtFusion(); + void startBaroHgtFusion(const baroSample &baro_sample); void stopBaroHgtFusion(); void startGpsHgtFusion(const gpsSample &gps_sample); diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp index e3be98581b..dc8739355d 100644 --- a/src/modules/ekf2/EKF/height_fusion.cpp +++ b/src/modules/ekf2/EKF/height_fusion.cpp @@ -48,7 +48,7 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s // measurement variance - user parameter defined const float measurement_var = sq(fmaxf(_params.baro_noise, 0.01f)); - const float measurement = _baro_sample_delayed.hgt; + const float measurement = baro_sample.hgt; // vertical position innovation - baro measurement has opposite sign to earth z axis baro_hgt.observation = -(measurement - _baro_b_est.getBias());