squashed

msg/CMakeLists.txt

@@ -227,6 +227,7 @@ set(msg_files
 	VehicleImu.msg
 	VehicleImuStatus.msg
 	VehicleLocalPositionSetpoint.msg
+	TaskLocalPositionSetpoint.msg
 	VehicleMagnetometer.msg
 	VehicleOpticalFlow.msg
 	VehicleOpticalFlowVel.msg

msg/EstimatorStatus.msg

@@ -39,14 +39,12 @@ uint8 CS_FIXED_WING = 17	# 17 - true when thought to be operating as a fixed win
 uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetometer has been declared faulty and is no longer being used
 uint8 CS_ASPD = 19		# 19 - true when airspeed measurements are being fused
 uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect induced static pressure rise is active
-uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected
 uint8 CS_GPS_YAW = 22		# 22 - true when yaw (not ground course) data from a GPS receiver is being fused
 uint8 CS_MAG_ALIGNED = 23	# 23 - true when the in-flight mag field alignment has been completed
 uint8 CS_EV_VEL = 24		# 24 - true when local frame velocity data fusion from external vision measurements is intended
 uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measurement for the magnetometer Z component
 uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
 uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
-uint8 CS_RNG_FAULT = 28		# 28 - true when the range finder has been declared faulty and is no longer being used
 uint8 CS_GNSS_VEL = 44		# 44 - true if GNSS velocity measurements are being fused
 
 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults

msg/EstimatorStatusFlags.msg

@@ -25,14 +25,12 @@ bool cs_fixed_wing            # 17 - true when the vehicle is operating as a fix
 bool cs_mag_fault             # 18 - true when the magnetometer has been declared faulty and is no longer being used
 bool cs_fuse_aspd             # 19 - true when airspeed measurements are being fused
 bool cs_gnd_effect            # 20 - true when protection from ground effect induced static pressure rise is active
-bool cs_rng_stuck             # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 bool cs_gnss_yaw              # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed
 bool cs_ev_vel                # 24 - true when local frame velocity data fusion from external vision measurements is intended
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gnss_yaw_fault        # 27 - true when the GNSS heading has been declared faulty and is no longer being used
-bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
@@ -49,7 +47,6 @@ bool cs_valid_fake_pos          # 41 - true if a valid constant position is bein
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
 bool cs_gnss_vel                # 44 - true if GNSS velocity measurement fusion is intended
-bool cs_rng_kin_unknown      	# 45 - true when the range finder kinematic consistency check is not running
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/PositionSetpoint.msg

@@ -36,3 +36,5 @@ float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
+
+# TOPICS position_setpoint task_position_setpoint

msg/TaskLocalPositionSetpoint.msg

@@ -0,0 +1,19 @@
+# Local position setpoint in NED frame
+# Telemetry of PID position controller to monitor tracking.
+# NaN means the state was not controlled
+
+uint64 timestamp	# time since system start (microseconds)
+
+float32 x		# in meters NED
+float32 y		# in meters NED
+float32 z		# in meters NED
+
+float32 vx		# in meters/sec
+float32 vy		# in meters/sec
+float32 vz		# in meters/sec
+
+float32[3] acceleration # in meters/sec^2
+float32[3] thrust	# normalized thrust vector in NED
+
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec

msg/VehicleConstraints.msg

@@ -7,3 +7,5 @@ float32 speed_up # in meters/sec
 float32 speed_down # in meters/sec
 
 bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)
+
+bool lock_dist_bottom # altitude is locked to the current distance to ground

range_alt_notes.txt

@@ -0,0 +1,20 @@
+## FlightTaskManualAltitude
+- dist_bottom_var -- currently terrain variance, I see occasionally dist_bottom diverge and then clamp back to expected
+- ground slowdown (_respectGroundSlowdown) uses mpc_land_alt ... weird, remove?
+- _respectMaxAltitude ... weird, remove? We can instead use dist_bottom validity
+- _respectMinAltitude ... weird, remove? No need for a function
+
+
+## FlightTask
+- _dist_to_bottom and _dist_to_ground ... confusing, unify
+-
+
+## FlightTaskAuto
+- reuse logic for range alt lock
+- _prepareLandSetpoints -- Slow down automatic descend close to ground ... use only with terrain estimate available? (baro estimate unreliable)
+
+## EKF
+- Vz does not reflect true Vz due to noisy baro
+- Vz errors cause rangefinder kinematic consistency check to fail
+- Position setpoint error remains over long periods (might be related to Vz issues above)
+

src/modules/ekf2/CMakeLists.txt

@@ -211,7 +211,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
-		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/jake_range_height_control.cpp
 		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -124,7 +124,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		aid_sources/range_finder/range_finder_consistency_check.cpp
-		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/jake_range_height_control.cpp
 		aid_sources/range_finder/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)

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

@@ -1,84 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Sensor.hpp
- * Abstract class for sensors
- *
- * @author Mathieu Bresciani <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/jake_range_height_control.cpp

@@ -0,0 +1,425 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 range_height_control.cpp
+ * Control functions for ekf range finder height fusion
+ */
+
+#include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_h.h"
+#include "ekf_derivation/generated/compute_hagl_innov_var.h"
+
+void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
+{
+	// Check if rangefinder is available/enabled
+	if (!_range_buffer) {
+		// PX4_INFO("no buff");
+		return;
+	}
+
+	// Pop rangefinder measurement from buffer of samples into active sample
+	sensor::rangeSample sample = {};
+	if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
+		if (_range_sensor.timedOut(imu_sample.time_us)) {
+			// Disable fusion if it's currently enabled
+			if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
+				PX4_INFO("stopping RNG fusion, sensor timed out");
+				stopRangeAltitudeFusion();
+				stopRangeTerrainFusion();
+			}
+			// PX4_INFO("timed out1");
+		}
+		return;
+	}
+
+	// PX4_INFO("got one!");
+	// PX4_INFO("rng: %f", (double)sample.rng);
+	// PX4_INFO("quality: %d", sample.quality);
+
+	// Set the raw sample
+	_range_sensor.setSample(sample);
+
+	// TODO: move setting params to init function
+	// Set all of the parameters
+	_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
+	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
+
+	// Update sensor to earth rotation
+	_range_sensor.updateSensorToEarthRotation(_R_to_earth);
+
+
+	// TODO: refactor into validity_checks()
+	// Gate sample consumption on these checks
+	bool quality_ok = sample.quality > 0; // TODO: what about unknown? (-1)
+	bool tilt_ok = _range_sensor.isTiltOk();
+	bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal();
+	// - Not stuck value
+	// - Not fog detected
+
+	// If quality, tilt, or value are outside of bounds -- throw away measurement
+	if (!quality_ok || !tilt_ok || !range_ok) {
+		if (_range_sensor.timedOut(imu_sample.time_us)) {
+			// Disable fusion if it's currently enabled
+			if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
+				PX4_INFO("stopping RNG fusion, sensor data invalid");
+				stopRangeAltitudeFusion();
+				stopRangeTerrainFusion();
+			}
+			PX4_INFO("timed out2");
+		}
+
+		if (!quality_ok) {
+			PX4_INFO("!quality_ok");
+		}
+		if (!tilt_ok) {
+			PX4_INFO("!tilt_ok");
+		}
+		if (!range_ok) {
+			PX4_INFO("!range_ok");
+		} // commander takeoff
+		return;
+	}
+
+
+	// TODO: refactor into apply_body_offset()
+	// Correct the range data for position offset relative to the IMU
+	const Vector3f rng_pos_body = { _params.ekf2_rng_pos_x, _params.ekf2_rng_pos_y, _params.ekf2_rng_pos_z };
+	const Vector3f imu_pos_body = _params.imu_pos_body;
+	const Vector3f pos_offset_body = rng_pos_body - imu_pos_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+	sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt();
+
+	// Provide sample from buffer to object
+	_range_sensor.setSample(sample);
+
+
+	// 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 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_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);
+
+	// Run the kinematic consistency check
+	_rng_consistency_check.run(z, z_var, vz, vz_var, dist, dist_var, imu_sample.time_us);
+
+	// Track kinematic consistency
+	// _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+	_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
+	updateRangeHagl(_aid_src_rng_hgt);
+
+	if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) {
+		PX4_INFO("INFINIFY OBSERVATION INVALID");
+	}
+
+	// Fuse Range data as Primary height source
+	// NOTE: terrain is not estimated in this mode
+	if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
+		fuseRangeAsHeightSource();
+	} else {
+		// Fuse Range data as aiding height source (Primary GPS or Baro)
+		fuseRangeAsHeightAiding();
+	}
+}
+
+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();
+		_state.terrain = 0.f;
+
+		// TODO: needed? It's set above in --> resetAidSourceStatusZeroInnovation()
+		// _aid_src_rng_hgt.time_last_fuse = imu_sample.time_us;
+	}
+
+
+}
+
+void Ekf::fuseRangeAsHeightAiding()
+{
+	bool range_aid_conditional = _params.rng_ctrl == RngCtrl::CONDITIONAL;
+	bool range_aid_enabled = _params.rng_ctrl == RngCtrl::ENABLED;
+
+	bool range_aid_conditions_passed = rangeAidConditionsPassed();
+	bool kinematically_consistent = _control_status.flags.rng_kin_consistent;
+
+	bool do_range_aid = kinematically_consistent &&
+						(range_aid_enabled || (range_aid_conditional && range_aid_conditions_passed));
+
+	bool fuse_measurement = false;
+
+
+	// Variables to use below
+	bool innovation_rejected = _aid_src_rng_hgt.innovation_rejected;
+	bool optical_flow_for_terrain = _control_status.flags.opt_flow_terrain;
+
+	// Fuse Range into Altitude if:
+	// - passes range_aid_conditionalchecks
+	// - kinematically consistent
+	if (do_range_aid) {
+		// Start fusion
+		if (!_control_status.flags.rng_hgt) {
+			// Fusion init logic
+			PX4_INFO("starting RNG Altitude fusion");
+			_control_status.flags.rng_hgt = true;
+			_height_sensor_ref = HeightSensor::RANGE;
+
+			// TODO: do we really need to stop terrain fusion here?
+			stopRangeTerrainFusion();
+
+			// TODO: review for correctness
+			if (innovation_rejected && kinematically_consistent) {
+				// Reset aid source
+				PX4_INFO("range alt fusion, resetting aid source");
+				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+			}
+		}
+
+		// Fuse
+		fuse_measurement = true;
+
+	} else {
+		// Stop fusion
+		stopRangeAltitudeFusion();
+
+		// if (!range_aid_conditions_passed) {
+		// 	PX4_INFO("range aid conditions failed");
+		// }
+		// if (!kinematically_consistent) {
+		// 	PX4_INFO("kinematically inconsistent");
+		// }
+	}
+
+	// Fuse Range into Terrain if:
+	// - kinematically consistent
+	if (kinematically_consistent) {
+
+		// Start fusion
+		if (!_control_status.flags.rng_terrain) {
+			// Fusion init logic
+			PX4_INFO("starting RNG Terrain fusion");
+			_control_status.flags.rng_terrain = true;
+
+			static bool first_init = true;
+			// Reset terrain when we first init
+			if (!optical_flow_for_terrain && first_init) {
+				first_init = false;
+				// Reset aid source and then reset terrain estimate
+				PX4_INFO("FIRST range terrain fusion, resetting terrain to range");
+				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+				resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
+				resetTerrainToRng(_aid_src_rng_hgt);
+			}
+		}
+
+		// Reset terrain to range if innovation is rejected
+		if (!optical_flow_for_terrain && innovation_rejected
+				&& kinematically_consistent) {
+			PX4_INFO("range terrain fusion, resetting terrain to range");
+			resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+			resetTerrainToRng(_aid_src_rng_hgt);
+		}
+
+		// Fuse
+		fuse_measurement = true;
+
+	} else {
+		// Stop fusion
+		stopRangeTerrainFusion();
+	}
+
+	if (fuse_measurement) {
+		// PX4_INFO("fusing: %f", (double)_aid_src_rng_hgt.observation);
+		// PX4_INFO("_control_status.flags.rng_hgt: %d", _control_status.flags.rng_hgt);
+		// PX4_INFO("_control_status.flags.rng_terrain: %d", _control_status.flags.rng_terrain);
+		fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
+
+		// commander takeoff
+
+		// TODO: _height_sensor_ref == HeightSensor::RANGE  is redundant
+		if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
+			PX4_INFO("RNG height fusion reset required, all height sources failing");
+
+			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()) {
+				PX4_INFO("resetting vertical velocity");
+				resetVerticalVelocityToZero();
+			}
+
+			_aid_src_rng_hgt.time_last_fuse = timestamp;
+		}
+	}
+}
+
+bool Ekf::rangeAidConditionsPassed()
+{
+	// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
+	// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
+	float range_hagl_max = _params.max_hagl_for_range_aid;
+	float max_vel_xy = _params.max_vel_for_range_aid;
+
+	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
+
+	bool is_hagl_stable = (hagl_test_ratio < 1.f);
+
+	if (!_control_status.flags.rng_hgt) {
+		range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
+		max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
+		is_hagl_stable = (hagl_test_ratio < 0.01f);
+	}
+
+	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::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
+{
+	// Since the distance is not a direct observation of the terrain state but is based
+	// on the height state, a reset should consider the height uncertainty. This can be
+	// done by manipulating the Kalman gain to inject all the innovation in the terrain state
+	// and create the correct correlation with the terrain state with a covariance update.
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 0.f);
+
+	const float old_terrain = _state.terrain;
+
+	VectorState H;
+	sym::ComputeHaglH(&H);
+
+	VectorState K;
+	K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset"
+
+	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
+
+	// record the state change
+	const float delta_terrain = _state.terrain - old_terrain;
+
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+
+	aid_src.time_last_fuse = _time_delayed_us;
+}
+
+void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
+{
+	const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
+	const float measurement_variance = getRngVar();
+
+	float innovation_variance;
+	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
+
+	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
+	updateAidSourceStatus(aid_src,
+			      _range_sensor.sample()->time_us, // sample timestamp
+			      measurement,                               // observation
+			      measurement_variance,                      // observation variance
+			      getHagl() - measurement,                   // innovation
+			      innovation_variance,                       // innovation variance
+			      innov_gate);                               // innovation gate
+
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
+	}
+}
+
+float Ekf::getRngVar() const
+{
+	const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
+	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
+	return dist_var;
+}
+
+void Ekf::stopRangeAltitudeFusion()
+{
+	if (_control_status.flags.rng_hgt) {
+		PX4_INFO("stopping RNG Altitude fusion");
+		_control_status.flags.rng_hgt = false;
+		if (_height_sensor_ref == HeightSensor::RANGE) {
+			_height_sensor_ref = HeightSensor::UNKNOWN;
+		}
+	}
+}
+
+void Ekf::stopRangeTerrainFusion()
+{
+	if (_control_status.flags.rng_terrain) {
+		PX4_INFO("stopping RNG Terrain fusion");
+		_control_status.flags.rng_terrain = false;
+	}
+}

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

