diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index 31c133ade983..0f570badef69 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -73,7 +73,7 @@ static constexpr uint8_t LL40LS_UNIT_ID_LOW = 0x17; static constexpr uint8_t LL40LS_SIG_COUNT_VAL_REG = 0x02; /* Maximum acquisition count register */ static constexpr uint8_t LL40LS_SIG_COUNT_VAL_MAX = 0xFF; /* Maximum acquisition count max value */ -static constexpr int LL40LS_SIGNAL_STRENGTH_MIN_V3HP = 70; /* Min signal strength for V3HP */ +static constexpr int LL40LS_SIGNAL_STRENGTH_MIN_V3HP = 0; /* Min signal strength for V3HP */ static constexpr int LL40LS_SIGNAL_STRENGTH_MAX_V3HP = 255; /* Max signal strength for V3HP */ static constexpr int LL40LS_SIGNAL_STRENGTH_LOW = 24; /* Minimum signal strength for a valid measurement */ diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 82e74236a31b..ed76d7de7dfb 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -430,7 +430,8 @@ struct parameters { float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion - float range_valid_quality_s{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_valid_quality_s{1.0f}; ///< Minimum duration during which the reported range finder signal quality needs to be >= EKF2_RNG_QMIN in order to be declared valid (s) + float range_valid_quality_min{0.0f}; ///< Range data will only be used in air if the sensor reports a quality metric >= EKF2_RNG_QMIN. 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{1.0f}; ///< gate size used by the range finder kinematic consistency check diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f3330fed8cb2..6f5726528069 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -76,6 +76,7 @@ void Ekf::reset() _range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); + _range_sensor.setQualityMinimum(_params.range_valid_quality_min); #endif // CONFIG_EKF2_RANGE_FINDER _control_status.value = 0; diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 8fede499e5a2..92b4cb06e7ae 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -53,6 +53,7 @@ void Ekf::controlRangeHeightFusion() _range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); + _range_sensor.setQualityMinimum(_params.range_valid_quality_min); _range_sensor.runChecks(_time_delayed_us, _R_to_earth); diff --git a/src/modules/ekf2/EKF/sensor_range_finder.cpp b/src/modules/ekf2/EKF/sensor_range_finder.cpp index c47cf0f74d00..dd1ce1f09a64 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/sensor_range_finder.cpp @@ -74,7 +74,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) if (_is_sample_ready) { _is_sample_valid = false; - if (_sample.quality == 0) { + if (_sample.quality < _quality_minimum) { _time_bad_quality_us = current_time_us; } else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) { diff --git a/src/modules/ekf2/EKF/sensor_range_finder.hpp b/src/modules/ekf2/EKF/sensor_range_finder.hpp index ec642f8e98eb..0c9dd1b66981 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/sensor_range_finder.hpp @@ -94,6 +94,8 @@ class SensorRangeFinder : public Sensor _quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f); } + void setQualityMinimum(float quality_minimum) { _quality_minimum = quality_minimum; } + float getCosTilt() const { return _cos_tilt_rng_to_earth; } void setRange(float rng) { _sample.rng = rng; } @@ -161,7 +163,8 @@ class SensorRangeFinder : public Sensor * Quality check */ uint64_t _time_bad_quality_us{}; ///< timestamp at which range finder signal quality was 0 (used for hysteresis) - uint64_t _quality_hyst_us{}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us) + uint64_t _quality_hyst_us{}; ///< minimum duration during which the reported range finder signal quality needs to be >= EKF2_RNG_QMIN in order to be declared valid (us) + float _quality_minimum{}; ///< range data will only be used in air if the sensor reports a quality metric >= EKF2_RNG_QMIN. }; } // namespace sensor diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d752659a69ca..cafdfac1d842 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -159,6 +159,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), + _param_ekf2_rng_qmin(_params->range_valid_quality_min), _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), _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 060ce108dbaa..f29f2250a652 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -654,6 +654,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem (ParamExtFloat) _param_ekf2_rng_a_hmax, (ParamExtFloat) _param_ekf2_rng_a_igate, (ParamExtFloat) _param_ekf2_rng_qlty_t, + (ParamExtFloat) _param_ekf2_rng_qmin, (ParamExtFloat) _param_ekf2_rng_k_gate, (ParamExtFloat) _param_ekf2_rng_pos_x, (ParamExtFloat) _param_ekf2_rng_pos_y, diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index cc8adcca56d7..ba1bba7cf14c 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1185,7 +1185,7 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f); PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f); /** - * Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) + * Minimum duration during which the reported range finder signal quality needs to be >= EKF2_RNG_QMIN in order to be declared valid (s) * * * @group EKF2 @@ -1195,6 +1195,15 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f); */ PARAM_DEFINE_FLOAT(EKF2_RNG_QLTY_T, 1.0f); +/** + * Range data will only be used in air if the sensor reports a quality metric >= EKF2_RNG_QMIN. + * + * @group EKF2 + * @min 0 + * @max 100 +*/ +PARAM_DEFINE_FLOAT(EKF2_RNG_QMIN, 27.0f); + /** * Gate size used for range finder kinematic consistency check *