kill of sensor_range_finder abstraction completely

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()

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()
 

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);

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) {

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp

@@ -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 <brescianimathieu@gmail.com>
- *
- */
-
-#include <aid_sources/range_finder/sensor_range_finder.hpp>
-
-#include <lib/matrix/matrix/math.hpp>
-
-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

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp

@@ -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 <brescianimathieu@gmail.com>
- *
- */
-#ifndef EKF_SENSOR_RANGE_FINDER_HPP
-#define EKF_SENSOR_RANGE_FINDER_HPP
-
-#include <matrix/math.hpp>
-#include <lib/mathlib/math/filter/MedianFilter.hpp>
-
-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

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]
 

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)

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);

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<int64_t>(_params.ekf2_rng_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2

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
 

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;
 		}

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)

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -34,7 +34,6 @@
 #include <gtest/gtest.h>
 #include <math.h>
 #include "EKF/common.h"
-#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp"
 #include <matrix/math.hpp>
 
 using estimator::rangeSample;