@@ -44,6 +44,8 @@ using namespace matrix;
 void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom,
 				       const float dist_bottom_var)
 {
+	printf("RangeFinderConsistencyCheck::init\n");
+
 	_P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
 	_Ht = sym::RangeValidationFilterH<float>();
 
@@ -89,7 +91,6 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
 			H(RangeFilter::z.idx) = 1.f;
 			H(RangeFilter::terrain.idx) = 0.f;
 			R = z_var;
-
 		}
 
 		// residual
@@ -130,25 +131,19 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
 void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
 {
 	// start the consistency check after 1s
-	if (_t_since_first_sample > _t_to_init) {
-		if (fabsf(_test_ratio_lpf.getState()) < 1.f) {
-			const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);
-
-			if (!horizontal_motion && vertical_motion) {
-				_state = KinematicState::CONSISTENT;
-
-			} else if (_state == KinematicState::CONSISTENT || vertical_motion) {
-				_state = KinematicState::UNKNOWN;
-			}
-
-		} else {
-			_t_since_first_sample = 0.f;
-			_state = KinematicState::INCONSISTENT;
-		}
-
-	} else {
+	if (_t_since_first_sample < _t_to_init) {
 		_t_since_first_sample += dt;
+		return;
 	}
+
+	if (fabsf(_test_ratio_lpf.getState()) > 1.f) {
+		printf("_test_ratio_lpf failed (>1)\n");
+		_t_since_first_sample = 0.f;
+		_state = KinematicState::INCONSISTENT;
+		return;
+	}
+
+	_state = KinematicState::CONSISTENT;
 }
 
 void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var,

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

@@ -73,7 +73,6 @@ public:
 	}
 
 	uint8_t current_posD_reset_count{0};
-	bool horizontal_motion{false};
 
 private:
 

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

@@ -44,64 +44,61 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
-	bool rng_data_ready = false;
-
-	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_sensor.isDataReady()) {
-
-			_range_sensor.setPitchOffset(_params.rng_sens_pitch);
-			_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-			_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
-			_range_sensor.setMaxFogDistance(_params.rng_fog);
-			_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-
-			_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
-
-			if (_range_sensor.isDataHealthy()) {
-				// correct the range data for position offset relative to the IMU
-				const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
-				const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-				_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
-
-				const float dist_var = getRngVar();
-				_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
-
-				const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
-
-				if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
-					_rng_consistency_check.reset();
-				}
-
-				_rng_consistency_check.horizontal_motion = updated_horizontal_motion;
-				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);
-				_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
-							   dist_var, imu_sample.time_us);
-
-			} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
-				_range_sensor.setRange(_params.rng_gnd_clearance);
-				_range_sensor.setValidity(true);
-
-			} else {
-				_rng_consistency_check.reset();
-			}
-		}
-
-		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
-		_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
-							&& _rng_consistency_check.isNotKinematicallyInconsistent();
-
-	} else {
+	if (!_range_buffer) {
 		return;
 	}
 
+	// Get range data from buffer and check validity
+	bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.sample());
+
+	_range_sensor.setSample();
+
+	_range_sensor.setDataReadiness(rng_data_ready);
+
+	if (_range_sensor.isDataReady()) {
+
+		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
+		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+		_range_sensor.setMaxFogDistance(_params.rng_fog);
+		_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
+
+		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
+
+		if (_range_sensor.isDataHealthy()) {
+			// correct the range data for position offset relative to the IMU
+			const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
+
+			// TODO: this is a constant
+			const float dist_var = getRngVar();
+			_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
+
+			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);
+
+			// TODO: review -- variance
+			_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
+						   dist_var, imu_sample.time_us);
+
+		} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
+			_range_sensor.setRange(_params.rng_gnd_clearance);
+			_range_sensor.setValidity(true);
+
+		} else {
+			_rng_consistency_check.reset();
+		}
+	}
+
+	_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
+						&& _rng_consistency_check.isNotKinematicallyInconsistent();
+
+
 	auto &aid_src = _aid_src_rng_hgt;
 
-	if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) {
+	if (_range_sensor.isDataReady() && _range_sensor.sample()) {
 
 		updateRangeHagl(aid_src);
 		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
@@ -113,6 +110,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				&& _range_sensor.isDataHealthy()
 				&& _rng_consistency_check.isNotKinematicallyInconsistent();
 
+		// SUS: _rng_consistency_check.isNotKinematicallyInconsistent()
+
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();
@@ -121,12 +120,14 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 						      && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
 						      && isConditionalRangeAidSuitable();
 
+		// SUS: isConditionalRangeAidSuitable()
+
 		const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 					  && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
 
 		if (_control_status.flags.rng_hgt) {
 			if (!(do_conditional_range_aid || do_range_aid)) {
-				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
+				PX4_INFO("stopping %s fusion", HGT_SRC_NAME);
 				stopRngHgtFusion();
 			}
 
@@ -134,7 +135,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			if (_params.height_sensor_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);
+					PX4_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 
 					_control_status.flags.rng_hgt = true;
@@ -149,7 +150,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				} 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);
+					PX4_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 
 					_information_events.flags.reset_hgt_to_rng = true;
@@ -164,7 +165,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 			} else {
 				if (do_conditional_range_aid || do_range_aid) {
-					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
+					PX4_INFO("starting %s height fusion", HGT_SRC_NAME);
 					_control_status.flags.rng_hgt = true;
 
 					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
@@ -185,7 +186,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 				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);
+					PX4_INFO("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_rng = true;
 					resetAltitudeTo(aid_src.observation - _state.terrain);
@@ -201,7 +202,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				} 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);
+						PX4_INFO("stopping %s fusion, fusion failing", HGT_SRC_NAME);
 						stopRngHgtFusion();
 						stopRngTerrFusion();
 
