From 6a1e609e3c3158605fbe0b6ea9f21cb8651ed1c6 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 24 Sep 2025 21:59:20 -0800 Subject: [PATCH] kill of sensor_range_finder abstraction completely --- src/modules/ekf2/CMakeLists.txt | 1 - src/modules/ekf2/EKF/CMakeLists.txt | 1 - .../optical_flow/optical_flow_control.cpp | 4 +- .../range_finder/range_height_control.cpp | 42 ++++----- .../range_finder/sensor_range_finder.cpp | 59 ------------ .../range_finder/sensor_range_finder.hpp | 93 ------------------- src/modules/ekf2/EKF/common.h | 9 ++ src/modules/ekf2/EKF/ekf.h | 3 +- src/modules/ekf2/EKF/ekf_helper.cpp | 4 +- src/modules/ekf2/EKF/estimator_interface.cpp | 4 + src/modules/ekf2/EKF/estimator_interface.h | 12 +-- src/modules/ekf2/EKF2.cpp | 11 +-- .../test/sensor_simulator/range_finder.cpp | 1 - .../ekf2/test/test_SensorRangeFinder.cpp | 1 - 14 files changed, 44 insertions(+), 201 deletions(-) delete mode 100644 src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp delete mode 100644 src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index a7b8e18f8a..b8762b4d56 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -213,7 +213,6 @@ if(CONFIG_EKF2_RANGE_FINDER) EKF/aid_sources/range_finder/range_finder_consistency_check.cpp EKF/aid_sources/range_finder/range_height_control.cpp EKF/aid_sources/range_finder/range_height_fusion.cpp - EKF/aid_sources/range_finder/sensor_range_finder.cpp ) list(APPEND EKF_MODULE_PARAMS params_range_finder.yaml) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 381e265380..1b6902f8e6 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -126,7 +126,6 @@ if(CONFIG_EKF2_RANGE_FINDER) aid_sources/range_finder/range_finder_consistency_check.cpp aid_sources/range_finder/range_height_control.cpp aid_sources/range_finder/range_height_fusion.cpp - aid_sources/range_finder/sensor_range_finder.cpp ) endif() diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 87a3b6af6d..7774147610 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -88,9 +88,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) bool is_tilt_good = true; #if defined(CONFIG_EKF2_RANGE_FINDER) - // TODO: magic number -- decouple max tilt for flow fusion from max tilt for range fusion - float cosine_max_tilt = 0.7071f; - is_tilt_good = (_R_to_earth(2, 2) > cosine_max_tilt); + is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); #endif // CONFIG_EKF2_RANGE_FINDER calcOptFlowBodyRateComp(flow_sample); 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 03a250e259..4c6b8fb0c1 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 @@ -56,6 +56,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) // no new sample available, check if we've timed out bool had_sample = _range_time_last_good_sample > 0; bool timed_out = imu_sample.time_us > _range_time_last_good_sample + 200'000; + if (had_sample && timed_out) { stopRangeAltitudeFusion("sensor timed out"); stopRangeTerrainFusion("sensor timed out"); @@ -64,9 +65,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) return; } - // Set the raw sample - _range_sensor.setSample(sample); - // Quality checks if (sample.quality == 0) { if (_control_status.flags.in_air) { @@ -83,7 +81,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { // While on ground fake a measurement at ground level if the signal quality is bad sample.quality = 100; - sample.range = _range_sensor.getValidMinVal(); + sample.range = sample.min_distance; _range_time_last_good_sample = sample.time_us; } @@ -91,9 +89,9 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _range_time_last_good_sample = sample.time_us; } - // Tilt and range checks - bool tilt_ok = _range_sensor.isTiltOk(); - bool range_ok = sample.range <= _range_sensor.getValidMaxVal() && sample.range >= _range_sensor.getValidMinVal(); + float cos_theta = _R_to_earth(2, 2); + bool tilt_ok = cos_theta > _params.range_cos_max_tilt; + bool range_ok = sample.range <= sample.max_distance && sample.range >= sample.min_distance; // If tilt or value are outside of bounds -- throw away measurement if (!tilt_ok || !range_ok) { @@ -105,19 +103,22 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) // TODO: refactor into apply_body_offset() // Correct the range data for position offset relative to the IMU const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - sample.range = sample.range + pos_offset_earth(2) / _R_to_earth(2, 2); // rotate into earth frame - // Provide sample from buffer to object - _range_sensor.setSample(sample); + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + + sample.range = sample.range + pos_offset_earth(2) / cos_theta; // rotate into earth frame // TODO: refactor into consintency_filter_update() that runs consistency status // Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); + // rotate into world frame + float sample_corrected = sample.range * cos_theta; + const float z = _gpos.altitude(); const float vz = _state.vel(2); - const float dist = _range_sensor.getDistBottom(); // NOTE: applies rotation into world frame + const float dist = sample_corrected; + const float dist_var = 0.05; 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); @@ -129,7 +130,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); // Publish EstimatorAidSource1d (observation, variance, rejected, fused) - updateRangeHagl(_aid_src_rng_hgt); + updateRangeHagl(_aid_src_rng_hgt, sample_corrected, sample.time_us); if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) { printf("INFINIFY OBSERVATION INVALID\n"); @@ -347,17 +348,17 @@ void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) aid_src.time_last_fuse = _time_delayed_us; } -void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) +void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src, float measurement, uint64_t time_us) { - const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng); - const float measurement_variance = getRngVar(); + measurement = math::max(measurement, _params.ekf2_min_rng); + const float measurement_variance = sq(_params.ekf2_rng_noise) + sq(_params.ekf2_rng_sfe * measurement); float innovation_variance; sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance); const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f); updateAidSourceStatus(aid_src, - _range_sensor.sample()->time_us, // sample timestamp + time_us, // sample timestamp measurement, // observation measurement_variance, // observation variance getHagl() - measurement, // innovation @@ -373,13 +374,6 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) } } -float Ekf::getRngVar() const -{ - const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom()); - const float dist_var = sq(_params.ekf2_rng_noise) + dist_dependant_var; - return dist_var; -} - void Ekf::stopRangeAltitudeFusion(const char *reason) { if (_control_status.flags.rng_hgt) { 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 deleted file mode 100644 index 9a79ba0e38..0000000000 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ /dev/null @@ -1,59 +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_range_finder.cpp - * - * @author Mathieu Bresciani - * - */ - -#include - -#include - -namespace estimator -{ - -void SensorRangeFinder::setSample(const rangeSample &sample) -{ - _sample = sample; -} - -void SensorRangeFinder::setLimits(float min_distance, float max_distance) -{ - _min_distance = min_distance; - _max_distance = max_distance; -} - -} // 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 deleted file mode 100644 index 7ab9d614ec..0000000000 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ /dev/null @@ -1,93 +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_range_finder.hpp - * Range finder class containing all the required checks - * - * @author Mathieu Bresciani - * - */ -#ifndef EKF_SENSOR_RANGE_FINDER_HPP -#define EKF_SENSOR_RANGE_FINDER_HPP - -#include -#include - -namespace estimator -{ - -struct rangeSample { - uint64_t time_us{}; // timestamp of the measurement (uSec) - float range{}; // range (distance to ground) measurement (m) - int8_t quality{}; // Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. -}; - -class SensorRangeFinder -{ -public: - SensorRangeFinder() = default; - ~SensorRangeFinder() = default; - - // This is required because of the ring buffer - // TODO: move the ring buffer here - rangeSample *sample() { return &_sample; } - - void setSample(const rangeSample &sample); - void setLimits(float min_distance, float max_distance); - - float getDistBottom() const { return _sample.range * _cos_tilt_rng_to_earth; } - float getValidMinVal() const { return _min_distance; } - float getValidMaxVal() const { return _max_distance; } - - bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } - -private: - - float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt - - rangeSample _sample{}; - - // Tilt check - float _cos_tilt_rng_to_earth{1.f}; // 2,2 element of the rotation matrix from sensor frame to earth frame - float _range_cos_max_tilt{0.9659}; // 10 degrees. Cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data - float _sin_pitch_offset{0.0f}; // sine of the range finder tilt rotation about the Y body axis - float _cos_pitch_offset{-1.0f}; // cosine of the range finder tilt rotation about the Y body axis - - // Range check - float _min_distance{}; // minimum distance that the rangefinder can measure (m) - float _max_distance{}; // maximum distance that the rangefinder can measure (m) -}; - -} // namespace estimator -#endif // !EKF_SENSOR_RANGE_FINDER_HPP diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index a205b87db6..57adfc0550 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -222,6 +222,14 @@ struct airspeedSample { float eas2tas{}; ///< equivalent to true airspeed factor }; +struct rangeSample { + uint64_t time_us{}; // timestamp of the measurement (uSec) + float range{}; // range (distance to ground) measurement (m) + int8_t quality{}; // Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. + float min_distance{}; + float max_distance{}; +}; + struct flowSample { uint64_t time_us{}; ///< timestamp of the integration period midpoint (uSec) Vector2f flow_rate{}; ///< measured angular rate of the image about the X and Y body axes (rad/s), RH rotation is positive @@ -423,6 +431,7 @@ struct parameters { float ekf2_rng_a_hmax{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) 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 ekf2_rng_k_gate{1.0f}; ///< 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] diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index ed873749a7..51221ba404 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -765,9 +765,8 @@ private: # if defined(CONFIG_EKF2_RANGE_FINDER) // update the terrain vertical position estimate using a height above ground measurement from the range finder bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain); - void updateRangeHagl(estimator_aid_source1d_s &aid_src); + void updateRangeHagl(estimator_aid_source1d_s &aid_src, float measurement, uint64_t time_us); void resetTerrainToRng(estimator_aid_source1d_s &aid_src); - float getRngVar() const; # endif // CONFIG_EKF2_RANGE_FINDER # if defined(CONFIG_EKF2_OPTICAL_FLOW) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5820494f8e..9cb9ecfd2d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -343,10 +343,10 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl #if defined(CONFIG_EKF2_RANGE_FINDER) // Calculate range finder limits - const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); + const float rangefinder_hagl_min = _range_min_distance; // Allow use of 90% of rangefinder maximum range to allow for angular motion - const float rangefinder_hagl_max = 0.9f * _range_sensor.getValidMaxVal(); + const float rangefinder_hagl_max = 0.9f * _range_max_distance; // TODO : calculate visual odometry limits const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 7c18f9def0..ce5907778b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -301,6 +301,10 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) } } + // TODO: better way to do this? + _range_min_distance = range_sample.min_distance; + _range_max_distance = range_sample.max_distance; + const int64_t time_us = range_sample.time_us - static_cast(_params.ekf2_rng_delay * 1000) - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 1c7965c077..7f4d23ba9a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -68,7 +68,6 @@ #if defined(CONFIG_EKF2_RANGE_FINDER) # include "aid_sources/range_finder/range_finder_consistency_check.hpp" -# include "aid_sources/range_finder/sensor_range_finder.hpp" #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_GNSS) @@ -115,12 +114,6 @@ public: #if defined(CONFIG_EKF2_RANGE_FINDER) void setRangeData(const estimator::rangeSample &range_sample); - // set sensor limitations reported by the rangefinder - void set_rangefinder_limits(float min_distance, float max_distance) - { - _range_sensor.setLimits(min_distance, max_distance); - } - #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -380,7 +373,10 @@ protected: uint64_t _time_rng_aid_conditions_valid{}; uint64_t _range_time_last_good_sample{}; - SensorRangeFinder _range_sensor{}; + // TODO: find a better way to store these? + float _range_min_distance{}; + float _range_max_distance{}; + RangeFinderConsistencyCheck _rng_consistency_check{}; #endif // CONFIG_EKF2_RANGE_FINDER diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b1f5811af2..0789fd0e45 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2402,11 +2402,10 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) .time_us = optical_flow.timestamp_sample, .range = optical_flow.distance_m, .quality = quality, + .min_distance = optical_flow.min_ground_distance, + .max_distance = optical_flow.max_ground_distance, }; _ekf.setRangeData(range_sample); - - // set sensor limits - _ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance); } #endif // CONFIG_EKF2_RANGE_FINDER @@ -2577,11 +2576,11 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) .time_us = distance_sensor.timestamp, .range = distance_sensor.current_distance, .quality = distance_sensor.signal_quality, + .min_distance = distance_sensor.min_distance, + .max_distance = distance_sensor.max_distance, }; - _ekf.setRangeData(range_sample); - // Save sensor limits reported by the rangefinder - _ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance); + _ekf.setRangeData(range_sample); _last_range_sensor_update = ekf2_timestamps.timestamp; } diff --git a/src/modules/ekf2/test/sensor_simulator/range_finder.cpp b/src/modules/ekf2/test/sensor_simulator/range_finder.cpp index 2cfdf2e2e0..8b4654c707 100644 --- a/src/modules/ekf2/test/sensor_simulator/range_finder.cpp +++ b/src/modules/ekf2/test/sensor_simulator/range_finder.cpp @@ -17,7 +17,6 @@ void RangeFinder::send(uint64_t time) { _range_sample.time_us = time; _ekf->setRangeData(_range_sample); - _ekf->set_rangefinder_limits(_min_distance, _max_distance); } void RangeFinder::setData(float range_data_meters, int8_t range_quality) diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index 5b3fa5bce3..a3299f1b16 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -34,7 +34,6 @@ #include #include #include "EKF/common.h" -#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp" #include using estimator::rangeSample;