From c47b3ba1b048284263117ccbd50417d6a9a4c7b7 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Tue, 19 Aug 2025 16:51:57 +0200 Subject: [PATCH] * parameter renaming * move hor-motion check to method --- .../range_finder/range_finder_consistency_check.cpp | 9 +++++++++ .../range_finder/range_finder_consistency_check.hpp | 2 +- .../aid_sources/range_finder/range_height_control.cpp | 2 +- src/modules/ekf2/EKF/common.h | 2 +- src/modules/ekf2/EKF2.cpp | 2 +- src/modules/ekf2/EKF2.hpp | 2 +- 6 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index 2a455185e1..56112b28b4 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -181,3 +181,12 @@ void RangeFinderConsistencyCheck::reset() _initialized = false; } + +void RangeFinderConsistencyCheck::setHorizontalMotion(const bool horizontal_motion) +{ + if (!horizontal_motion && getHorizontalMotion()) { + reset(); + } + + _horizontal_motion = horizontal_motion; +} diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp index 4c948e3740..00b7920b3e 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp @@ -65,7 +65,7 @@ public: void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; } void reset(); - void setHorizontalMotion(const bool horizontal_motion) { _horizontal_motion = horizontal_motion; } + void setHorizontalMotion(const bool horizontal_motion); bool getHorizontalMotion() const { return _horizontal_motion; } void setIsLanded(bool landed); 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 5612348a30..49d6b0d29a 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 @@ -57,7 +57,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t); _range_sensor.setMaxFogDistance(_params.ekf2_rng_fog); - _rng_consistency_check.setGate(_params.range_kin_consistency_gate); + _rng_consistency_check.setGate(_params.ekf2_rng_k_gate); _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index d05ef6aac5..9420a05767 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -425,7 +425,7 @@ struct parameters { float ekf2_rng_a_vmax{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) float ekf2_rng_qlty_t{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data - float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check + float ekf2_rng_k_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check float ekf2_rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m] Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 3196818ce2..9fd6368edd 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -163,7 +163,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_vmax(_params->ekf2_rng_a_vmax), _param_ekf2_rng_a_hmax(_params->ekf2_rng_a_hmax), _param_ekf2_rng_qlty_t(_params->ekf2_rng_qlty_t), - _param_range_kin_consistency_gate(_params->range_kin_consistency_gate), + _param_ekf2_rng_k_gate(_params->ekf2_rng_k_gate), _param_ekf2_rng_fog(_params->ekf2_rng_fog), _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 466d6d9717..c95e3c3623 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -622,7 +622,7 @@ private: (ParamExtFloat) _param_ekf2_rng_a_vmax, (ParamExtFloat) _param_ekf2_rng_a_hmax, (ParamExtFloat) _param_ekf2_rng_qlty_t, - (ParamExtFloat) _param_range_kin_consistency_gate, + (ParamExtFloat) _param_ekf2_rng_k_gate, (ParamExtFloat) _param_ekf2_rng_fog, (ParamExtFloat) _param_ekf2_rng_pos_x, (ParamExtFloat) _param_ekf2_rng_pos_y,