@@ -212,7 +213,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				}
 
 			} else {
-				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
+				PX4_INFO("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
 				stopRngTerrFusion();
 			}
@@ -239,7 +240,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 	} 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);
+		PX4_INFO("stopping %s fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
 		stopRngTerrFusion();
 	}
@@ -255,7 +256,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
 	updateAidSourceStatus(aid_src,
-			      _range_sensor.getSampleAddress()->time_us, // sample timestamp
+			      _range_sensor.sample()->time_us, // sample timestamp
 			      measurement,                               // observation
 			      measurement_variance,                      // observation variance
 			      getHagl() - measurement,                   // innovation

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

@@ -47,10 +47,34 @@ namespace estimator
 namespace sensor
 {
 
-void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
+void SensorRangeFinder::setSample(const rangeSample &sample)
 {
-	updateSensorToEarthRotation(R_to_earth);
-	updateValidity(current_time_us);
+	_sample = sample;
+}
+
+bool SensorRangeFinder::timedOut(uint64_t time_now) const
+{
+	if (_sample.time_us > time_now) {
+		return false;
+	}
+
+	// TODO: 200ms?
+	return time_now > _sample.time_us + 200'000;
+}
+
+void SensorRangeFinder::setPitchOffset(float new_pitch_offset)
+{
+	if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) {
+		_sin_pitch_offset = sinf(new_pitch_offset);
+		_cos_pitch_offset = cosf(new_pitch_offset);
+		_pitch_offset_rad = new_pitch_offset;
+	}
+}
+
+void SensorRangeFinder::setLimits(float min_distance, float max_distance)
+{
+	_rng_valid_min_val = min_distance;
+	_rng_valid_max_val = max_distance;
 }
 
 void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
@@ -60,115 +84,5 @@ void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_ear
 	_cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset;
 }
 
-void SensorRangeFinder::updateValidity(uint64_t current_time_us)
-{
-	updateDtDataLpf(current_time_us);
-
-	if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
-		_is_sample_valid = false;
-		_is_regularly_sending_data = false;
-		return;
-	}
-
-	_is_regularly_sending_data = true;
-
-	// Don't run the checks unless we have retrieved new data from the buffer
-	if (_is_sample_ready) {
-		_is_sample_valid = false;
-
-		_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
-
-		if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) {
-			return;
-		}
-
-		updateStuckCheck();
-		updateFogCheck(getDistBottom(), _sample.time_us);
-
-		if (!_is_stuck && !_is_blocked) {
-			_is_sample_valid = true;
-			_time_last_valid_us = _sample.time_us;
-		}
-	}
-}
-
-bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const
-{
-	return current_time_us - _time_bad_quality_us > _quality_hyst_us;
-}
-
-void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us)
-{
-	// Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant.
-	float alpha = 0.5f * _dt_update;
-	_dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us);
-
-	// Apply spike protection to the filter state.
-	_dt_data_lpf = fminf(_dt_data_lpf, 4e6f);
-}
-
-inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const
-{
-	return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL;
-}
-
-inline bool SensorRangeFinder::isDataInRange() const
-{
-	return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val);
-}
-
-void SensorRangeFinder::updateStuckCheck()
-{
-	if (!isStuckDetectorEnabled()) {
-		// Stuck detector disabled
-		_is_stuck = false;
-		return;
-	}
-
-	// Check for "stuck" range finder measurements when range was not valid for certain period
-	// This handles a failure mode observed with some lidar sensors
-	if (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) {
-
-		// require a variance of rangefinder values to check for "stuck" measurements
-		if (_stuck_max_val - _stuck_min_val > _stuck_threshold) {
-			_stuck_min_val = 0.0f;
-			_stuck_max_val = 0.0f;
-			_is_stuck = false;
-
-		} else {
-			if (_sample.rng > _stuck_max_val) {
-				_stuck_max_val = _sample.rng;
-			}
-
-			if (_stuck_min_val < 0.1f || _sample.rng < _stuck_min_val) {
-				_stuck_min_val = _sample.rng;
-			}
-
-			_is_stuck = true;
-		}
-	}
-}
-
-void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
-{
-	if (_max_fog_dist > 0.f) {
-
-		const float median_dist = _median_dist.apply(dist_bottom);
-		const float factor = 2.f; // magic hardcoded factor
-
-		if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) {
-			_is_blocked = true;
-
-		} else if (_is_blocked && median_dist > factor * _max_fog_dist) {
-			_is_blocked = false;
-		}
-
-		_prev_median_dist = median_dist;
-
-	} else {
-		_prev_median_dist = dist_bottom;
-	}
-}
-
 } // namespace sensor
 } // namespace estimator

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

@@ -41,8 +41,6 @@
 #ifndef EKF_SENSOR_RANGE_FINDER_HPP
 #define EKF_SENSOR_RANGE_FINDER_HPP
 
-#include "Sensor.hpp"
-
 #include <matrix/math.hpp>
 #include <lib/mathlib/math/filter/MedianFilter.hpp>
 
@@ -57,105 +55,62 @@ struct rangeSample {
 	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
 };
 
-static constexpr uint64_t RNG_MAX_INTERVAL =
-	200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
 
-class SensorRangeFinder : public Sensor
+class SensorRangeFinder
 {
 public:
 	SensorRangeFinder() = default;
-	~SensorRangeFinder() override = default;
+	~SensorRangeFinder() = default;
 
-	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
-	bool isHealthy() const override { return _is_sample_valid; }
-	bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
-	bool isDataReady() const { return _is_sample_ready; }
-	bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
-	bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; }
+	struct Parameters {
+		float ekf2_imu_pos_x{};
+		float ekf2_imu_pos_y{};
+		float ekf2_imu_pos_z{};
+
+		float ekf2_rng_pos_x{};
+		float ekf2_rng_pos_y{};
+		float ekf2_rng_pos_z{};
+
+		float ekf2_rng_pitch{};
+
+		float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt
+	};
+
+	void updateParameters(Parameters& parameters) { _parameters = parameters; };
+
+	bool timedOut(uint64_t time_now) const;
+	void setSample(const rangeSample &sample);
 
