From ba8760a3d885089f1c6e75d1f2cf49d3ce75ca8b Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 17 Jun 2025 19:48:10 -0800 Subject: [PATCH] massive refactoring --- src/modules/ekf2/CMakeLists.txt | 2 +- .../EKF/aid_sources/range_finder/Sensor.hpp | 84 ----- .../jake_range_height_control.cpp | 340 +++--------------- .../range_finder_consistency_check.cpp | 7 +- .../range_finder/range_height_control.cpp | 10 +- .../range_finder/sensor_range_finder.cpp | 140 ++------ .../range_finder/sensor_range_finder.hpp | 117 ++---- src/modules/ekf2/EKF/common.h | 10 +- src/modules/ekf2/EKF/ekf.cpp | 19 +- src/modules/ekf2/EKF/estimator_interface.h | 2 +- src/modules/ekf2/EKF2.cpp | 13 +- src/modules/ekf2/EKF2.hpp | 1 - src/modules/ekf2/params_range_finder.yaml | 10 - .../ekf2/test/test_SensorRangeFinder.cpp | 10 +- 14 files changed, 142 insertions(+), 623 deletions(-) delete mode 100644 src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index a7b8e18f8a..36eca7ec50 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -211,7 +211,7 @@ endif() if(CONFIG_EKF2_RANGE_FINDER) list(APPEND EKF_SRCS EKF/aid_sources/range_finder/range_finder_consistency_check.cpp - EKF/aid_sources/range_finder/range_height_control.cpp + EKF/aid_sources/range_finder/jake_range_height_control.cpp EKF/aid_sources/range_finder/range_height_fusion.cpp EKF/aid_sources/range_finder/sensor_range_finder.cpp ) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp deleted file mode 100644 index 8134418fab..0000000000 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp +++ /dev/null @@ -1,84 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Sensor.hpp - * Abstract class for sensors - * - * @author Mathieu Bresciani - * - */ - -#ifndef EKF_SENSOR_HPP -#define EKF_SENSOR_HPP - -#include - -namespace estimator -{ -namespace sensor -{ - -class Sensor -{ -public: - virtual ~Sensor() {}; - - /* - * run sanity checks on the current data - * this has to be called immediately after - * setting new data - */ - virtual void runChecks() {}; - - /* - * return true if the sensor is healthy - */ - virtual bool isHealthy() const = 0; - - /* - * return true if the delayed sample is healthy - * and can be fused in the estimator - */ - virtual bool isDataHealthy() const = 0; - - /* - * return true if the sensor data rate is - * stable and high enough - */ - virtual bool isRegularlySendingData() const = 0; -}; - -} // namespace sensor -} // namespace estimator -#endif // !EKF_SENSOR_HPP diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp index 7e378ce375..e477354c6a 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp @@ -42,316 +42,74 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) { - static constexpr const char *HGT_SRC_NAME = "RNG"; - + // Check if rangefinder is available/enabled if (!_range_buffer) { return; } - // Get range data from buffer and check validity - bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); - _range_sensor.setDataReadiness(rng_data_ready); + bool sample_valid = false; + // Pop rangefinder measurement from buffer of samples into active sample + sensor::rangeSample sample = {}; + if (_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { - if (_range_sensor.isDataReady()) { - - _range_sensor.setPitchOffset(_params.rng_sens_pitch); + // Set all of the parameters + _range_sensor.setPitchOffset(_params.ekf2_rng_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.setMaxFogDistance(_params.rng_fog); _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); + // Update sensor to earth rotation + _range_sensor.updateSensorToEarthRotation(_R_to_earth); - if (_range_sensor.isDataHealthy()) { - // correct the range data for position offset relative to the IMU - const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; + // Gate sample consumption on these checks + // - Quality is OK + bool quality_ok = sample.quality > 0; + // - Tilt is OK + bool tilt_ok = _range_sensor.isTiltOk(); + // - Value is IN range + bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal(); + // - Not stuck value + // - Not fog detected + + if (quality_ok && tilt_ok && range_ok) { + + // Correct the range data for position offset relative to the IMU + const Vector3f rng_pos_body = { _params.ekf2_rng_pos_x, _params.ekf2_rng_pos_y, _params.ekf2_rng_pos_z }; + // const Vector3f imu_pos_body = { _params.ekf2_imu_pos_x, _params.ekf2_imu_pos_y, _params.ekf2_imu_pos_z }; + const Vector3f imu_pos_body = _params.imu_pos_body; + const Vector3f pos_offset_body = rng_pos_body - imu_pos_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); + sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt(); - // TODO: this is a constant - const float dist_var = getRngVar(); - _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); + // Provide sample from buffer to object + _range_sensor.setSample(sample); - const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); - const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); + // Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate - // TODO: review -- variance - _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), - dist_var, imu_sample.time_us); - - } else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) { - _range_sensor.setRange(_params.rng_gnd_clearance); - _range_sensor.setValidity(true); - - } else { - _rng_consistency_check.reset(); + // Passes kinematic consistency check? + sample_valid = true; } } - _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); - - _control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent() - && _rng_consistency_check.isNotKinematicallyInconsistent(); - - - auto &aid_src = _aid_src_rng_hgt; - - if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) { - - updateRangeHagl(aid_src); - const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); - - const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast(RngCtrl::ENABLED)) - || (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) - && _control_status.flags.tilt_align - && measurement_valid - && _range_sensor.isDataHealthy() - && _rng_consistency_check.isNotKinematicallyInconsistent(); - - // SUS: _rng_consistency_check.isNotKinematicallyInconsistent() - - const bool starting_conditions_passing = continuing_conditions_passing - && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) - && _range_sensor.isRegularlySendingData(); - - const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) - && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) - && isConditionalRangeAidSuitable(); - - // SUS: isConditionalRangeAidSuitable() - - const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) - && (_params.rng_ctrl == static_cast(RngCtrl::ENABLED)); - - if (_control_status.flags.rng_hgt) { - if (!(do_conditional_range_aid || do_range_aid)) { - PX4_INFO("stopping %s fusion", HGT_SRC_NAME); - stopRngHgtFusion(); - } - - } else { - if (_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) { - if (do_conditional_range_aid) { - // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. - PX4_INFO("starting conditional %s height fusion", HGT_SRC_NAME); - _height_sensor_ref = HeightSensor::RANGE; - - _control_status.flags.rng_hgt = true; - stopRngTerrFusion(); - - if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected - && _rng_consistency_check.isKinematicallyConsistent()) { - resetTerrainToRng(aid_src); - resetAidSourceStatusZeroInnovation(aid_src); - } - - } else if (do_range_aid) { - // Range finder is the primary height source, the ground is now the datum used - // to compute the local vertical position - PX4_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); - _height_sensor_ref = HeightSensor::RANGE; - - _information_events.flags.reset_hgt_to_rng = true; - resetAltitudeTo(aid_src.observation, aid_src.observation_variance); - _state.terrain = 0.f; - resetAidSourceStatusZeroInnovation(aid_src); - _control_status.flags.rng_hgt = true; - stopRngTerrFusion(); - - aid_src.time_last_fuse = imu_sample.time_us; - } - - } else { - if (do_conditional_range_aid || do_range_aid) { - PX4_INFO("starting %s height fusion", HGT_SRC_NAME); - _control_status.flags.rng_hgt = true; - - if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected - && _rng_consistency_check.isKinematicallyConsistent()) { - resetTerrainToRng(aid_src); - resetAidSourceStatusZeroInnovation(aid_src); - } - } - } - } - - if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) { - if (continuing_conditions_passing) { - - fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); - - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - - if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) { - // All height sources are failing - PX4_INFO("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); - - _information_events.flags.reset_hgt_to_rng = true; - resetAltitudeTo(aid_src.observation - _state.terrain); - resetAidSourceStatusZeroInnovation(aid_src); - - // reset vertical velocity if no valid sources available - if (!isVerticalVelocityAidingActive()) { - resetVerticalVelocityToZero(); - } - - aid_src.time_last_fuse = imu_sample.time_us; - - } else if (is_fusion_failing) { - // Some other height source is still working - if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) { - PX4_INFO("stopping %s fusion, fusion failing", HGT_SRC_NAME); - stopRngHgtFusion(); - stopRngTerrFusion(); - - } else if (_rng_consistency_check.isKinematicallyConsistent()) { - resetTerrainToRng(aid_src); - resetAidSourceStatusZeroInnovation(aid_src); - } - } - - } else { - PX4_INFO("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); - stopRngHgtFusion(); - stopRngTerrFusion(); - } - - } else { - if (starting_conditions_passing) { - if (_control_status.flags.opt_flow_terrain) { - if (!aid_src.innovation_rejected) { - _control_status.flags.rng_terrain = true; - fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); - } - - } else { - if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) { - resetTerrainToRng(aid_src); - resetAidSourceStatusZeroInnovation(aid_src); - } - - _control_status.flags.rng_terrain = true; - } - } - } - - } else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) - && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) { - // No data anymore. Stop until it comes back. - PX4_INFO("stopping %s fusion, no data", HGT_SRC_NAME); - stopRngHgtFusion(); - stopRngTerrFusion(); - } -} - -void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) -{ - const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance); - const float measurement_variance = getRngVar(); - - float innovation_variance; - sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance); - - const float innov_gate = math::max(_params.range_innov_gate, 1.f); - updateAidSourceStatus(aid_src, - _range_sensor.getSampleAddress()->time_us, // sample timestamp - measurement, // observation - measurement_variance, // observation variance - getHagl() - measurement, // innovation - innovation_variance, // innovation variance - innov_gate); // innovation gate - - // z special case if there is bad vertical acceleration data, then don't reject measurement, - // but limit innovation to prevent spikes that could destabilise the filter - if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { - const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance); - aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit); - aid_src.innovation_rejected = false; - } -} - -float Ekf::getRngVar() const -{ - const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); - const float dist_var = sq(_params.range_noise) + dist_dependant_var; - return dist_var; -} - -void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) -{ - // Since the distance is not a direct observation of the terrain state but is based - // on the height state, a reset should consider the height uncertainty. This can be - // done by manipulating the Kalman gain to inject all the innovation in the terrain state - // and create the correct correlation with the terrain state with a covariance update. - P.uncorrelateCovarianceSetVariance(State::terrain.idx, 0.f); - - const float old_terrain = _state.terrain; - - VectorState H; - sym::ComputeHaglH(&H); - - VectorState K; - K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset" - - measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation); - - // record the state change - const float delta_terrain = _state.terrain - old_terrain; - - if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { - _state_reset_status.hagl_change = delta_terrain; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.hagl_change += delta_terrain; + // Check if rangefinder has timed out + if (_range_sensor.timedOut(imu_sample.time_us)) { + // TODO: timeout value? + // TODO: handle timeout } - _state_reset_status.reset_count.hagl++; - - aid_src.time_last_fuse = _time_delayed_us; -} - -bool Ekf::isConditionalRangeAidSuitable() -{ - // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching - // Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice - float range_hagl_max = _params.max_hagl_for_range_aid; - float max_vel_xy = _params.max_vel_for_range_aid; - - const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio; - - bool is_hagl_stable = (hagl_test_ratio < 1.f); - - if (!_control_status.flags.rng_hgt) { - range_hagl_max = 0.7f * _params.max_hagl_for_range_aid; - max_vel_xy = 0.7f * _params.max_vel_for_range_aid; - is_hagl_stable = (hagl_test_ratio < 0.01f); + if (!sample_valid) { + // Sample is invalid + // - Timeout + // - Quality + // - Tilt + // - Range + // - KinematicConsistency + // - RangeAidChecks... + // - Horizontal Velocity + // - aid_src_rng_hgt.test_ratio + return; } - const bool is_in_range = (getHagl() < range_hagl_max); + // Publish EstimatorAidSource1d (observation, variance, rejected, fused) - bool is_below_max_speed = true; - - if (isHorizontalAidingActive()) { - is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy); - } - - return is_in_range && is_hagl_stable && is_below_max_speed; -} - -void Ekf::stopRngHgtFusion() -{ - if (_control_status.flags.rng_hgt) { - - if (_height_sensor_ref == HeightSensor::RANGE) { - _height_sensor_ref = HeightSensor::UNKNOWN; - } - - _control_status.flags.rng_hgt = false; - } -} - -void Ekf::stopRngTerrFusion() -{ - _control_status.flags.rng_terrain = false; + // Conditionally enable/disable rangefinder fusion for Altitude Estimate and Terrain Estimate } 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 01d1c48a27..65236e36d0 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 @@ -39,15 +39,12 @@ #include "ekf_derivation/generated/range_validation_filter_h.h" #include "ekf_derivation/generated/range_validation_filter_P_init.h" -#include - - using namespace matrix; void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom, const float dist_bottom_var) { - PX4_INFO("RangeFinderConsistencyCheck::init"); + printf("RangeFinderConsistencyCheck::init\n"); _P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var); _Ht = sym::RangeValidationFilterH(); @@ -140,7 +137,7 @@ void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, } if (fabsf(_test_ratio_lpf.getState()) > 1.f) { - PX4_INFO("_test_ratio_lpf failed (>1)"); + printf("_test_ratio_lpf failed (>1)\n"); _t_since_first_sample = 0.f; _state = KinematicState::INCONSISTENT; return; 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 7e378ce375..92629a130b 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 @@ -49,14 +49,16 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } // Get range data from buffer and check validity - bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); + bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.sample()); + + _range_sensor.setSample(); + _range_sensor.setDataReadiness(rng_data_ready); if (_range_sensor.isDataReady()) { _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.setMaxFogDistance(_params.rng_fog); _rng_consistency_check.setGate(_params.range_kin_consistency_gate); @@ -96,7 +98,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) auto &aid_src = _aid_src_rng_hgt; - if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) { + if (_range_sensor.isDataReady() && _range_sensor.sample()) { updateRangeHagl(aid_src); const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); @@ -254,7 +256,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) const float innov_gate = math::max(_params.range_innov_gate, 1.f); updateAidSourceStatus(aid_src, - _range_sensor.getSampleAddress()->time_us, // sample timestamp + _range_sensor.sample()->time_us, // sample timestamp measurement, // observation measurement_variance, // observation variance getHagl() - measurement, // innovation 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 891447a4cd..1984dea688 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,10 +47,34 @@ namespace estimator namespace sensor { -void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth) +void SensorRangeFinder::setSample(const rangeSample &sample) { - updateSensorToEarthRotation(R_to_earth); - updateValidity(current_time_us); + _sample = sample; +} + +bool SensorRangeFinder::timedOut(uint64_t time_now) const +{ + if (_sample.time_us > time_now) { + return false; + } + + // TODO: 200ms? + return time_now - _sample.time_us > 200'000; +} + +void SensorRangeFinder::setPitchOffset(float new_pitch_offset) +{ + if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) { + _sin_pitch_offset = sinf(new_pitch_offset); + _cos_pitch_offset = cosf(new_pitch_offset); + _pitch_offset_rad = new_pitch_offset; + } +} + +void SensorRangeFinder::setLimits(float min_distance, float max_distance) +{ + _rng_valid_min_val = min_distance; + _rng_valid_max_val = max_distance; } void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth) @@ -60,115 +84,5 @@ void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_ear _cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset; } -void SensorRangeFinder::updateValidity(uint64_t current_time_us) -{ - updateDtDataLpf(current_time_us); - - if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) { - _is_sample_valid = false; - _is_regularly_sending_data = false; - return; - } - - _is_regularly_sending_data = true; - - // Don't run the checks unless we have retrieved new data from the buffer - 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; - } - - updateStuckCheck(); - updateFogCheck(getDistBottom(), _sample.time_us); - - if (!_is_stuck && !_is_blocked) { - _is_sample_valid = true; - _time_last_valid_us = _sample.time_us; - } - } -} - -bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const -{ - return current_time_us - _time_bad_quality_us > _quality_hyst_us; -} - -void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us) -{ - // Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant. - float alpha = 0.5f * _dt_update; - _dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us); - - // Apply spike protection to the filter state. - _dt_data_lpf = fminf(_dt_data_lpf, 4e6f); -} - -inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const -{ - return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL; -} - -inline bool SensorRangeFinder::isDataInRange() const -{ - return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val); -} - -void SensorRangeFinder::updateStuckCheck() -{ - if (!isStuckDetectorEnabled()) { - // Stuck detector disabled - _is_stuck = false; - return; - } - - // Check for "stuck" range finder measurements when range was not valid for certain period - // This handles a failure mode observed with some lidar sensors - if (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) { - - // require a variance of rangefinder values to check for "stuck" measurements - if (_stuck_max_val - _stuck_min_val > _stuck_threshold) { - _stuck_min_val = 0.0f; - _stuck_max_val = 0.0f; - _is_stuck = false; - - } else { - if (_sample.rng > _stuck_max_val) { - _stuck_max_val = _sample.rng; - } - - if (_stuck_min_val < 0.1f || _sample.rng < _stuck_min_val) { - _stuck_min_val = _sample.rng; - } - - _is_stuck = true; - } - } -} - -void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us) -{ - if (_max_fog_dist > 0.f) { - - const float median_dist = _median_dist.apply(dist_bottom); - const float factor = 2.f; // magic hardcoded factor - - if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) { - _is_blocked = true; - - } else if (_is_blocked && median_dist > factor * _max_fog_dist) { - _is_blocked = false; - } - - _prev_median_dist = median_dist; - - } else { - _prev_median_dist = dist_bottom; - } -} - } // namespace sensor } // namespace estimator 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..0e92012f1c 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 @@ -41,8 +41,6 @@ #ifndef EKF_SENSOR_RANGE_FINDER_HPP #define EKF_SENSOR_RANGE_FINDER_HPP -#include "Sensor.hpp" - #include #include @@ -57,105 +55,62 @@ struct rangeSample { int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. }; -static constexpr uint64_t RNG_MAX_INTERVAL = - 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) -class SensorRangeFinder : public Sensor +class SensorRangeFinder { public: SensorRangeFinder() = default; - ~SensorRangeFinder() override = default; + ~SensorRangeFinder() = default; - void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth); - 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; } - bool isRegularlySendingData() const override { return _is_regularly_sending_data; } - bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; } + struct Parameters { + float ekf2_imu_pos_x{}; + float ekf2_imu_pos_y{}; + float ekf2_imu_pos_z{}; + + float ekf2_rng_pos_x{}; + float ekf2_rng_pos_y{}; + float ekf2_rng_pos_z{}; + + float ekf2_rng_pitch{}; + + float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt + }; + + void updateParameters(Parameters& parameters) { _parameters = parameters; }; + + bool timedOut(uint64_t time_now) const; + void setSample(const rangeSample &sample); - void setSample(const rangeSample &sample) - { - _sample = sample; - _is_sample_ready = true; - } // This is required because of the ring buffer // TODO: move the ring buffer here - rangeSample *getSampleAddress() { return &_sample; } + rangeSample *sample() { return &_sample; } - void setPitchOffset(float new_pitch_offset) - { - if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) { - _sin_pitch_offset = sinf(new_pitch_offset); - _cos_pitch_offset = cosf(new_pitch_offset); - _pitch_offset_rad = new_pitch_offset; - } - } + void setPitchOffset(float new_pitch_offset); void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; } - void setLimits(float min_distance, float max_distance) - { - _rng_valid_min_val = min_distance; - _rng_valid_max_val = max_distance; - } - - void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; } - - void setQualityHysteresis(float valid_quality_threshold_s) - { - _quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f); - } - float getCosTilt() const { return _cos_tilt_rng_to_earth; } - void setRange(float rng) { _sample.rng = rng; } - float getRange() const { return _sample.rng; } + void setLimits(float min_distance, float max_distance); + + // void setRange(float rng) { _sample.rng = rng; } + // float getRange() const { return _sample.rng; } float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; } - void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; } - void setValidity(bool is_valid) { _is_sample_valid = is_valid; } - 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); - void updateValidity(uint64_t current_time_us); - void updateDtDataLpf(uint64_t current_time_us); - bool isSampleOutOfDate(uint64_t current_time_us) const; - 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; - void updateStuckCheck(); - void updateFogCheck(const float dist_bottom, const uint64_t time_us); + +private: rangeSample _sample{}; - bool _is_sample_ready{}; ///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused - 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 - */ - bool _is_stuck{}; - float _stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m), set to zero to disable - float _stuck_min_val{}; ///< minimum value for new rng measurement when being stuck - float _stuck_max_val{}; ///< maximum value for new rng measurement when being stuck - - /* - * Data regularity check - */ - static constexpr float _dt_update{0.01f}; ///< delta time since last ekf update TODO: this should be a parameter - float _dt_data_lpf{}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec) + Parameters _parameters{}; /* * Tilt check @@ -172,20 +127,6 @@ private: float _rng_valid_min_val{}; ///< minimum distance that the rangefinder can measure (m) float _rng_valid_max_val{}; ///< maximum distance that the rangefinder can measure (m) - /* - * 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) - - /* - * Fog check - */ - bool _is_blocked{false}; - float _max_fog_dist{0.f}; //< maximum distance at which the range finder could detect fog (m) - math::MedianFilter _median_dist{}; - float _prev_median_dist{0.f}; - }; } // namespace sensor diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index b4452b0bee..6d275d588f 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -415,17 +415,19 @@ struct parameters { float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float range_noise{0.1f}; ///< observation noise for range finder measurements (m) float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) - float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. - float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m) + float ekf2_rng_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. + float ekf2_rng_sfe{0.0f}; ///< scaling from range measurement to noise (m/m) 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_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 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) + float ekf2_rng_pos_x{0.f}; + float ekf2_rng_pos_y{0.f}; + float ekf2_rng_pos_z{0.f}; + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 5444886870..357b257c83 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -77,13 +77,6 @@ void Ekf::reset() _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; #endif // CONFIG_EKF2_TERRAIN -#if defined(CONFIG_EKF2_RANGE_FINDER) - _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.setMaxFogDistance(_params.rng_fog); -#endif // CONFIG_EKF2_RANGE_FINDER - _control_status.value = 0; _control_status_prev.value = 0; @@ -390,6 +383,18 @@ void Ekf::updateParameters() #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) _aux_global_position.updateParameters(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +#if defined (CONFIG_EKF2_RANGE_FINDER) + + sensor::SensorRangeFinder::Parameters params = {}; + // params.ekf2_imu_pos_x = _param_ekf2_imu_pos_x.get(); + // params.ekf2_imu_pos_y = _param_ekf2_imu_pos_y.get(); + // params.ekf2_imu_pos_z = _param_ekf2_imu_pos_z.get(); + // params.ekf2_rng_pos_x = _param_ekf2_rng_pos_x.get(); + // params.ekf2_rng_pos_y = _param_ekf2_rng_pos_y.get(); + // params.ekf2_rng_pos_z = _param_ekf2_rng_pos_z.get(); + params.ekf2_rng_pitch = _params.ekf2_rng_pitch; + _range_sensor.updateParameters(params); +#endif } template diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 0a8272a929..080dcba60b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -121,7 +121,7 @@ public: _range_sensor.setLimits(min_distance, max_distance); } - const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } + const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.sample()); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b4fc5f2458..fe5fadce5e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -155,18 +155,17 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_ctrl(_params->rng_ctrl), _param_ekf2_rng_delay(_params->range_delay_ms), _param_ekf2_rng_noise(_params->range_noise), - _param_ekf2_rng_sfe(_params->range_noise_scaler), + _param_ekf2_rng_sfe(_params->ekf2_rng_sfe), _param_ekf2_rng_gate(_params->range_innov_gate), - _param_ekf2_rng_pitch(_params->rng_sens_pitch), + _param_ekf2_rng_pitch(_params->ekf2_rng_pitch), _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), _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_k_gate(_params->range_kin_consistency_gate), _param_ekf2_rng_fog(_params->rng_fog), - _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), - _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), - _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), + _param_ekf2_rng_pos_x(_params->ekf2_rng_pos_x), + _param_ekf2_rng_pos_y(_params->ekf2_rng_pos_y), + _param_ekf2_rng_pos_z(_params->ekf2_rng_pos_z), #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) _param_ekf2_ev_delay(_params->ev_delay_ms), @@ -1610,8 +1609,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f); // TODO: review, we should use sensor variance not terrain lpos.dist_bottom_var = _ekf.getTerrainVariance(); - // Variance can be hardcoded to: EKF2_RNG_NOISE + (distance * EKF2_RNG_SFE) - // lpos.dist_bottom_var = _params->range_noise + (_params->range_noise_scaler); _ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index deb08c5a60..de813e8a0b 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -619,7 +619,6 @@ private: (ParamExtFloat) _param_ekf2_rng_a_vmax, (ParamExtFloat) _param_ekf2_rng_a_hmax, (ParamExtFloat) _param_ekf2_rng_a_igate, - (ParamExtFloat) _param_ekf2_rng_qlty_t, (ParamExtFloat) _param_ekf2_rng_k_gate, (ParamExtFloat) _param_ekf2_rng_fog, (ParamExtFloat) _param_ekf2_rng_pos_x, diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 68f8a9107f..fbd1f6db3f 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/src/modules/ekf2/params_range_finder.yaml @@ -93,16 +93,6 @@ parameters: unit: SD min: 0.1 max: 5.0 - EKF2_RNG_QLTY_T: - description: - short: Minumum range validity period - long: Minimum duration during which the reported range finder signal quality - needs to be non-zero in order to be declared valid (s) - type: float - default: 1.0 - unit: s - min: 0.1 - max: 5 EKF2_RNG_K_GATE: description: short: Gate size used for range finder kinematic consistency check diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index ecf69d83b5..74e35565e9 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -51,7 +51,6 @@ public: _range_finder.setPitchOffset(0.f); _range_finder.setCosMaxTilt(0.707f); _range_finder.setLimits(_min_range, _max_range); - _range_finder.setMaxFogDistance(2.f); } // Use this method to clean up any memory, network etc. after each test @@ -118,9 +117,8 @@ TEST_F(SensorRangeFinderTest, setRange) sample.time_us = 1e6; sample.quality = 9; - _range_finder.setRange(sample.rng); - _range_finder.setDataReadiness(true); - _range_finder.setValidity(true); + _range_finder.setSample(sample) + EXPECT_TRUE(_range_finder.isDataHealthy()); EXPECT_TRUE(_range_finder.isHealthy()); } @@ -363,7 +361,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog) // WHEN: sensor is then blocked by fog // range jumps to value below 2m - uint64_t t_now_us = _range_finder.getSampleAddress()->time_us; + uint64_t t_now_us = _range_finder.sample()->time_us; rangeSample sample{t_now_us, 1.f, 100}; updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us); @@ -373,7 +371,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog) // WHEN: the sensor is not blocked by fog anymore sample.rng = 5.f; - sample.time_us = _range_finder.getSampleAddress()->time_us; + sample.time_us = _range_finder.sample()->time_us; updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us); // THEN: the data should be marked as healthy again