diff --git a/boards/ark/pi6x/init/rc.board_defaults b/boards/ark/pi6x/init/rc.board_defaults index 1b92949d1d..6c778b3ee8 100644 --- a/boards/ark/pi6x/init/rc.board_defaults +++ b/boards/ark/pi6x/init/rc.board_defaults @@ -36,7 +36,6 @@ param set-default EKF2_MULTI_IMU 0 param set-default EKF2_OF_CTRL 1 param set-default EKF2_OF_N_MIN 0.05 param set-default EKF2_RNG_A_HMAX 25 -param set-default EKF2_RNG_QLTY_T 0.1 param set-default SENS_FLOW_RATE 150 param set-default SENS_IMU_MODE 1 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 35d56422f2..6fb2a39afa 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.setQualityHysteresis(_params.ekf2_rng_qlty_t); _range_sensor.setMaxFogDistance(_params.ekf2_rng_fog); - _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); + _range_sensor.runChecks(imu_sample.time_us, _R_to_earth, _control_status.flags.in_air); if (_range_sensor.isDataHealthy()) { // correct the range data for position offset relative to the IMU @@ -76,17 +76,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _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, imu_sample.time_us); } - - } else { - // If we are supposed to be using range finder data but have bad range measurements - // and are on the ground, then synthesise a measurement at the expected on ground value - if (!_control_status.flags.in_air - && _range_sensor.isRegularlySendingData() - && _range_sensor.isDataReady()) { - - _range_sensor.setRange(_params.ekf2_min_rng); - _range_sensor.setValidity(true); // bypass the checks - } } _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index ab1c747e01..17c353d4d2 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -47,8 +47,10 @@ namespace estimator namespace sensor { -void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth) +void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth, bool in_air) { + _in_air = in_air; + updateSensorToEarthRotation(R_to_earth); updateValidity(current_time_us); } @@ -64,7 +66,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) { updateDtDataLpf(current_time_us); - if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) { + if (isSampleOutOfDate(current_time_us) || !isDataContinuous()) { _is_sample_valid = false; _is_regularly_sending_data = false; return; @@ -76,7 +78,6 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) if (_is_sample_ready) { _is_sample_valid = false; - _time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us; if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) { return; @@ -92,8 +93,16 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) } } -bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const +bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) { + // Mark quality as OK while on the ground + if (!_in_air) { + _sample.rng = _rng_valid_min_val; // set to min val while on ground + return true; + } + + _time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us; + return current_time_us - _time_bad_quality_us > _quality_hyst_us; } diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index e3e4f6e6cd..f57510029d 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -66,7 +66,7 @@ public: SensorRangeFinder() = default; ~SensorRangeFinder() override = default; - void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth); + void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth, bool in_air); bool isHealthy() const override { return _is_sample_valid; } bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; } bool isDataReady() const { return _is_sample_ready; } @@ -120,8 +120,6 @@ public: float getValidMinVal() const { return _rng_valid_min_val; } float getValidMaxVal() const { return _rng_valid_max_val; } - void setFaulty(bool faulty = true) { _is_faulty = faulty; } - private: void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth); @@ -131,7 +129,7 @@ private: bool isDataContinuous() const { return _dt_data_lpf < 2e6f; } bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } bool isDataInRange() const; - bool isQualityOk(uint64_t current_time_us) const; + bool isQualityOk(uint64_t current_time_us); void updateStuckCheck(); void updateFogCheck(const float dist_bottom, const uint64_t time_us); @@ -141,7 +139,6 @@ private: bool _is_sample_valid{}; ///< true if range finder sample retrieved from buffer is valid bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval uint64_t _time_last_valid_us{}; ///< time the last range finder measurement was ready (uSec) - bool _is_faulty{false}; ///< the sensor should not be used anymore /* * Stuck check @@ -177,6 +174,7 @@ private: */ 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) + bool _in_air{}; /* * Fog check diff --git a/src/modules/ekf2/params_terrain.yaml b/src/modules/ekf2/params_terrain.yaml index 5fc361c5cd..6a0b61295d 100644 --- a/src/modules/ekf2/params_terrain.yaml +++ b/src/modules/ekf2/params_terrain.yaml @@ -28,7 +28,7 @@ parameters: where the range finder may be inside its minimum measurements distance when on ground. type: float - default: 0.1 + default: 0.01 min: 0.01 unit: m decimal: 2