From c1fb142e726cc77b89bdb9130a6a080f7cf485b1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Jun 2024 12:33:17 -0400 Subject: [PATCH] ekf2: directly use IMU sample to find corresponding aid source sample - I think this helps make it clear we're using a sensor sample corresponding with a particular IMU sample --- .../ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp | 4 ++-- .../aid_sources/barometer/baro_height_control.cpp | 6 +++--- .../EKF/aid_sources/external_vision/ev_control.cpp | 2 +- .../EKF/aid_sources/magnetometer/mag_control.cpp | 10 +++++----- .../range_finder/range_height_control.cpp | 12 ++++++------ src/modules/ekf2/EKF/control.cpp | 4 ++-- src/modules/ekf2/EKF/ekf.h | 6 +++--- src/modules/ekf2/EKF/height_control.cpp | 2 +- 8 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp index 132aaa10559b..e05e7a9a95ed 100644 --- a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp @@ -33,12 +33,12 @@ #include "ekf.h" -void Ekf::controlAuxVelFusion() +void Ekf::controlAuxVelFusion(const imuSample &imu_sample) { if (_auxvel_buffer) { auxVelSample sample; - if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) { + if (_auxvel_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { updateAidSourceStatus(_aid_src_aux_vel, sample.time_us, // sample timestamp diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 9153856009fd..c6c6f220de70 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -49,7 +49,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) baroSample baro_sample; - if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) { + if (_baro_buffer && _baro_buffer->pop_first_older_than(imu_sample.time_us, &baro_sample)) { #if defined(CONFIG_EKF2_BARO_COMPENSATION) const float measurement = compensateBaroForDynamicPressure(imu_sample, baro_sample.hgt); @@ -137,7 +137,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) // reset vertical velocity resetVerticalVelocityToZero(); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else if (is_fusion_failing) { // Some other height source is still working @@ -166,7 +166,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); } - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; bias_est.setFusionActive(); _control_status.flags.baro_hgt = true; } diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index b41c2d4b5959..5b7fc27c46b1 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -46,7 +46,7 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample) // Check for new external vision data extVisionSample ev_sample; - if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) { + if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)) { bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter); diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 74d017add5f9..2fc094f84474 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -41,7 +41,7 @@ #include -void Ekf::controlMagFusion() +void Ekf::controlMagFusion(const imuSample &imu_sample) { static constexpr const char *AID_SRC_NAME = "mag"; estimator_aid_source3d_s &aid_src = _aid_src_mag; @@ -59,7 +59,7 @@ void Ekf::controlMagFusion() magSample mag_sample; - if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) { + if (_mag_buffer && _mag_buffer->pop_first_older_than(imu_sample.time_us, &mag_sample)) { if (mag_sample.reset || (_mag_counter == 0)) { // sensor or calibration has changed, reset low pass filter @@ -184,7 +184,7 @@ void Ekf::controlMagFusion() if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) { ECL_INFO("reset to %s", AID_SRC_NAME); resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else { // The normal sequence is to fuse the magnetometer data first before fusing @@ -212,7 +212,7 @@ void Ekf::controlMagFusion() if (no_ne_aiding_or_pre_takeoff) { ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else { ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); @@ -241,7 +241,7 @@ void Ekf::controlMagFusion() bool reset_heading = !_control_status.flags.yaw_align; resetMagStates(_mag_lpf.getState(), reset_heading); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; if (reset_heading) { _control_status.flags.yaw_align = true; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 104ec47d9190..c090d4fb1faf 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -39,7 +39,7 @@ #include "ekf.h" #include "ekf_derivation/generated/compute_hagl_innov_var.h" -void Ekf::controlRangeHaglFusion() +void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) { static constexpr const char *HGT_SRC_NAME = "RNG"; @@ -47,7 +47,7 @@ void Ekf::controlRangeHaglFusion() if (_range_buffer) { // Get range data from buffer and check validity - rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress()); + rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); _range_sensor.setDataReadiness(rng_data_ready); // update range sensor angle parameters in case they have changed @@ -55,7 +55,7 @@ void Ekf::controlRangeHaglFusion() _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.runChecks(_time_delayed_us, _R_to_earth); + _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); if (_range_sensor.isDataHealthy()) { // correct the range data for position offset relative to the IMU @@ -72,7 +72,7 @@ void Ekf::controlRangeHaglFusion() _rng_consistency_check.setGate(_params.range_kin_consistency_gate); _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), - P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us); + P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us); } } else { @@ -151,7 +151,7 @@ void Ekf::controlRangeHaglFusion() _control_status.flags.rng_hgt = true; stopRngTerrFusion(); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } } else { @@ -183,7 +183,7 @@ void Ekf::controlRangeHaglFusion() // reset vertical velocity resetVerticalVelocityToZero(); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else if (is_fusion_failing) { // Some other height source is still working diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 37d13a32e36b..b8aca027e769 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -102,7 +102,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_MAGNETOMETER) // control use of observations for aiding - controlMagFusion(); + controlMagFusion(imu_delayed); #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -142,7 +142,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_AUXVEL) // Additional horizontal velocity data from an auxiliary sensor can be fused - controlAuxVelFusion(); + controlAuxVelFusion(imu_delayed); #endif // CONFIG_EKF2_AUXVEL // #if defined(CONFIG_EKF2_TERRAIN) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b9f9525b9bd3..b54ef3a5b2ec 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -825,7 +825,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_RANGE_FINDER) // range height - void controlRangeHaglFusion(); + void controlRangeHaglFusion(const imuSample &imu_delayed); bool isConditionalRangeAidSuitable(); void stopRngHgtFusion(); void stopRngTerrFusion(); @@ -982,7 +982,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations - void controlMagFusion(); + void controlMagFusion(const imuSample &imu_sample); bool checkHaglYawResetReq() const; @@ -1015,7 +1015,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_AUXVEL) // control fusion of auxiliary velocity observations - void controlAuxVelFusion(); + void controlAuxVelFusion(const imuSample &imu_sample); void stopAuxVelFusion(); #endif // CONFIG_EKF2_AUXVEL diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index a2a128b8de82..50d0464606c7 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) - controlRangeHaglFusion(); + controlRangeHaglFusion(imu_delayed); #endif // CONFIG_EKF2_RANGE_FINDER checkHeightSensorRefFallback();