-	void setSample(const rangeSample &sample)
-	{
-		_sample = sample;
-		_is_sample_ready = true;
-	}
 
 	// This is required because of the ring buffer
 	// TODO: move the ring buffer here
-	rangeSample *getSampleAddress() { return &_sample; }
+	rangeSample *sample() { return &_sample; }
 
-	void setPitchOffset(float new_pitch_offset)
-	{
-		if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) {
-			_sin_pitch_offset = sinf(new_pitch_offset);
-			_cos_pitch_offset = cosf(new_pitch_offset);
-			_pitch_offset_rad = new_pitch_offset;
-		}
-	}
+	void setPitchOffset(float new_pitch_offset);
 
 	void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; }
 
-	void setLimits(float min_distance, float max_distance)
-	{
-		_rng_valid_min_val = min_distance;
-		_rng_valid_max_val = max_distance;
-	}
-
-	void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; }
-
-	void setQualityHysteresis(float valid_quality_threshold_s)
-	{
-		_quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f);
-	}
-
 	float getCosTilt() const { return _cos_tilt_rng_to_earth; }
 
-	void setRange(float rng) { _sample.rng = rng; }
-	float getRange() const { return _sample.rng; }
+	void setLimits(float min_distance, float max_distance);
+
+	// void setRange(float rng) { _sample.rng = rng; }
+	// float getRange() const { return _sample.rng; }
 
 	float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }
 
-	void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; }
-	void setValidity(bool is_valid) { _is_sample_valid = is_valid; }
-
 	float getValidMinVal() const { return _rng_valid_min_val; }
 	float getValidMaxVal() const { return _rng_valid_max_val; }
 
-	void setFaulty(bool faulty = true) { _is_faulty = faulty; }
-
-private:
 	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
 
-	void updateValidity(uint64_t current_time_us);
-	void updateDtDataLpf(uint64_t current_time_us);
-	bool isSampleOutOfDate(uint64_t current_time_us) const;
-	bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
 	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
-	bool isDataInRange() const;
-	bool isQualityOk(uint64_t current_time_us) const;
-	void updateStuckCheck();
-	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
+
+private:
 
 	rangeSample _sample{};
 
-	bool _is_sample_ready{};	///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
-	bool _is_sample_valid{};	///< true if range finder sample retrieved from buffer is valid
-	bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval
-	uint64_t _time_last_valid_us{};	///< time the last range finder measurement was ready (uSec)
-	bool _is_faulty{false};         ///< the sensor should not be used anymore
-
-	/*
-	 * Stuck check
-	 */
-	bool _is_stuck{};
-	float _stuck_threshold{0.1f};	///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m), set to zero to disable
-	float _stuck_min_val{};		///< minimum value for new rng measurement when being stuck
-	float _stuck_max_val{};		///< maximum value for new rng measurement when being stuck
-
-	/*
-	 * Data regularity check
-	 */
-	static constexpr float _dt_update{0.01f}; 	///< delta time since last ekf update TODO: this should be a parameter
-	float _dt_data_lpf{};	///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
+	Parameters _parameters{};
 
 	/*
 	 * Tilt check
@@ -172,20 +127,6 @@ private:
 	float _rng_valid_min_val{};	///< minimum distance that the rangefinder can measure (m)
 	float _rng_valid_max_val{};	///< maximum distance that the rangefinder can measure (m)
 
-	/*
-	 * Quality check
-	 */
-	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
-	uint64_t _quality_hyst_us{};		///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
-
-	/*
-	 * Fog check
-	 */
-	bool _is_blocked{false};
-	float _max_fog_dist{0.f};	//< maximum distance at which the range finder could detect fog (m)
-	math::MedianFilter<float, 5> _median_dist{};
-	float _prev_median_dist{0.f};
-
 };
 
 } // namespace sensor

src/modules/ekf2/EKF/common.h

@@ -148,7 +148,7 @@ enum class GnssCtrl : uint8_t {
 	YAW  = (1 << 3)
 };
 
-enum class RngCtrl : uint8_t {
+enum RngCtrl : uint8_t {
 	DISABLED    = 0,
 	CONDITIONAL = 1,
 	ENABLED     = 2
@@ -415,17 +415,19 @@ struct parameters {
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
-	float rng_sens_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
-	float range_noise_scaler{0.0f};         ///< scaling from range measurement to noise (m/m)
+	float ekf2_rng_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
+	float ekf2_rng_sfe{0.0f};         ///< scaling from range measurement to noise (m/m)
 	float max_hagl_for_range_aid{5.0f};     ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
 	float max_vel_for_range_aid{1.0f};      ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
 	float range_aid_innov_gate{1.0f};       ///< gate size used for innovation consistency checks for range aid fusion
-	float range_valid_quality_s{1.0f};      ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
 	float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check
 	float rng_fog{0.f};                 	///< max distance which a blocked range sensor measures (fog, dirt) [m]
 
-	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
+	float ekf2_rng_pos_x{0.f};
+	float ekf2_rng_pos_y{0.f};
+	float ekf2_rng_pos_z{0.f};
+
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -570,8 +572,6 @@ uint64_t mag_fault               :
 		uint64_t fuse_aspd               : 1; ///< 19 - true when airspeed measurements are being fused
 uint64_t gnd_effect              :
 		1; ///< 20 - true when protection from ground effect induced static pressure rise is active
-uint64_t rng_stuck               :
-		1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 uint64_t gnss_yaw                 :
 		1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 		uint64_t mag_aligned_in_flight   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
@@ -582,8 +582,6 @@ uint64_t synthetic_mag_z         :
 		uint64_t vehicle_at_rest         : 1; ///< 26 - true when the vehicle is at rest
 uint64_t gnss_yaw_fault           :
 		1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
-uint64_t rng_fault               :
-		1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
 uint64_t inertial_dead_reckoning :
 		1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 		uint64_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
@@ -604,7 +602,6 @@ uint64_t mag_heading_consistent  :
 		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
 		uint64_t baro_fault              : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
 		uint64_t gnss_vel                : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
-		uint64_t rng_kin_unknown	 : 1; ///< 45 - true when the range finder kinematic consistency check is not running
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/ekf.cpp

@@ -77,13 +77,6 @@ void Ekf::reset()
 	_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
 #endif // CONFIG_EKF2_TERRAIN
 
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
-	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
-	_range_sensor.setMaxFogDistance(_params.rng_fog);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
 
@@ -390,6 +383,18 @@ void Ekf::updateParameters()
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 	_aux_global_position.updateParameters();
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+#if defined (CONFIG_EKF2_RANGE_FINDER)
+
+	sensor::SensorRangeFinder::Parameters params = {};
+	// params.ekf2_imu_pos_x = _param_ekf2_imu_pos_x.get();
+	// params.ekf2_imu_pos_y = _param_ekf2_imu_pos_y.get();
+	// params.ekf2_imu_pos_z = _param_ekf2_imu_pos_z.get();
+	// params.ekf2_rng_pos_x = _param_ekf2_rng_pos_x.get();
+	// params.ekf2_rng_pos_y = _param_ekf2_rng_pos_y.get();
+	// params.ekf2_rng_pos_z = _param_ekf2_rng_pos_z.get();
+	params.ekf2_rng_pitch = _params.ekf2_rng_pitch;
+	_range_sensor.updateParameters(params);
+#endif
 }
 
 template<typename T>

src/modules/ekf2/EKF/ekf.h

@@ -762,9 +762,18 @@ private:
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
 	void controlRangeHaglFusion(const imuSample &imu_delayed);
+
+
+	void fuseRangeAsHeightSource();
+	void fuseRangeAsHeightAiding();
+
 	bool isConditionalRangeAidSuitable();
-	void stopRngHgtFusion();
-	void stopRngTerrFusion();
+	bool rangeAidConditionsPassed();
+
+
+	void stopRangeAltitudeFusion();
+	void stopRangeTerrainFusion();
+
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)

src/modules/ekf2/EKF/estimator_interface.h

@@ -120,8 +120,6 @@ public:
 	{
 		_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)

src/modules/ekf2/EKF/terrain_control.cpp

@@ -54,7 +54,6 @@ 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

src/modules/ekf2/EKF2.cpp

@@ -155,18 +155,17 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_ctrl(_params->rng_ctrl),
 	_param_ekf2_rng_delay(_params->range_delay_ms),
 	_param_ekf2_rng_noise(_params->range_noise),
-	_param_ekf2_rng_sfe(_params->range_noise_scaler),
+	_param_ekf2_rng_sfe(_params->ekf2_rng_sfe),
 	_param_ekf2_rng_gate(_params->range_innov_gate),
-	_param_ekf2_rng_pitch(_params->rng_sens_pitch),
+	_param_ekf2_rng_pitch(_params->ekf2_rng_pitch),
 	_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
 	_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
 	_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
-	_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
 	_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
 	_param_ekf2_rng_fog(_params->rng_fog),
-	_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
-	_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
-	_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
+	_param_ekf2_rng_pos_x(_params->ekf2_rng_pos_x),
+	_param_ekf2_rng_pos_y(_params->ekf2_rng_pos_y),
+	_param_ekf2_rng_pos_z(_params->ekf2_rng_pos_z),
 #endif // CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_param_ekf2_ev_delay(_params->ekf2_ev_delay),
@@ -1601,8 +1600,16 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
+
+	// TODO: this makes dist_bottom a function of the Terrain State estimate, which is not
+	// exactly correct. Terrain State can diverge from the distance_sensor.current_distance
+	// when fusion is disabled. This causes Terrain Hold to drift due to dist_bottom diverging.
+	// Ultimately the logic for rangefinder fusion needs to be improved, and when distance_sensor
+	// is not fused into the Terrain State estimate we should not use dist_bottom for Altitude Lock.
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
+	// TODO: review, we should use sensor variance not terrain
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
+
 	_ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter);
 
 	lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE;
@@ -1910,14 +1917,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_mag_fault             = _ekf.control_status_flags().mag_fault;
 		status_flags.cs_fuse_aspd             = _ekf.control_status_flags().fuse_aspd;
 		status_flags.cs_gnd_effect            = _ekf.control_status_flags().gnd_effect;
-		status_flags.cs_rng_stuck             = _ekf.control_status_flags().rng_stuck;
 		status_flags.cs_gnss_yaw               = _ekf.control_status_flags().gnss_yaw;
 		status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
 		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
 		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
 		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
 		status_flags.cs_gnss_yaw_fault         = _ekf.control_status_flags().gnss_yaw_fault;
-		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
 		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
@@ -1934,7 +1939,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_constant_pos        = _ekf.control_status_flags().constant_pos;
 		status_flags.cs_baro_fault	    = _ekf.control_status_flags().baro_fault;
 		status_flags.cs_gnss_vel            = _ekf.control_status_flags().gnss_vel;
-		status_flags.cs_rng_kin_unknown     = _ekf.control_status_flags().rng_kin_unknown;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;

src/modules/ekf2/EKF2.hpp

@@ -619,7 +619,6 @@ private:
 		(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_A_IGATE>) _param_ekf2_rng_a_igate,
-		(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,7 +48,7 @@ parameters:
       description:
         short: Measurement noise for range finder fusion
       type: float
-      default: 0.1
+      default: 0.05
       min: 0.01
       unit: m
       decimal: 2
@@ -93,16 +93,6 @@ parameters:
       unit: SD
       min: 0.1
       max: 5.0
-    EKF2_RNG_QLTY_T:
-      description:
-        short: Minumum range validity period
-        long: Minimum duration during which the reported range finder signal quality
-          needs to be non-zero in order to be declared valid (s)
-      type: float
-      default: 1.0
-      unit: s
-      min: 0.1
-      max: 5
     EKF2_RNG_K_GATE:
       description:
         short: Gate size used for range finder kinematic consistency check
@@ -122,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.05
+      default: 0.01
       min: 0.0
       max: 0.2
       unit: m/m

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -123,7 +123,6 @@ TEST_F(EkfBasicsTest, initialControlMode)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
@@ -179,7 +178,6 @@ TEST_F(EkfBasicsTest, gpsFusion)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -51,7 +51,6 @@ public:
 		_range_finder.setPitchOffset(0.f);
 		_range_finder.setCosMaxTilt(0.707f);
 		_range_finder.setLimits(_min_range, _max_range);
-		_range_finder.setMaxFogDistance(2.f);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test
@@ -118,9 +117,8 @@ TEST_F(SensorRangeFinderTest, setRange)
 	sample.time_us = 1e6;
 	sample.quality = 9;
 
-	_range_finder.setRange(sample.rng);
-	_range_finder.setDataReadiness(true);
-	_range_finder.setValidity(true);
+	_range_finder.setSample(sample)
+
 	EXPECT_TRUE(_range_finder.isDataHealthy());
 	EXPECT_TRUE(_range_finder.isHealthy());
 }
@@ -363,7 +361,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// WHEN: sensor is then blocked by fog
 	// range jumps to value below 2m
-	uint64_t t_now_us = _range_finder.getSampleAddress()->time_us;
+	uint64_t t_now_us = _range_finder.sample()->time_us;
 	rangeSample sample{t_now_us, 1.f, 100};
 	updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
 
@@ -373,7 +371,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// WHEN: the sensor is not blocked by fog anymore
 	sample.rng = 5.f;
