From d72685120c4f2ee245fe08a628781070f2d70e44 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 22 Sep 2025 18:57:54 -0800 Subject: [PATCH] move init --- .../ekf2/EKF/aid_sources/range_finder/range_height_control.cpp | 3 +-- src/modules/ekf2/EKF/ekf.cpp | 3 ++- src/modules/ekf2/EKF/estimator_interface.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) 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 4c6c585cb3..a5b30676ac 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 @@ -46,8 +46,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) return; } - // TODO: move setting params to init function - _rng_consistency_check.setGate(_params.ekf2_rng_k_gate); + // Apply rotation to earth frame _range_sensor.updateSensorToEarthRotation(_R_to_earth); // TODO: why isn't this being done anywhere? diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 27c111a7b5..3cb878f691 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -402,7 +402,8 @@ void Ekf::updateParameters() _aux_global_position.updateParameters(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION #if defined (CONFIG_EKF2_RANGE_FINDER) - // TODO: set the tilt/rotation shit + // TODO: initialize static rangefinder parameters + _rng_consistency_check.setGate(_params.ekf2_rng_k_gate); #endif } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 834584121c..1c7965c077 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -381,7 +381,7 @@ protected: uint64_t _range_time_last_good_sample{}; SensorRangeFinder _range_sensor{}; - RangeFinderConsistencyCheck _rng_consistency_check; + RangeFinderConsistencyCheck _rng_consistency_check{}; #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW)