rtl_mission_fast: fix FW landing by setting previous wp in landing

Signed-off-by: Silvan <silvan@auterion.com>

ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500

@@ -49,9 +49,3 @@ param set-default SIM_GZ_EC_MAX4 1000
 
 param set-default MPC_THR_HOVER 0.60
 param set-default NAV_DLL_ACT 2
-
-# DEBUGGING: ekf range fusion
-# default, ekf replay, cv+avoid, high rate sensors
-param set-default SDLOG_PROFILE 2179
-# sim distance on ground
-param set-default EKF2_MIN_RNG 0.177

src/modules/ekf2/CMakeLists.txt

@@ -213,6 +213,7 @@ 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,6 +126,7 @@ 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/range_finder/Sensor.hpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   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 <brescianimathieu@gmail.com>
+ *
+ */
+
+#ifndef EKF_SENSOR_HPP
+#define EKF_SENSOR_HPP
+
+#include <cstdint>
+
+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

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

@@ -36,131 +36,56 @@
  */
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
-#include "ekf_derivation/generated/range_validation_filter_h.h"
-#include "ekf_derivation/generated/range_validation_filter_P_init.h"
 
-using namespace matrix;
-
-void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom,
-				       const float dist_bottom_var)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
+		bool horizontal_motion, uint64_t time_us)
 {
-	printf("RangeFinderConsistencyCheck::init\n");
+	if (horizontal_motion) {
+		_time_last_horizontal_motion = time_us;
+	}
 
-	_p = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
-	_ht = sym::RangeValidationFilterH<float>();
-
-	_x(RangeFilter::z.idx) = z;
-	_x(RangeFilter::terrain.idx) = z - dist_bottom;
-	_initialized = true;
-	_test_ratio = 0.f;
-	_t_since_first_sample = 0.f;
-}
-
-void RangeFinderConsistencyCheck::update(const float z, const float z_var, const float vz, const float vz_var,
-		const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
-{
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
 
-	if (dt > 1.f) {
+	if ((_time_last_update_us == 0)
+	    || (dt < 0.001f) || (dt > 0.5f)) {
 		_time_last_update_us = time_us;
-		_initialized = false;
-		return;
-
-	} else if (!_initialized) {
-		init(z, z_var, dist_bottom, dist_bottom_var);
+		_dist_bottom_prev = dist_bottom;
 		return;
 	}
 
-	// prediction step
+	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
+	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
+
+	// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
+	const float var = 2.f * dist_bottom_var / (dt * dt);
+	_innov_var = var + vz_var;
+
+	const float normalized_innov_sq = (_innov * _innov) / _innov_var;
+	_test_ratio = normalized_innov_sq / (_gate * _gate);
+	_signed_test_ratio_lpf.setParameters(dt, _signed_test_ratio_tau);
+	const float signed_test_ratio = matrix::sign(_innov) * _test_ratio;
+	_signed_test_ratio_lpf.update(signed_test_ratio);
+
+	updateConsistency(vz, time_us);
+
 	_time_last_update_us = time_us;
-	_x(RangeFilter::z.idx) -= dt * vz;
-	_p(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
-	_p(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
+	_dist_bottom_prev = dist_bottom;
+}
 
-	// iterate through both measurements (z and dist_bottom)
-	const Vector2f measurements{z, dist_bottom};
-
-	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
-
-		// dist_bottom
-		Vector2f H = _ht;
-		float R = dist_bottom_var;
-
-		// z, direct state measurement
-		if (measurement_idx == 0) {
-			H(RangeFilter::z.idx) = 1.f;
-			H(RangeFilter::terrain.idx) = 0.f;
-			R = z_var;
+void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
+{
+	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
+		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
+			_is_kinematically_consistent = false;
+			_time_last_inconsistent_us = time_us;
 		}
 
-		// residual
-		const float measurement_pred = H * _x;
-		const float y = measurements(measurement_idx) - measurement_pred;
-
-		// for H as col-vector:
-		// innovation variance S = H^T * P * H + R
-		// kalman gain K = P * H / S
-		const float S = (H.transpose() * _p * H + R)(0, 0);
-		Vector2f K = (_p * H / S);
-
-		if (measurement_idx == 0) {
-			K(RangeFilter::z.idx) = 1.f;
-
-		} else if (measurement_idx == 1) {
-			_innov = y;
-			const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
-			_test_ratio = sign(_innov) * test_ratio;
+	} else {
+		if ((fabsf(vz) > _min_vz_for_valid_consistency)
+		    && (_test_ratio < 1.f)
+		    && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
+		   ) {
+			_is_kinematically_consistent = true;
 		}
-
-		// update step
-		_x(RangeFilter::z.idx)       += K(RangeFilter::z.idx) * y;
-		_x(RangeFilter::terrain.idx) += K(RangeFilter::terrain.idx) * y;
-
-		// covariance update with Joseph form:
-		// P = (I - K H) P (I - K H)^T + K R K^T
-		Matrix2f I;
-		I.setIdentity();
-		Matrix2f IKH = I - K.multiplyByTranspose(H);
-		_p = IKH * _p * IKH.transpose() + (K * R).multiplyByTranspose(K);
 	}
-
-	evaluateState(dt, vz, vz_var);
-}
-
-void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
-{
-	// TODO: magic test ratio
-	if (fabsf(_test_ratio) > 1.f) {
-		_t_since_first_sample = 0.f;
-		_state = KinematicState::INCONSISTENT;
-		return;
-	}
-
-	_state = KinematicState::CONSISTENT;
-}
-
-bool RangeFinderConsistencyCheck::isKinematicallyConsistent()
-{
-	return _state == KinematicState::CONSISTENT;
-}
-
-void RangeFinderConsistencyCheck::reset()
-{
-	if (_initialized && _state == KinematicState::CONSISTENT) {
-		_state = KinematicState::UNKNOWN;
-	}
-
-	_initialized = false;
-}
-
-void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var,
-				      const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
-{
-	if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
-		printf("kcc run, resetting\n");
-		_last_posD_reset_count = current_posD_reset_count;
-		_initialized = false;
-	}
-
-	update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
 }

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

@@ -40,9 +40,7 @@
 #ifndef EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
 #define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
 
-#include <mathlib/math/Functions.hpp>
-
-using namespace math;
+#include <mathlib/math/filter/AlphaFilter.hpp>
 
 class RangeFinderConsistencyCheck final
 {
@@ -50,56 +48,36 @@ public:
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;
 
-	enum class KinematicState : int {
-		INCONSISTENT = 0,
-		CONSISTENT = 1,
-		UNKNOWN = 2
-	};
+	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
 
-	float getTestRatioLpf() const { return _test_ratio; }
+	void setGate(float gate) { _gate = gate; }
 
-	float getInnov() const { return _initialized ? _innov : 0.f; }
-	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
-	bool isKinematicallyConsistent();
-	void setGate(const float gate) { _gate = gate; }
-	void run(float z, float z_var, float vz, float vz_var,
-		 float dist_bottom, float dist_bottom_var, uint64_t time_us);
-	void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; }
-	void reset();
-
-	uint8_t current_posD_reset_count{0};
+	float getTestRatio() const { return _test_ratio; }
+	float getSignedTestRatioLpf() const { return _signed_test_ratio_lpf.getState(); }
+	float getInnov() const { return _innov; }
+	float getInnovVar() const { return _innov_var; }
+	bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
 
 private:
+	void updateConsistency(float vz, uint64_t time_us);
 
-	void update(float z, float z_var, float vz, float vz_var, float dist_bottom,
-		    float dist_bottom_var, uint64_t time_us);
-	void init(float z, float z_var, float dist_bottom, float dist_bottom_var);
-	void evaluateState(float dt, float vz, float vz_var);
-	float _terrain_process_noise{0.0f};
-	matrix::SquareMatrix<float, 2> _p{};
-	matrix::Vector2f _ht{};
-	matrix::Vector2f _x{};
-	bool _initialized{false};
-	float _innov{0.f};
-	float _innov_var{0.f};
-	uint64_t _time_last_update_us{0};
-	static constexpr float time_constant{1.f};
+	uint64_t _time_last_update_us{};
+	float _dist_bottom_prev{};
 
-	float _test_ratio{0.f};
+	float _test_ratio{};
+	AlphaFilter<float> _signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
+	float _gate{.2f};
+	float _innov{};
+	float _innov_var{};
 
-	float _gate{1.0f};
-	KinematicState _state{KinematicState::UNKNOWN};
-	float _t_since_first_sample{0.f};
-	uint8_t _last_posD_reset_count{0};
-	static constexpr float _t_to_init{1.f};
+	bool _is_kinematically_consistent{true};
+	uint64_t _time_last_inconsistent_us{};
+	uint64_t _time_last_horizontal_motion{};
+
+	static constexpr float _signed_test_ratio_tau = 2.f;
+
+	static constexpr float _min_vz_for_valid_consistency = .5f;
+	static constexpr uint64_t _consistency_hyst_time_us = 1e6;
 };
 
-namespace RangeFilter
-{
-struct IdxDof { unsigned idx; unsigned dof; };
-static constexpr IdxDof z{0, 1};
-static constexpr IdxDof terrain{1, 1};
-static constexpr uint8_t size{2};
-}
-
 #endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP

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

@@ -42,276 +42,237 @@
 
 void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
-	if (!_range_buffer) {
-		return;
-	}
+	static constexpr const char *HGT_SRC_NAME = "RNG";
 
-	// TODO: why isn't this being done anywhere?
-	_aid_src_rng_hgt.fused = false;
+	bool rng_data_ready = false;
 
-	// Pop rangefinder measurement from buffer of samples into active sample
-	rangeSample sample = {};
+	if (_range_buffer) {
+		// Get range data from buffer and check validity
+		rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
+		_range_sensor.setDataReadiness(rng_data_ready);
 
-	if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &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;
+		// update range sensor angle parameters in case they have changed
+		_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
+		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+		_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
+		_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
 
-		if (had_sample && timed_out) {
-			stopRangeAltitudeFusion("sensor timed out");
-			stopRangeTerrainFusion("sensor timed out");
-		}
+		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 
-		return;
-	}
+		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;
+			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
-	// Quality checks
-	if (sample.quality == 0) {
-		if (_control_status.flags.in_air) {
-			// Disable fusion after 1s of bad quality
-			uint64_t elapsed = sample.time_us - _range_time_last_good_sample;
+			if (_control_status.flags.in_air) {
+				const bool horizontal_motion = _control_status.flags.fixed_wing
+							       || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
 
-			if (elapsed > 1'000'000) {
-				stopRangeAltitudeFusion("sensor quality bad");
-				stopRangeTerrainFusion("sensor quality bad");
+				const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
+				const float var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
+
+				_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
+				_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);
 			}
 
-			return;
-
 		} else {
-			// While on ground fake a measurement at ground level if the signal quality is bad
-			sample.quality = 100;
-			sample.range = sample.min_distance;
-			_range_time_last_good_sample = sample.time_us;
+			// 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();
+
 	} else {
-		_range_time_last_good_sample = sample.time_us;
-	}
-
-	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) {
-		stopRangeAltitudeFusion("pre checks failed");
-		stopRangeTerrainFusion("pre checks failed");
 		return;
 	}
 
-	// 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;
+	auto &aid_src = _aid_src_rng_hgt;
 
-	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+	if (rng_data_ready && _range_sensor.getSampleAddress()) {
 
-	sample.range = sample.range + pos_offset_earth(2) / cos_theta; // rotate into earth frame
+		updateRangeHagl(aid_src);
+		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
 
-	// 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();
+		const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
+				|| (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
+				&& _control_status.flags.tilt_align
+				&& measurement_valid
+				&& _range_sensor.isDataHealthy()
+				&& _rng_consistency_check.isKinematicallyConsistent();
 
-	// rotate into world frame
-	float sample_corrected = sample.range * cos_theta;
+		const bool starting_conditions_passing = continuing_conditions_passing
+				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
+				&& _range_sensor.isRegularlySendingData();
 
-	const float z = _gpos.altitude();
-	const float vz = _state.vel(2);
-	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);
+		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
+						      && (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+						      && isConditionalRangeAidSuitable();
 
-	// Run the kinematic consistency check
-	_rng_consistency_check.run(z, z_var, vz, vz_var, dist, dist_var, imu_sample.time_us);
+		const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
+					  && (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
 
-	// Track kinematic consistency
-	_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+		if (_control_status.flags.rng_hgt) {
+			if (!(do_conditional_range_aid || do_range_aid)) {
+				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
+				stopRngHgtFusion();
+			}
 
-	// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
-	updateRangeHagl(_aid_src_rng_hgt, sample_corrected, sample.time_us);
+		} else {
+			if (_params.ekf2_hgt_ref == static_cast<int32_t>(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.
+					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
+					_height_sensor_ref = HeightSensor::RANGE;
 
-	if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) {
-		printf("INFINIFY OBSERVATION INVALID\n");
-	}
+					_control_status.flags.rng_hgt = true;
+					stopRngTerrFusion();
 
-	// Fuse Range data as Primary height source
-	// NOTE: terrain is not estimated in this mode
-	if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
-		fuseRangeAsHeightSource();
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+						resetTerrainToRng(aid_src);
+						resetAidSourceStatusZeroInnovation(aid_src);
+					}
 
-	} else {
-		// Fuse Range data as aiding height source (Primary GPS or Baro)
-		fuseRangeAsHeightAiding();
+				} 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
+					ECL_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) {
+					ECL_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) {
+						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
+					ECL_WARN("%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()) {
+						ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
+						stopRngHgtFusion();
+						stopRngTerrFusion();
+
+					} else {
+						resetTerrainToRng(aid_src);
+						resetAidSourceStatusZeroInnovation(aid_src);
+					}
+				}
+
+			} else {
+				ECL_WARN("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) {
+						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.
+		ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
+		stopRngHgtFusion();
+		stopRngTerrFusion();
 	}
 }
 
-void Ekf::fuseRangeAsHeightAiding()
+void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 {
-	const char *kNotKinematicallyConsistentText = "not kinematically consistent";
-	const char *kConditionsFailingText = "conditions failing";
+	const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
+	const float measurement_variance = getRngVar();
 
-	// Stop fusion if rangefinder kinematic consistency fails
-	if (!_control_status.flags.rng_kin_consistent) {
-		stopRangeTerrainFusion(kNotKinematicallyConsistentText);
-		stopRangeAltitudeFusion(kNotKinematicallyConsistentText);
-		return;
-	}
+	float innovation_variance;
+	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
 
-	//// TERRAIN FUSION ////
+	const float innov_gate = math::max(_params.ekf2_rng_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
 
-	// Fuse Range into Terrain if:
-	// - kinematically consistent (hagl_rate < 1)
-
-	// Start fusion
-	if (!_control_status.flags.rng_terrain) {
-		printf("START RNG Terrain fusion\n");
-		_control_status.flags.rng_terrain = true;
-
-		// We must reset terrain to range before we start fusing again, otherwise there
-		// can be a delta between RNG terrain estimate and previous, which will cause a
-		// discontinuity in the lpos.z state
-		printf("resetting terrain to range\n");
-		resetTerrainToRng(_aid_src_rng_hgt);
-		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-	}
-
-
-	//// ALTITUDE FUSION ////
-
-	bool range_aid_conditional = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::CONDITIONAL;
-	bool range_aid_enabled = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::ENABLED;
-
-	bool range_aid_conditions_passed = rangeAidConditionsPassed();
-
-	bool do_range_aid = range_aid_enabled || (range_aid_conditional && range_aid_conditions_passed);
-
-	// Fuse Range into Altitude if:
-	// - passes range_aid_conditionalchecks
-	// - kinematically consistent
-	if (do_range_aid) {
-
-		if (!_control_status.flags.rng_hgt) {
-			printf("START RNG Altitude fusion\n");
-			_control_status.flags.rng_hgt = true;
-
-			// Reset altitude to rangefinder if on ground
-			if (!_control_status.flags.in_air) {
-				printf("GND resetting altitude to range\n");
-				resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
-			}
-
-			// TODO: review for correctness
-			if (_aid_src_rng_hgt.innovation_rejected) {
-				// Reset aid source
-				printf("resetting aid source\n");
-				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-			}
-		}
-
-	} else {
-		stopRangeAltitudeFusion(kConditionsFailingText);
-	}
-
-	// If we make it here, fuse
-	fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
-}
-
-// TODO: remove this mode entirely
-void Ekf::fuseRangeAsHeightSource()
-{
-	// When primary height source is RANGE, we always fuse it
-	// I don't think conditional range aid mode makes sense in the context of RANGE = primary
-
-	// Fusion init logic
-	if (_height_sensor_ref != HeightSensor::RANGE) {
-
-		_height_sensor_ref = HeightSensor::RANGE;
-		_information_events.flags.reset_hgt_to_rng = true;
-
-		// Reset aid source innovation
-		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-
-		// Reset altitude to RANGE
-		resetAltitudeTo(_aid_src_rng_hgt.observation, _aid_src_rng_hgt.observation_variance);
-		_control_status.flags.rng_hgt = true;
-
-		// Cannot have terrain estimate fusion while RANGE is primary
-		stopRangeTerrainFusion("Cannot have terrain estimate fusion while RANGE is primary");
-		printf("initializing range as primary\n");
-		_state.terrain = 0.f;
-
-		// TODO: needed? It's set above in --> resetAidSourceStatusZeroInnovation()
-		// _aid_src_rng_hgt.time_last_fuse = imu_sample.time_us;
-	}
-
-	// TODO:
-	// When RNG is primary height source
-	if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
-		printf("RNG height fusion reset required, all height sources failing\n");
-
-		uint64_t timestamp = _aid_src_rng_hgt.timestamp;
-		_information_events.flags.reset_hgt_to_rng = true;
-		resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
-		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-
-		// reset vertical velocity if no valid sources available
-		if (!isVerticalVelocityAidingActive()) {
-			printf("resetting vertical velocity\n");
-			resetVerticalVelocityToZero();
-		}
-
-		_aid_src_rng_hgt.time_last_fuse = timestamp;
+	// 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;
 	}
 }
 
-bool Ekf::rangeAidConditionsPassed()
+float Ekf::getRngVar() const
 {
-	bool is_in_range = getHagl() < _params.ekf2_rng_a_hmax;
-	// bool is_hagl_stable = _aid_src_rng_hgt.test_ratio < 1.f;
-	// bool is_horizontal_aiding_active = isHorizontalAidingActive();
-	// bool is_below_max_speed = is_horizontal_aiding_active && !_state.vel.xy().longerThan(_params.ekf2_rng_a_vmax);
-	bool is_below_max_speed = !_state.vel.xy().longerThan(_params.ekf2_rng_a_vmax);
-
-	// Require conditions passing for 1_s (same as kinematic consistency check)
-	bool conditions_passing = false;
-	// bool conditions_met = is_in_range && is_hagl_stable && is_below_max_speed;
-	bool conditions_met = is_in_range && is_below_max_speed;
-
-	if (conditions_met) {
-		if (!_rng_aid_conditions_valid) {
-			// Conditions just became valid
-			printf("RNG AID conditions valid\n");
-			_rng_aid_conditions_valid = true;
-			_time_rng_aid_conditions_valid = _time_latest_us;
-		}
-
-		if (_time_latest_us > _time_rng_aid_conditions_valid + 1'000'000) {
-			// Conditions have been valid for at least 1s
-			conditions_passing = true;
-		}
-
-	} else {
-
-		if (_rng_aid_conditions_valid) {
-			_rng_aid_conditions_valid = false;
-
-			// if (!is_hagl_stable) {
-			//  printf("!is_hagl_stable\n");
-			// }
-			// if (is_horizontal_aiding_active && !is_below_max_speed) {
-			if (!is_below_max_speed) {
-				printf("!is_below_max_speed\n");
-			}
-
-			if (!is_in_range) {
-				printf("!is_in_range\n");
-			}
-		}
-	}
-
-	return conditions_passing;
+	return fmaxf(
+		       P(State::pos.idx + 2, State::pos.idx + 2)
+		       + sq(_params.ekf2_rng_noise)
+		       + sq(_params.ekf2_rng_sfe * _range_sensor.getRange()),
+		       0.f);
 }
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
@@ -348,48 +309,47 @@ 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, float measurement, uint64_t time_us)
+bool Ekf::isConditionalRangeAidSuitable()
 {
-	measurement = math::max(measurement, _params.ekf2_min_rng);
-	const float measurement_variance = sq(_params.ekf2_rng_noise) + sq(_params.ekf2_rng_sfe * measurement);
+	// 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.ekf2_rng_a_hmax;
+	float max_vel_xy = _params.ekf2_rng_a_vmax;
 
-	float innovation_variance;
-	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
+	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
 
-	const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
-	updateAidSourceStatus(aid_src,
-			      time_us,                                   // sample timestamp
-			      measurement,                               // observation
-			      measurement_variance,                      // observation variance
-			      getHagl() - measurement,                   // innovation
-			      innovation_variance,                       // innovation variance
-			      innov_gate);                               // innovation gate
+	bool is_hagl_stable = (hagl_test_ratio < 1.f);
 
-	// 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;
+	if (!_control_status.flags.rng_hgt) {
+		range_hagl_max = 0.7f * _params.ekf2_rng_a_hmax;
+		max_vel_xy = 0.7f * _params.ekf2_rng_a_vmax;
+		is_hagl_stable = (hagl_test_ratio < 0.01f);
 	}
+
+	const bool is_in_range = (getHagl() < range_hagl_max);
+
+	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::stopRangeAltitudeFusion(const char *reason)
+void Ekf::stopRngHgtFusion()
 {
 	if (_control_status.flags.rng_hgt) {
-		printf("STOP RNG Altitude fusion: %s\n", reason);
-		_control_status.flags.rng_hgt = false;
 
 		if (_height_sensor_ref == HeightSensor::RANGE) {
 			_height_sensor_ref = HeightSensor::UNKNOWN;
 		}
+
+		_control_status.flags.rng_hgt = false;
 	}
 }
 
-void Ekf::stopRangeTerrainFusion(const char *reason)
+void Ekf::stopRngTerrFusion()
 {
-	if (_control_status.flags.rng_terrain) {
-		printf("STOP RNG Terrain fusion: %s\n", reason);
-		_control_status.flags.rng_terrain = false;
-	}
+	_control_status.flags.rng_terrain = false;
 }

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

@@ -0,0 +1,171 @@
+/****************************************************************************
+ *
+ *   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
+{
+namespace sensor
+{
+
+void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
+{
+	updateSensorToEarthRotation(R_to_earth);
+	updateValidity(current_time_us);
+}
+
+void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
+{
+	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
+	// this is required for use of range finder and flow data
+	_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;
+	}
+}
+
+} // namespace sensor
+} // namespace estimator

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

@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   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 "Sensor.hpp"
+
+#include <matrix/math.hpp>
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
+
+namespace estimator
+{
+namespace sensor
+{
+
+struct rangeSample {
+	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
+	float       rng{};      ///< 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.
+};
+
+static constexpr uint64_t RNG_MAX_INTERVAL =
+	200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
+
+class SensorRangeFinder : public Sensor
+{
+public:
+	SensorRangeFinder() = default;
+	~SensorRangeFinder() override = 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; }
+
+	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; }
+
+	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 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; }
+
+	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);
+
+	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)
+
+	/*
+	 * 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.7071f};	///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
+	float _pitch_offset_rad{3.14f}; 		///< range finder tilt rotation about the Y body axis
+	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 _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<float, 5> _median_dist{};
+	float _prev_median_dist{0.f};
+
+};
+
+} // namespace sensor
+} // namespace estimator
+#endif // !EKF_SENSOR_RANGE_FINDER_HPP

src/modules/ekf2/EKF/common.h

@@ -222,14 +222,6 @@ 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
@@ -427,6 +419,7 @@ struct parameters {
 	float ekf2_rng_delay{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float ekf2_rng_noise{0.1f};             ///< observation noise for range finder measurements (m)
 	float ekf2_rng_gate{5.0f};              ///< range finder fusion innovation consistency gate size (STD)
+	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 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)

src/modules/ekf2/EKF/covariance.cpp

@@ -105,8 +105,8 @@ void Ekf::initialiseCovariance()
 #endif // CONFIG_EKF2_WIND
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	// use the noise value as our uncertainty
-	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_rng_noise));
+	// use the ground clearance value as our uncertainty
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_min_rng));
 #endif // CONFIG_EKF2_TERRAIN
 }
 
@@ -227,10 +227,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.ekf2_terr_grad) * (sq(_state.vel(0)) + sq(_state.vel(
 						 1)));
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-		_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
-#endif // CONFIG_EKF2_RANGE_FINDER
 	}
 
 #endif // CONFIG_EKF2_TERRAIN

src/modules/ekf2/EKF/ekf.cpp

@@ -77,6 +77,13 @@ void Ekf::reset()
 	_state.terrain = -_gpos.altitude() + _params.ekf2_min_rng;
 #endif // CONFIG_EKF2_TERRAIN
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
+	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
+	_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
+#endif // CONFIG_EKF2_RANGE_FINDER
+
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
 
@@ -401,11 +408,6 @@ 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)
-	// TODO: initialize static rangefinder parameters
-	_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
-
-#endif
 }
 
 template<typename T>

src/modules/ekf2/EKF/ekf.h

@@ -117,7 +117,7 @@ public:
 
 	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
 	float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); }
-	float getHaglRateInnovRatio() const { return _rng_consistency_check.getTestRatioLpf(); }
+	float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -765,8 +765,9 @@ 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, float measurement, uint64_t time_us);
+	void updateRangeHagl(estimator_aid_source1d_s &aid_src);
 	void resetTerrainToRng(estimator_aid_source1d_s &aid_src);
+	float getRngVar() const;
 # endif // CONFIG_EKF2_RANGE_FINDER
 
 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -778,12 +779,9 @@ private:
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
 	void controlRangeHaglFusion(const imuSample &imu_delayed);
-	void fuseRangeAsHeightSource();
-	void fuseRangeAsHeightAiding();
 	bool isConditionalRangeAidSuitable();
-	bool rangeAidConditionsPassed();
-	void stopRangeAltitudeFusion(const char *reason);
-	void stopRangeTerrainFusion(const char *reason);
+	void stopRngHgtFusion();
+	void stopRngTerrFusion();
 #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_min_distance;
+	const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
 
 	// Allow use of 90% of rangefinder maximum range to allow for angular motion
-	const float rangefinder_hagl_max = 0.9f * _range_max_distance;
+	const float rangefinder_hagl_max = 0.9f * _range_sensor.getValidMaxVal();
 
 	// TODO : calculate visual odometry limits
 	const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt);

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -283,7 +283,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-void EstimatorInterface::setRangeData(const rangeSample &range_sample)
+void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 {
 	if (!_initialised) {
 		return;
@@ -291,7 +291,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 
 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
+		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
 
 		if (_range_buffer == nullptr || !_range_buffer->valid()) {
 			delete _range_buffer;
@@ -301,10 +301,6 @@ 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
@@ -312,7 +308,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 	// limit data rate to prevent data being lost
 	if (time_us >= static_cast<int64_t>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {
 
-		rangeSample range_sample_new{range_sample};
+		sensor::rangeSample range_sample_new{range_sample};
 		range_sample_new.time_us = time_us;
 
 		_range_buffer->push(range_sample_new);

src/modules/ekf2/EKF/estimator_interface.h

@@ -68,6 +68,7 @@
 
 #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)
@@ -112,8 +113,15 @@ public:
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	void setRangeData(const estimator::rangeSample &range_sample);
+	void setRangeData(const estimator::sensor::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);
+	}
+
+	const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -365,19 +373,11 @@ protected:
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	RingBuffer<rangeSample> *_range_buffer {nullptr};
+	RingBuffer<sensor::rangeSample> *_range_buffer {nullptr};
 	uint64_t _time_last_range_buffer_push{0};
 
-	// Range aiding
-	bool _rng_aid_conditions_valid{};
-	uint64_t _time_rng_aid_conditions_valid{};
-	uint64_t _range_time_last_good_sample{};
-
-	// TODO: find a better way to store these?
-	float _range_min_distance{};
-	float _range_max_distance{};
-
-	RangeFinderConsistencyCheck _rng_consistency_check{};
+	sensor::SensorRangeFinder _range_sensor{};
+	RangeFinderConsistencyCheck _rng_consistency_check;
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -721,37 +721,6 @@ def compute_gravity_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
-def range_validation_filter_h() -> sf.Matrix:
-
-    state = Values(
-        z=sf.Symbol("z"),
-        terrain=sf.Symbol("terrain")
-    )
-    dist_bottom = state["z"] - state["terrain"]
-
-    H = sf.M21()
-    H[:, 0] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False).transpose()
-
-    return H
-
-def range_validation_filter_P_init(
-        z_var: sf.Scalar,
-        dist_bottom_var: sf.Scalar
-    ) -> sf.Matrix:
-
-    H = range_validation_filter_h().T
-    # enforce terrain to match the measurement
-    K = sf.V2(0, 1/H[1])
-    R = sf.V1(dist_bottom_var)
-
-    # dist_bottom = z - terrain
-    P = sf.M22.diag([z_var, z_var + dist_bottom_var])
-    I = sf.M22.eye()
-    # Joseph form
-    P = (I - K * H) * P * (I - K * H).T + K * R * K.T
-
-    return P
-
 print("Derive EKF2 equations...")
 generate_px4_function(predict_covariance, output_names=None)
 
@@ -783,7 +752,5 @@ generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_va
 generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
 generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
 generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
-generate_px4_function(range_validation_filter_h, output_names=None)
-generate_px4_function(range_validation_filter_P_init, output_names=None)
 
 generate_px4_state(State, tangent_idx)

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h

@@ -1,46 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: range_validation_filter_p_init
- *
- * Args:
- *     z_var: Scalar
- *     dist_bottom_var: Scalar
- *
- * Outputs:
- *     res: Matrix22
- */
-template <typename Scalar>
-matrix::Matrix<Scalar, 2, 2> RangeValidationFilterPInit(const Scalar z_var,
-                                                       const Scalar dist_bottom_var) {
-  // Total ops: 1
-
-  // Input arrays
-
-  // Intermediate terms (0)
-
-  // Output terms (1)
-  matrix::Matrix<Scalar, 2, 2> _res;
-
-  _res(0, 0) = z_var;
-  _res(1, 0) = z_var;
-  _res(0, 1) = z_var;
-  _res(1, 1) = dist_bottom_var + z_var;
-
-  return _res;
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h

@@ -1,41 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: range_validation_filter_h
- *
- * Args:
- *
- * Outputs:
- *     res: Matrix21
- */
-template <typename Scalar>
-matrix::Matrix<Scalar, 2, 1> RangeValidationFilterH() {
-  // Total ops: 0
-
-  // Input arrays
-
-  // Intermediate terms (0)
-
-  // Output terms (1)
-  matrix::Matrix<Scalar, 2, 1> _res;
-
-  _res(0, 0) = 1;
-  _res(1, 0) = -1;
-
-  return _res;
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

src/modules/ekf2/EKF/terrain_control.cpp

@@ -45,8 +45,8 @@ void Ekf::initTerrain()
 	// assume a ground clearance
 	_state.terrain = -_gpos.altitude() + _params.ekf2_min_rng;
 
-	// use the sensor noise value as our uncertainty
-	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_rng_noise));
+	// use the ground clearance value as our uncertainty
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_min_rng));
 }
 
 void Ekf::controlTerrainFakeFusion()