-	sample.time_us = _range_finder.getSampleAddress()->time_us;
+	sample.time_us = _range_finder.sample()->time_us;
 	updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
 
 	// THEN: the data should be marked as healthy again

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -97,94 +97,132 @@ void FlightTaskManualAltitude::_scaleSticks()
 
 void FlightTaskManualAltitude::_updateAltitudeLock()
 {
-	// Depending on stick inputs and velocity, position is locked.
-	// If not locked, altitude setpoint is set to NAN.
+	// - check if sticks are released (braking)
+	// - check if no vertical motion (altitude lock) (noisy baro, increase default threshold)
+	// - check if horizontal motion is within limit (altitude lock)
 
-	// Check if user wants to break
-	const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON;
-
-	// Check if vehicle has stopped
-	const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());
-
-	// Manage transition between use of distance to ground and distance to local origin
-	// when terrain hold behaviour has been selected.
-	if (_param_mpc_alt_mode.get() == 2) {
-		// Use horizontal speed as a transition criteria
-		float spd_xy = Vector2f(_velocity).length();
-
-		// Use presence of horizontal stick inputs as a transition criteria
-		float stick_xy = Vector2f(_sticks.getPitchRollExpo()).length();
-		bool stick_input = stick_xy > 0.001f;
-
-		if (_terrain_hold) {
-			bool too_fast = spd_xy > _param_mpc_hold_max_xy.get();
-
-			if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
-				// Stop using distance to ground
-				_terrain_hold = false;
-
-				// Adjust the setpoint to maintain the same height error to reduce control transients
-				if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
-					_position_setpoint(2) = _position(2) - (_dist_to_ground_lock - _dist_to_bottom);
-
-				} else {
-					_position_setpoint(2) = _position(2);
-					_dist_to_ground_lock = NAN;
-				}
-			}
-
-		} else {
-			bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get() && stopped;
-
-			if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
-				// Start using distance to ground
-				_terrain_hold = true;
-
-				// Adjust the setpoint to maintain the same height error to reduce control transients
-				if (PX4_ISFINITE(_position_setpoint(2))) {
-					_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
-				}
-			}
+	// First check if user is controlling Z velocity with sticks
+	if (fabsf(_sticks.getPositionExpo()(2)) > 0.001f) {
+		if (PX4_ISFINITE(_position_setpoint(2)) || PX4_ISFINITE(_dist_to_bottom_lock)) {
+			// PX4_INFO("Setting position sp to NAN");
+			_current_mode = AltitudeMode::None;
+			_position_setpoint(2) = NAN;
+			_dist_to_bottom_lock = NAN;
+			_constraints.lock_dist_bottom = false;
 		}
 
+		return;
 	}
 
-	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
-		// terrain following
-		_terrainFollowing(apply_brake, stopped);
+	switch ((AltitudeMode)_param_mpc_alt_mode.get()) {
+	case AltitudeMode::AltitudeFollow: {
+			// Altitude following - relative earth frame origin which may drift due to sensors
+			// - No user throttle
+			_altitude_follow_mode();
+			break;
+		}
 
-	} else {
-		// normal mode where height is dependent on local frame
+	case AltitudeMode::TerrainFollow: {
+			// Terrain following - relative to ground which changes with terrain variation
+			// - distance sensor valid
+			// - No user throttle
 
-		if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) {
-			// lock position
-			_position_setpoint(2) = _position(2);
-
-			// Ensure that minimum altitude is respected if
-			// there is a distance sensor and distance to bottom is below minimum.
-			if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _min_distance_to_ground) {
-				_terrainFollowing(apply_brake, stopped);
+			// Cannot perform terrain follow without distance sensor
+			if (PX4_ISFINITE(_dist_to_bottom)) {
+				_terrain_follow_mode();
 
 			} else {
-				_dist_to_ground_lock = NAN;
+				_altitude_follow_mode();
+				break;
 			}
+			break;
+		}
 
-		} else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) {
-			// Position is locked but check if a reset event has happened.
-			// We will shift the setpoints.
-			if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) {
-				_position_setpoint(2) = _position(2);
-				_reset_counter = _sub_vehicle_local_position.get().z_reset_counter;
+	case AltitudeMode::TerrainHold: {
+			// Terrain hold - relative to ground when within thresholds
+			// - distance sensor valid
+			// - No user throttle
+			// - XY vel low
+			// - Z vel stopped
+
+			// Cannot perform terrain hold without distance sensor
+			if (PX4_ISFINITE(_dist_to_bottom)) {
+				_terrain_hold_mode();
+
+			} else {
+				_altitude_follow_mode();
+				break;
 			}
+		}
 
-		} else  {
-			// user demands velocity change
-			_position_setpoint(2) = NAN;
-			// ensure that maximum altitude is respected
+	case AltitudeMode::None: {
+			// Nothing to do
+			break;
 		}
 	}
+}
+
+void FlightTaskManualAltitude::_terrain_hold_mode()
+{
+	// Check if XY velocity is within limit to activate Terrain Hold
+	bool xy_vel_okay = Vector2f(_velocity).length() < _param_mpc_hold_max_xy.get();
+
+	if (!xy_vel_okay) {
+		// XY_vel is above threshold, just follow altitude setpoint
+		// Lock the position setpoint to the current Z position estimate
+		if (_current_mode != AltitudeMode::AltitudeFollow) {
+			_current_mode = AltitudeMode::AltitudeFollow;
+
+			PX4_INFO("Locking to Z estimate");
+			_position_setpoint(2) = _position(2);
+			_dist_to_bottom_lock = NAN;
+			_constraints.lock_dist_bottom = false;
+		}
+
+		return;
+	}
+
+	if (_current_mode != AltitudeMode::TerrainHold) {
+		// Set velocity setpoint to 0, switch into TerrainHold
+		_current_mode = AltitudeMode::TerrainHold;
+		_velocity_setpoint(2) = 0.f;
+		PX4_INFO("setting terrain hold");
+		return;
+	}
+
+	if (!PX4_ISFINITE(_dist_to_bottom_lock)) {
+		// Wait for Z to come to a stop before locking in the distance
+		if (fabsf(_velocity(2)) < 0.1f) {
+			_dist_to_bottom_lock = _dist_to_bottom;
+			_position_setpoint(2) = _position(2);
+			_constraints.lock_dist_bottom = true;
+			PX4_INFO("Locking distance to %f", (double)_dist_to_bottom);
+		}
+
+		return;
+	}
+
+	// TODO:
+	// - use dist_bottom_var as heuristic for distance lock
+
+	// All criteria met
+	// - distance sensor valid
+	// - No user throttle
+	// - XY vel low
+	// - Z vel stopped
+	// Update position setpoint to keep fixed distance from terrain
+	float delta_distance = _dist_to_bottom - _dist_to_bottom_lock;
+	_position_setpoint(2) = _position(2) + delta_distance;
+}
+
+void FlightTaskManualAltitude::_terrain_follow_mode()
+{
+
+}
+
+void FlightTaskManualAltitude::_altitude_follow_mode()
+{
 
-	_respectMaxAltitude();
 }
 
 void FlightTaskManualAltitude::_respectMinAltitude()
@@ -198,7 +236,7 @@ void FlightTaskManualAltitude::_respectMinAltitude()
 
 void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
 {
-	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) {
+	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_bottom_lock)) {
 		// User wants to break and vehicle reached zero velocity. Lock height to ground.
 
 		// lock position
@@ -206,19 +244,19 @@ void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
 		// ensure that minimum altitude is respected
 		_respectMinAltitude();
 		// lock distance to ground but adjust first for minimum altitude
-		_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
+		_dist_to_bottom_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
 
-	} else if (apply_brake && PX4_ISFINITE(_dist_to_ground_lock)) {
+	} else if (apply_brake && PX4_ISFINITE(_dist_to_bottom_lock)) {
 		// vehicle needs to follow terrain
 
 		// difference between the current distance to ground and the desired distance to ground
-		const float delta_distance_to_ground = _dist_to_ground_lock - _dist_to_bottom;
+		const float delta_distance_to_ground = _dist_to_bottom_lock - _dist_to_bottom;
 		// adjust position setpoint for the delta (note: NED frame)
 		_position_setpoint(2) = _position(2) - delta_distance_to_ground;
 
 	} else {
 		// user demands velocity change in D-direction
-		_dist_to_ground_lock = _position_setpoint(2) = NAN;
+		_dist_to_bottom_lock = _position_setpoint(2) = NAN;
 	}
 }
 
@@ -269,7 +307,8 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
 
 void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl)
 {
-	_dist_to_ground_lock = NAN;
+	PX4_INFO("_ekfResetHandlerHagl");
+	_dist_to_bottom_lock = NAN;
 }
 
 void FlightTaskManualAltitude::_updateSetpoints()
@@ -278,7 +317,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
 				      _yaw_setpoint);
 	_updateAltitudeLock();
-	_respectGroundSlowdown();
+	// _respectGroundSlowdown();
 }
 
 bool FlightTaskManualAltitude::_checkTakeoff()

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -45,6 +45,13 @@
 #include "StickTiltXY.hpp"
 #include <uORB/Subscription.hpp>
 
+enum class AltitudeMode : int {
+	AltitudeFollow,
+	TerrainFollow,
+	TerrainHold,
+	None,
+};
+
 class FlightTaskManualAltitude : public FlightTask
 {
 public:
@@ -76,7 +83,8 @@ protected:
 	StickYaw _stick_yaw{this};
 
 	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
-	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
+	// bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
+	AltitudeMode _current_mode = AltitudeMode::AltitudeFollow;
 
 	float _velocity_constraint_up{INFINITY};
 	float _velocity_constraint_down{INFINITY};
@@ -85,15 +93,20 @@ protected:
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
 					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
-					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
-					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
-					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below which to land with land speed */
-					(ParamFloat<px4::params::MPC_LAND_SPEED>)
-					_param_mpc_land_speed, /**< desired downwards speed when approaching the ground */
-					(ParamFloat<px4::params::MPC_TKO_SPEED>)
-					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
+					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
+					(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
+					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 				       )
 private:
+
+	void _terrain_hold_mode();
+	void _terrain_follow_mode();
+	void _altitude_follow_mode();
 	/**
 	 * Terrain following.
 	 * During terrain following, the position setpoint is adjusted
@@ -119,8 +132,6 @@ private:
 	 */
 	void _respectGroundSlowdown();
 
-	void setGearAccordingToSwitch();
-
 	bool _updateYawCorrection();
 
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
@@ -132,7 +143,7 @@ private:
 	 * Distance to ground during terrain following.
 	 * If user does not demand velocity change in D-direction and the vehcile
 	 * is in terrain-following mode, then height to ground will be locked to
-	 * _dist_to_ground_lock.
+	 * _dist_to_bottom_lock.
 	 */
-	float _dist_to_ground_lock = NAN;
+	float _dist_to_bottom_lock = NAN;
 };

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -107,7 +107,7 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
 
-	if (!_terrain_hold) {
+	if (_current_mode != AltitudeMode::TerrainHold) {
 		if (_terrain_hold_previous) {
 			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
 			_smoothing.setCurrentPosition(_position(2));
@@ -116,5 +116,18 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 		_position_setpoint(2) = _smoothing.getCurrentPosition();
 	}
 
-	_terrain_hold_previous = _terrain_hold;
+	_terrain_hold_previous = _current_mode == AltitudeMode::TerrainHold;
+
+	task_local_position_setpoint_s msg = {};
+
+	msg.timestamp = hrt_absolute_time();
+
+	msg.x = _position_setpoint(0);
+	msg.y = _position_setpoint(1);
+	msg.z = _position_setpoint(2);
+	msg.vx = _velocity_setpoint(0);
+	msg.vy = _velocity_setpoint(1);
+	msg.vz = _velocity_setpoint(2);
+
+	_setpoint_pub.publish(msg);
 }

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -42,6 +42,9 @@
 #include "FlightTaskManualAltitude.hpp"
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
+#include <uORB/Publication.hpp>
+#include <uORB/topics/task_local_position_setpoint.h>
+
 class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 {
 public:
@@ -62,12 +65,7 @@ protected:
 
 	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
 
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
-					(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
-					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
-					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
-				       )
-
 private:
+	uORB::Publication<task_local_position_setpoint_s> _setpoint_pub{ORB_ID(task_local_position_setpoint)};
 	bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
 };

src/modules/logger/logged_topics.cpp

@@ -143,6 +143,8 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_local_position", 100);
 	add_topic("vehicle_local_position_setpoint", 100);
+	// DEBUG
+	add_topic("task_local_position_setpoint");
 	add_topic("vehicle_magnetometer", 200);
 	add_topic("vehicle_rates_setpoint", 20);
 	add_topic("vehicle_roi", 1000);
@@ -316,6 +318,10 @@ void LoggedTopics::add_estimator_replay_topics()
 	add_topic("vehicle_visual_odometry");
 	add_topic("aux_global_position");
 	add_topic_multi("distance_sensor");
+	add_topic("vehicle_local_position");
+	add_topic("estimator_states");
+	add_topic("estimator_status");
+	add_topic("estimator_aid_src_rng_hgt");
 }
 
 void LoggedTopics::add_thermal_calibration_topics()

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -800,7 +800,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 = -1;
+	report.signal_quality = 100; // Always perfect in sim
 	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
 
 	gz::msgs::Quaternion pose_orientation = msg.world_pose().orientation();