@@ -54,6 +54,7 @@ void Ekf::controlTerrainFakeFusion()
 	// If we are on ground, store the local position and time to use as a reference
 	if (!_control_status.flags.in_air) {
 		_last_on_ground_posD = -_gpos.altitude();
+		_control_status.flags.rng_fault = false;
 
 	} else if (!_control_status_prev.flags.in_air) {
 		// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff
@@ -77,13 +78,14 @@ void Ekf::updateTerrainValidity()
 {
 	bool valid_opt_flow_terrain = false;
 	bool valid_rng_terrain = false;
+	bool positive_hagl_var = false;
+	bool small_relative_hagl_var = false;
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 
-	bool flow_terrain = _control_status.flags.opt_flow_terrain;
-	bool flow_recent = isRecent(_aid_src_optical_flow.time_last_fuse, _params.hgt_fusion_timeout_max);
-
-	if (flow_terrain && flow_recent) {
+	if (_control_status.flags.opt_flow_terrain
+	    && isRecent(_aid_src_optical_flow.time_last_fuse, _params.hgt_fusion_timeout_max)
+	   ) {
 		valid_opt_flow_terrain = true;
 	}
 
@@ -91,34 +93,42 @@ void Ekf::updateTerrainValidity()
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 
-	bool rng_terrain = _control_status.flags.rng_terrain;
-	bool rng_recent = isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max);
-
-	if (rng_terrain && rng_recent) {
+	if (_control_status.flags.rng_terrain
+	    && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max)
+	   ) {
 		valid_rng_terrain = true;
 	}
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-	bool small_relative_hagl_var = false;
-
 	if (_time_last_terrain_fuse != 0) {
 		// Assume being valid when the uncertainty is small compared to the height above ground
 		float hagl_var = INFINITY;
 		sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
 
-		// TODO: quantify this hagl_var check
-		if ((hagl_var > 0.f) && (hagl_var < sq(fmaxf(0.1f * getHagl(), 0.5f)))) {
+		positive_hagl_var = hagl_var > 0.f;
+
+		if (positive_hagl_var
+		    && (hagl_var < sq(fmaxf(0.1f * getHagl(), 0.5f)))
+		   ) {
 			small_relative_hagl_var = true;
 		}
 	}
 
 	const bool positive_hagl = getHagl() >= 0.f;
 
-	// TODO: review terrain validity requirements
-	// dist_bottom_valid == _terrain_valid
+	if (!_terrain_valid) {
+		// require valid RNG or optical flow (+valid variance) to initially consider terrain valid
+		if (positive_hagl
+		    && positive_hagl_var
+		    && (valid_rng_terrain
+			|| (valid_opt_flow_terrain && small_relative_hagl_var))
+		   ) {
+			_terrain_valid = true;
+		}
 
-	const bool terrain_source_valid = (valid_rng_terrain || valid_opt_flow_terrain);
-
-	_terrain_valid = terrain_source_valid && positive_hagl && small_relative_hagl_var;
+	} else {
+		// terrain was previously valid, continue considering valid if variance is good
+		_terrain_valid = positive_hagl && positive_hagl_var && small_relative_hagl_var;
+	}
 }

src/modules/ekf2/EKF2.cpp

@@ -159,8 +159,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_noise(_params->ekf2_rng_noise),
 	_param_ekf2_rng_sfe(_params->ekf2_rng_sfe),
 	_param_ekf2_rng_gate(_params->ekf2_rng_gate),
+	_param_ekf2_rng_pitch(_params->ekf2_rng_pitch),
 	_param_ekf2_rng_a_vmax(_params->ekf2_rng_a_vmax),
 	_param_ekf2_rng_a_hmax(_params->ekf2_rng_a_hmax),
+	_param_ekf2_rng_qlty_t(_params->ekf2_rng_qlty_t),
 	_param_ekf2_rng_k_gate(_params->ekf2_rng_k_gate),
 	_param_ekf2_rng_fog(_params->ekf2_rng_fog),
 	_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
@@ -1617,7 +1619,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
-	// TODO: review -- we should merge getHagl() and isTerrainEstimateValid() since they must always be used together
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
@@ -2398,14 +2399,15 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 
 			int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;
 
-			estimator::rangeSample range_sample {
+			estimator::sensor::rangeSample range_sample {
 				.time_us = optical_flow.timestamp_sample,
-				.range = optical_flow.distance_m,
+				.rng = 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
@@ -2572,16 +2574,16 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 	if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
 		// EKF range sample
 		if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
-			estimator::rangeSample range_sample {
+			estimator::sensor::rangeSample range_sample {
 				.time_us = distance_sensor.timestamp,
-				.range = distance_sensor.current_distance,
+				.rng = 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);
+
 			_last_range_sensor_update = ekf2_timestamps.timestamp;
 		}
 

src/modules/ekf2/EKF2.hpp

@@ -353,7 +353,7 @@ private:
 	uint32_t _device_id_baro{0};
 	hrt_abstime _status_baro_hgt_pub_last{0};
 
-	float _last_baro_bias_published{NAN};
+	float _last_baro_bias_published{};
 
 	uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
 
@@ -618,8 +618,10 @@ private:
 		(ParamExtFloat<px4::params::EKF2_RNG_NOISE>) _param_ekf2_rng_noise,
 		(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe,
 		(ParamExtFloat<px4::params::EKF2_RNG_GATE>) _param_ekf2_rng_gate,
+		(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch,
 		(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
 		(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
+		(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
 		(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
 		(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,

src/modules/ekf2/params_range_finder.yaml

@@ -48,10 +48,19 @@ parameters:
       description:
         short: Measurement noise for range finder fusion
       type: float
-      default: 0.05
+      default: 0.1
       min: 0.01
       unit: m
       decimal: 2
+    EKF2_RNG_PITCH:
+      description:
+        short: Range sensor pitch offset
+      type: float
+      default: 0.0
+      min: -0.75
+      max: 0.75
+      unit: rad
+      decimal: 3
     EKF2_RNG_A_VMAX:
       description:
         short: Maximum horizontal velocity allowed for conditional range aid mode
@@ -70,10 +79,20 @@ parameters:
           will not fuse range measurements to estimate its height. This only applies
           when conditional range aid mode is activated (EKF2_RNG_CTRL = 1).
       type: float
-      default: 25.0
+      default: 5.0
       min: 1.0
-      max: 50.0
+      max: 10.0
       unit: m
+    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
@@ -84,7 +103,7 @@ parameters:
           ...). Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE)
           before tuning this gate.'
       type: float
-      default: 0.5
+      default: 1.0
       unit: SD
       min: 0.1
       max: 5.0
@@ -93,7 +112,7 @@ parameters:
         short: Range finder range dependent noise scaler
         long: Specifies the increase in range finder noise with range.
       type: float
-      default: 0.01
+      default: 0.05
       min: 0.0
       max: 0.2
       unit: m/m

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
-      min: 0
+      default: 0.1
+      min: 0.01
       unit: m
       decimal: 2

src/modules/ekf2/test/sensor_simulator/range_finder.cpp

@@ -17,6 +17,7 @@ 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/sensor_simulator/range_finder.h

@@ -55,7 +55,7 @@ public:
 	void setLimits(float min_distance_m, float max_distance_m);
 
 private:
-	estimator::rangeSample _range_sample{};
+	estimator::sensor::rangeSample _range_sample{};
 	float _min_distance{0.2f};
 	float _max_distance{20.0f};
 

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -34,9 +34,10 @@
 #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;
+using estimator::sensor::rangeSample;
 using matrix::Dcmf;
 using matrix::Eulerf;
 using namespace estimator::sensor;
@@ -47,7 +48,10 @@ public:
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
+		_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

src/modules/logger/logged_topics.cpp

@@ -372,7 +372,6 @@ void LoggedTopics::add_high_rate_sensors_topics()
 	add_topic_multi("sensor_optical_flow", 0, 2);
 	add_topic_multi("sensor_gps", 0, 4);
 	add_topic_multi("sensor_mag", 0, 4);
-	add_topic_multi("sensor_baro", 0, 4);
 }
 
 void LoggedTopics::add_mavlink_tunnel()

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -122,6 +122,7 @@ void RtlDirectMissionLand::setActiveMissionItems()
 {
 	WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
 
 	// Climb to altitude
 	if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
@@ -203,8 +204,6 @@ void RtlDirectMissionLand::setActiveMissionItems()
 
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
-
-			pos_sp_triplet->previous = pos_sp_triplet->current;
 		}
 
 		if (num_found_items > 0) {
@@ -213,6 +212,12 @@ void RtlDirectMissionLand::setActiveMissionItems()
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
+		// Only set the previous position item if the current one really changed
+		if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
+		    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
+			pos_sp_triplet->previous = current_setpoint_copy;
+		}
+
 		// prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
 		// is not achieved.
 		const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING

src/modules/navigator/rtl_mission_fast.cpp

@@ -91,6 +91,7 @@ void RtlMissionFast::setActiveMissionItems()
 {
 	WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
 
 	/* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/
 	if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) &&
@@ -158,17 +159,20 @@ void RtlMissionFast::setActiveMissionItems()
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
 
-			pos_sp_triplet->previous = pos_sp_triplet->current;
 		}
 
-
-
 		if (num_found_items > 0) {
 			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
 		}
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
+		// Only set the previous position item if the current one really changed
+		if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
+		    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
+			pos_sp_triplet->previous = current_setpoint_copy;
+		}
+
 		if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() &&
 		    _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
 			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -814,7 +814,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg)
 	report.max_distance = static_cast<float>(msg.range_max());
 	report.current_distance = static_cast<float>(msg.ranges()[0]);
 	report.variance = 0.0f;
-	report.signal_quality = 100;
+	report.signal_quality = -1;
 	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
 
 	gz::msgs::Quaternion pose_orientation = msg.world_pose().orientation();