cs_check: remove unnecessary constrain

instead only leaving tilt. adjust function / variable names accordingly.

msg/CMakeLists.txt

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

msg/EstimatorStatus.msg

@@ -39,12 +39,14 @@ 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,12 +25,14 @@ 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

msg/PositionSetpoint.msg

@@ -36,5 +36,3 @@ 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

@@ -1,19 +0,0 @@
-# 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,5 +7,3 @@ 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

msg/versioned/VehicleCommand.msg

@@ -77,6 +77,7 @@ uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVL
 uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal.
 
 uint16 VEHICLE_CMD_MISSION_START = 300 # Start running a mission. |first_item: the first mission item to run|last_item: the last mission item to run (after this item is run, the mission ends)|
+uint16 VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK = 309 # Do control surface preflight check. Axis|value [-1, 1]|Unused|Unused|Unused|Unused|Unused|
 uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command. |[@range -1,1] value|[s] timeout|Unused|Unused|output function|
 uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command. |configuration|Unused|Unused|Unused|output function|
 uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component. |1 to arm, 0 to disarm.
@@ -177,6 +178,12 @@ int8 ARMING_ACTION_ARM = 1
 uint8 GRIPPER_ACTION_RELEASE = 0
 uint8 GRIPPER_ACTION_GRAB = 1
 
+# param1 in VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK
+uint8 AXIS_ROLL = 0
+uint8 AXIS_PITCH = 1
+uint8 AXIS_YAW = 2
+uint8 AXIS_COLLECTIVE_TILT = 3
+
 uint8 ORB_QUEUE_LENGTH = 8
 
 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.

platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c

@@ -206,7 +206,7 @@ static void init_timers_dma_up(void)
 		timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
 
 		if (timer_configs[timer_index].dma_handle == NULL) {
-			PX4_DEBUG("Failed to allocate Timer %u DMA UP", timer_index);
+			PX4_WARN("Failed to allocate Timer %u DMA UP", timer_index);
 			continue;
 		}
 
@@ -214,12 +214,16 @@ static void init_timers_dma_up(void)
 		timer_configs[timer_index].initialized = true;
 	}
 
-	// Free the allocated DMA channels
-	for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
-		if (timer_configs[timer_index].dma_handle != NULL) {
-			stm32_dmafree(timer_configs[timer_index].dma_handle);
-			timer_configs[timer_index].dma_handle = NULL;
-			PX4_INFO("Freed DMA UP Timer Index %u", timer_index);
+	// Bidirectional DShot will free/allocate DMA stream on every update event. This is required
+	// in order to reconfigure the DMA stream between Timer Burst and CaptureCompare.
+	if (_bidirectional) {
+		// Free the allocated DMA channels
+		for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
+			if (timer_configs[timer_index].dma_handle != NULL) {
+				stm32_dmafree(timer_configs[timer_index].dma_handle);
+				timer_configs[timer_index].dma_handle = NULL;
+				PX4_INFO("Freed DMA UP Timer Index %u", timer_index);
+			}
 		}
 	}
 }
@@ -341,8 +345,15 @@ void up_dshot_trigger()
 
 			io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count);
 
-			// Allocate DMA
-			if (timer_configs[timer_index].dma_handle == NULL) {
+			if (_bidirectional) {
+				// Deallocate DMA from previous transaction
+				if (timer_configs[timer_index].dma_handle != NULL) {
+					stm32_dmastop(timer_configs[timer_index].dma_handle);
+					stm32_dmafree(timer_configs[timer_index].dma_handle);
+					timer_configs[timer_index].dma_handle = NULL;
+				}
+
+				// Allocate DMA
 				timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
 
 				if (timer_configs[timer_index].dma_handle == NULL) {
@@ -502,11 +513,8 @@ static void capture_complete_callback(void *arg)
 	// Disable capture DMA
 	io_timer_capture_dma_req(timer_index, capture_channel, false);
 
-	if (timer_configs[timer_index].dma_handle != NULL) {
-		stm32_dmastop(timer_configs[timer_index].dma_handle);
-		stm32_dmafree(timer_configs[timer_index].dma_handle);
-		timer_configs[timer_index].dma_handle = NULL;
-	}
+	// Stop DMA (should already be finished)
+	stm32_dmastop(timer_configs[timer_index].dma_handle);
 
 	// Re-initialize all output channels on this timer
 	for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
@@ -553,7 +561,7 @@ void process_capture_results(uint8_t timer_index, uint8_t channel_index)
 
 	} else {
 		// Convert the period to eRPM
-		_erpms[output_channel] = (1000000 * 60) / period;
+		_erpms[output_channel] = (1000000 * 60 / 100 + period / 2) / period;
 	}
 
 	// We set it ready anyway, not to hold up other channels when used in round robin.

range_alt_notes.txt

@@ -1,20 +0,0 @@
-## 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/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -101,3 +101,8 @@ void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_mo
 		}
 	}
 }
+
+void ActuatorEffectiveness::overrideCollectiveTilt(bool do_override, float collective_tilt)
+{
+	// empty implementation to be overridden if needed.
+}

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -219,6 +219,20 @@ public:
 	 */
 	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
 
+	/**
+	 * If overridden by derived classes, optionally bypass the info usually
+	 * contained in tiltrotor_extra_controls -- normalised collective thrust
+	 * and tilt setpoints.
+	 *
+	 * Base class implementation is empty.
+	 *
+	 * @param bypass Flag indicating whether or not to use the other
+	 * arguments to bypass setpoints
+	 * @param collective_tilt Collective tilt normalized setpoint, in [0, 1]. 0: vertical, 1: horizontal.
+	 * @param collective_thrust Collective thrust normalized setpoint, in [0, 1].
+	 */
+	virtual void overrideCollectiveTilt(bool do_override, float collective_tilt);
+
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
 	uint32_t _stopped_motors_mask{0};

src/modules/commander/Commander.cpp

@@ -302,6 +302,51 @@ int Commander::custom_command(int argc, char *argv[])
 		return 0;
 	}
 
+	if (!strcmp(argv[0], "cs_check")) {
+
+		if (argc > 1) {
+
+			float torque = 1.0;
+
+			if (argc > 2) {
+				const float user_torque = std::atof(argv[2]);
+
+				// If instead argv[2] is not a float at all, we get torque=0 and nothing happens
+				if (!std::isnan(user_torque) && PX4_ISFINITE(user_torque)) {
+					torque = user_torque;
+
+				} else {
+					PX4_WARN("cs_check: torque \"%s\" is invalid. Using default +1.0.", argv[2]);
+				}
+			}
+
+			if (!strcmp(argv[1], "roll")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_ROLL, torque);
+
+			} else if (!strcmp(argv[1], "pitch")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_PITCH, torque);
+
+			} else if (!strcmp(argv[1], "yaw")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_YAW, torque);
+
+			} else if (!strcmp(argv[1], "tilt")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_COLLECTIVE_TILT,
+						     torque);
+
+			} else {
+				PX4_ERR("argument %s unsupported.", argv[1]);
+				return 1;
+			}
+
+			return 0;
+
+		} else {
+			PX4_ERR("missing argument");
+		}
+
+		return 0;
+	}
+
 	if (!strcmp(argv[0], "arm")) {
 		float param2 = 0.f;
 
@@ -1517,6 +1562,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
 	case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE:
 	case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION:
+	case vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
@@ -2255,9 +2301,7 @@ void Commander::handleAutoDisarm()
 		// Check for auto-disarm on landing or pre-flight
 		if (_param_com_disarm_land.get() > 0 || _param_com_disarm_prflt.get() > 0) {
 
-			const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
-							 && !_mission_result_sub.get().finished;
-			const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !landed_amid_mission
+			const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !_mission_in_progress
 							      && !_config_overrides.disable_auto_disarm;
 
 			if (auto_disarm_land_enabled && _have_taken_off_since_arming) {
@@ -3014,6 +3058,9 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false);
 	PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("cs_check", "Run control surface preflight checks");
+	PRINT_MODULE_USAGE_ARG("roll|pitch|yaw|tilt", "Axis", false);
+	PRINT_MODULE_USAGE_ARG("torque", "Normalized torque command [-1.0, +1.0], default +1.0", true);
 	PRINT_MODULE_USAGE_COMMAND("arm");
 	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true);
 	PRINT_MODULE_USAGE_COMMAND("disarm");

src/modules/control_allocator/ControlAllocator.cpp

@@ -46,6 +46,8 @@
 #include <mathlib/math/Limits.hpp>
 #include <mathlib/math/Functions.hpp>
 
+#include <math.h>
+
 using namespace matrix;
 using namespace time_literals;
 
@@ -333,6 +335,12 @@ ControlAllocator::Run()
 		return;
 	}
 
+	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+	const hrt_abstime now = hrt_absolute_time();
+	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
+
+	bool is_vtol = false;
+
 	{
 		vehicle_status_s vehicle_status;
 
@@ -340,6 +348,10 @@ ControlAllocator::Run()
 
 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 
+			if (_armed && _preflight_check_running) {
+				preflight_check_abort(now);
+			}
+
 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
 
 			// Check if the current flight phase is HOVER or FIXED_WING
@@ -360,6 +372,9 @@ ControlAllocator::Run()
 				}
 			}
 
+			// Set this here to avoid another class member
+			is_vtol = vehicle_status.is_vtol;
+
 			// Forward to effectiveness source
 			_actuator_effectiveness->setFlightPhase(flight_phase);
 		}
@@ -373,9 +388,7 @@ ControlAllocator::Run()
 		}
 	}
 
-	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
-	const hrt_abstime now = hrt_absolute_time();
-	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
+	preflight_check_handle_command(now);
 
 	bool do_update = false;
 	vehicle_torque_setpoint_s vehicle_torque_setpoint;
@@ -424,12 +437,18 @@ ControlAllocator::Run()
 			}
 		}
 
+		preflight_check_update_state(now);
+		preflight_check_overwrite_torque_sp(c, is_vtol);
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
 			_control_allocation[i]->setControlSetpoint(c[i]);
 
 			// Do allocation
 			_control_allocation[i]->allocate();
+
+			preflight_check_handle_tilt_control(now);
+
 			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
 			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
 								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
@@ -460,6 +479,177 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
+void ControlAllocator::preflight_check_handle_command(hrt_abstime now)
+{
+
+	// Only start the check if prearmed but not armed. Otherwise, output a
+	// warning message and reject the command.
+
+	vehicle_command_s vehicle_command;
+
+	if (_vehicle_command_sub.update(&vehicle_command)) {
+
+		uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK) {
+			if (!_armed) {
+
+				actuator_armed_s armed;
+
+				if (_armed_sub.copy(&armed))  {
+
+					const bool prearmed = armed.prearmed;
+
+					if (prearmed) {
+						// currently this does not check prearmed status. if not prearmed, it will just do nothing.
+						preflight_check_start(vehicle_command, now);
+						result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
+
+					} else {
+						result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+						PX4_WARN("Control surface preflight check rejected (not prearmed)");
+					}
+
+				} else {
+					result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					PX4_WARN("Control surface preflight check rejected (failed to get prearmed state)");
+				}
+
+			} else {
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				PX4_WARN("Control surface preflight check rejected (armed)");
+			}
+
+			preflight_check_send_ack(result, now);
+		}
+	}
+}
+
+void ControlAllocator::preflight_check_start(vehicle_command_s &cmd, hrt_abstime now)
+{
+
+	// If already running, abort it. this is only to send the correct ack.
+	if (_preflight_check_running) {
+		preflight_check_abort(now);
+	}
+
+	const int axis = (uint8_t) lroundf(cmd.param1);
+	const float input = cmd.param2;
+
+	_preflight_check_running = true;
+	_preflight_check_axis = axis;
+	_preflight_check_input = input;
+	_preflight_check_started = now;
+	_last_preflight_check_command = cmd;
+}
+
+void ControlAllocator::preflight_check_send_ack(uint8_t result, hrt_abstime now)
+{
+	if (_last_preflight_check_command.from_external) {
+		vehicle_command_ack_s command_ack{};
+		command_ack.timestamp = now;
+		command_ack.command = _last_preflight_check_command.command;
+		command_ack.result = result;
+		command_ack.target_system = _last_preflight_check_command.source_system;
+		command_ack.target_component = _last_preflight_check_command.source_component;
+
+		uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
+		command_ack_pub.publish(command_ack);
+	}
+
+	_preflight_check_last_ack = now;
+}
+
+void ControlAllocator::preflight_check_abort(hrt_abstime now)
+{
+	_preflight_check_running = false;
+	preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
+
+}
+
+void ControlAllocator::preflight_check_finish(hrt_abstime now)
+{
+	_preflight_check_running = false;
+	preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED, now);
+}
+
+
+void ControlAllocator::preflight_check_update_state(hrt_abstime now)
+{
+	if (_preflight_check_running) {
+
+		if (now - _preflight_check_started >= PREFLIGHT_CHECK_DURATION) {
+			preflight_check_finish(now);
+		}
+
+		if (now - _preflight_check_last_ack >= PREFLIGHT_CHECK_ACK_PERIOD) {
+			preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS, now);
+		}
+	}
+}
+
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES>
+		(&c)[ActuatorEffectiveness::MAX_NUM_MATRICES], bool is_vtol)
+{
+	if (!_preflight_check_running) { return; }
+
+	int axis = -1;
+
+	switch (_preflight_check_axis) {
+	case vehicle_command_s::AXIS_ROLL:
+		axis = 0;
+		break;
+
+	case vehicle_command_s::AXIS_PITCH:
+		axis = 1;
+		break;
+
+	case vehicle_command_s::AXIS_YAW:
+		axis = 2;
+		break;
+
+	default:
+		// If none of roll, pitch, yaw, we do nothing here
+		return;
+	}
+
+	// If VTOL, instance 1 is the fixed wing part, otherwise instance 0.
+	const int instance = is_vtol ? 1 : 0;
+
+	c[instance](0) = 0.;
+	c[instance](1) = 0.;
+	c[instance](2) = 0.;
+	c[instance](axis) = _preflight_check_input;
+}
+
+void ControlAllocator::preflight_check_handle_tilt_control(hrt_abstime now)
+{
+	const bool is_tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+
+	if (_preflight_check_running) {
+
+		if (_preflight_check_axis == vehicle_command_s::AXIS_COLLECTIVE_TILT) {
+
+			if (is_tiltrotor) {
+				_actuator_effectiveness->overrideCollectiveTilt(true, _preflight_check_input);
+
+			} else {
+				// Commanded collective tilt axis but the vehicle is not a tiltrotor. Abort
+				_preflight_check_running = false;
+				PX4_WARN("Control surface preflight check rejected (tilt commanded but not available)");
+				preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED, now);
+
+			}
+		}
+
+	} else {
+		// strictly speaking this is only necessary if is_tiltrotor but
+		// can't hurt to call it always in case other
+		// ActuatorEffectiveness* classes implement similar things
+		_actuator_effectiveness->overrideCollectiveTilt(false, 0.0f);
+	}
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {

src/modules/control_allocator/ControlAllocator.hpp

@@ -68,6 +68,8 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/actuator_servos.h>
 #include <uORB/topics/actuator_servos_trim.h>
@@ -77,8 +79,11 @@
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/failure_detector_status.h>
 
+
 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -88,6 +93,9 @@ public:
 	static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS;
 	static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS;
 
+	static constexpr uint32_t PREFLIGHT_CHECK_DURATION = 500_ms;
+	static constexpr uint32_t PREFLIGHT_CHECK_ACK_PERIOD = 1000_ms;
+
 	using ActuatorVector = ActuatorEffectiveness::ActuatorVector;
 
 	ControlAllocator();
@@ -138,6 +146,16 @@ private:
 
 	void publish_actuator_controls();
 
+	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES],
+			const bool is_vtol);
+	void preflight_check_handle_command(const hrt_abstime now);
+	void preflight_check_update_state(const hrt_abstime now);
+	void preflight_check_handle_tilt_control(const hrt_abstime now);
+	void preflight_check_start(vehicle_command_s &cmd, const hrt_abstime now);
+	void preflight_check_send_ack(uint8_t result, const hrt_abstime now);
+	void preflight_check_abort(const hrt_abstime now);
+	void preflight_check_finish(const hrt_abstime now);
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -180,6 +198,9 @@ private:
 	uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1};  /**< vehicle torque setpoint subscription (2. instance) */
 	uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */
 
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+
 	// Outputs
 	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
 
@@ -201,6 +222,14 @@ private:
 	// For example, the system might report two motor failures, but only the first one is handled by CA
 	uint16_t _handled_motor_failure_bitmask{0};
 
+	bool _preflight_check_running{false};
+
+	uint8_t _preflight_check_axis{0};
+	float _preflight_check_input{0.0f};
+	vehicle_command_s _last_preflight_check_command{0};
+	hrt_abstime _preflight_check_started{0};
+	hrt_abstime _preflight_check_last_ack{0};
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -124,88 +124,118 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
 
 }
 
+void ActuatorEffectivenessTiltrotorVTOL::overrideCollectiveTilt(bool do_override, float collective_tilt)
+{
+	// if _do_override_collective_tilt (used for control surface preflight
+	// check), we alter the behaviour of processTiltrotorControls in these
+	// two ways:
+	// - collective tilt and thrust setpoints are NOT taken from uOrb
+	//   message, but from class member variable, which we can arbitrarily set
+	//   before calling this function using overrideCollectiveTilt
+	// - collective tilt is added to actuator_sp even if
+	//   (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT)
+	//   evaluates to false
+
+	_do_override_collective_tilt = do_override;
+	_collective_tilt_normalized_setpoint = collective_tilt;
+}
+
+void ActuatorEffectivenessTiltrotorVTOL::processTiltrotorControls(ActuatorVector &actuator_sp,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+
+{
+	tiltrotor_extra_controls_s tiltrotor_extra_controls;
+
+	if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || _do_override_collective_tilt) {
+
+		float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
+		float control_collective_thrust = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
+
+		if (_do_override_collective_tilt) {
+			control_collective_tilt = _collective_tilt_normalized_setpoint * 2.f - 1.f;
+		}
+
+		// set control_collective_tilt to exactly -1 or 1 if close to these end points
+		control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
+		control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
+
+		// initialize _last_collective_tilt_control
+		if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+			_last_collective_tilt_control = control_collective_tilt;
+
+		} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+			_collective_tilt_updated = true;
+			_last_collective_tilt_control = control_collective_tilt;
+		}
+
+		// During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle the effectiveness
+		// of the thrust axis in z is apporaching 0, and by that is increasing the motor output to max.
+		// Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid
+		// a thrust spike when the transition is initiated (as then the tilt is fully forward).
+		if (_flight_phase == FlightPhase::TRANSITION_HF_TO_FF) {
+			_last_collective_tilt_control = math::constrain(_last_collective_tilt_control, -1.f, 0.f);
+
+		} else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) {
+			_last_collective_tilt_control = -1.f;
+		}
+
+		bool yaw_saturated_positive = true;
+		bool yaw_saturated_negative = true;
+
+		for (int i = 0; i < _tilts.count(); ++i) {
+			if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
+
+				// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
+				if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || _do_override_collective_tilt) {
+					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
+
+				} else {
+					actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
+				}
+			}
+
+			// custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit
+			if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) {
+
+				if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+					yaw_saturated_positive = false;
+				}
+
+				if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+					yaw_saturated_negative = false;
+				}
+
+			} else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) {
+				if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+					yaw_saturated_negative = false;
+				}
+
+				if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+					yaw_saturated_positive = false;
+				}
+			}
+		}
+
+		_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
+		_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
+
+		// in FW directly use throttle sp
+		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+			for (int i = 0; i < _first_tilt_idx; ++i) {
+				actuator_sp(i) = control_collective_thrust;
+			}
+		}
+	}
+}
+
 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
-	// apply tilt
 	if (matrix_index == 0) {
-		tiltrotor_extra_controls_s tiltrotor_extra_controls;
 
-		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
-			float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
-
-			// set control_collective_tilt to exactly -1 or 1 if close to these end points
-			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
-			control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
-
-			// initialize _last_collective_tilt_control
-			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
-				_last_collective_tilt_control = control_collective_tilt;
-
-			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
-				_collective_tilt_updated = true;
-				_last_collective_tilt_control = control_collective_tilt;
-			}
-
-			// During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle the effectiveness
-			// of the thrust axis in z is apporaching 0, and by that is increasing the motor output to max.
-			// Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid
-			// a thrust spike when the transition is initiated (as then the tilt is fully forward).
-			if (_flight_phase == FlightPhase::TRANSITION_HF_TO_FF) {
-				_last_collective_tilt_control = math::constrain(_last_collective_tilt_control, -1.f, 0.f);
-
-			} else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) {
-				_last_collective_tilt_control = -1.f;
-			}
-
-			bool yaw_saturated_positive = true;
-			bool yaw_saturated_negative = true;
-
-			for (int i = 0; i < _tilts.count(); ++i) {
-				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
-
-					// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
-					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) {
-						actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
-
-					} else {
-						actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
-					}
-				}
-
-				// custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit
-				if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) {
-
-					if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
-						yaw_saturated_positive = false;
-					}
-
-					if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
-						yaw_saturated_negative = false;
-					}
-
-				} else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) {
-					if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
-						yaw_saturated_negative = false;
-					}
-
-					if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
-						yaw_saturated_positive = false;
-					}
-				}
-			}
-
-			_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
-			_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
-
-			// in FW directly use throttle sp
-			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
-				for (int i = 0; i < _first_tilt_idx; ++i) {
-					actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
-				}
-			}
-		}
+		processTiltrotorControls(actuator_sp, actuator_min, actuator_max);
 
 		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
 			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -88,6 +88,13 @@ public:
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 
+	void overrideCollectiveTilt(bool do_override, float collective_tilt) override;
+
+	void processTiltrotorControls(ActuatorVector &actuator_sp,
+				      const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+				      const matrix::Vector<float, NUM_ACTUATORS> &actuator_max);
+
+
 protected:
 	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
@@ -114,6 +121,9 @@ protected:
 
 	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
 
+	bool _do_override_collective_tilt{false};
+	float _collective_tilt_normalized_setpoint{0.5f};
+
 private:
 
 	void updateParams() override;

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/jake_range_height_control.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
 	)

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/jake_range_height_control.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
 	)

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/jake_range_height_control.cpp

@@ -1,425 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -36,123 +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_lpf.reset(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_lpf.setParameters(dt, _t_to_init);
-			_test_ratio_lpf.update(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)
-{
-	// start the consistency check after 1s
-	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,
-				      const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
-{
-	if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
-		_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

@@ -48,61 +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 _initialized ? _test_ratio_lpf.getState() : 0.f; }
-	float getInnov() const { return _initialized ? _innov : 0.f; }
-	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
-	bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT && _t_since_first_sample > _t_to_init; }
-	bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; }
-	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()
-	{
-		if (_initialized && _state == KinematicState::CONSISTENT) {
-			_state = KinematicState::UNKNOWN;
-		}
+	void setGate(float gate) { _gate = gate; }
 
-		_initialized = false;
-	}
-
-	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};
-	AlphaFilter<float> _test_ratio_lpf{time_constant};
-	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};
+	uint64_t _time_last_update_us{};
+	float _dist_bottom_prev{};
+
+	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{};
+
+	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

@@ -44,23 +44,18 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
-	if (!_range_buffer) {
-		return;
-	}
+	bool rng_data_ready = false;
 
-	// 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()) {
+	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);
 
+		// update range sensor angle parameters in case they have changed
 		_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);
 
@@ -70,35 +65,39 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			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();
+			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));
 
-			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 float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+				const float var = sq(_params.range_noise) + dist_dependant_var;
 
-			// 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);
+				_rng_consistency_check.setGate(_params.range_kin_consistency_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);
+			}
 
 		} else {
-			_rng_consistency_check.reset();
+			// 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.rng_gnd_clearance);
+				_range_sensor.setValidity(true); // bypass the checks
+			}
 		}
+
+		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	} else {
+		return;
 	}
 
-	_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.sample()) {
+	if (rng_data_ready && _range_sensor.getSampleAddress()) {
 
 		updateRangeHagl(aid_src);
 		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
@@ -108,26 +107,23 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				&& _control_status.flags.tilt_align
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy()
-				&& _rng_consistency_check.isNotKinematicallyInconsistent();
-
-		// SUS: _rng_consistency_check.isNotKinematicallyInconsistent()
+				&& _rng_consistency_check.isKinematicallyConsistent();
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();
 
+
 		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 						      && (_params.rng_ctrl == static_cast<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)) {
-				PX4_INFO("stopping %s fusion", HGT_SRC_NAME);
+				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
 				stopRngHgtFusion();
 			}
 
@@ -135,14 +131,13 @@ 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.
-					PX4_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
+					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
-					    && _rng_consistency_check.isKinematicallyConsistent()) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -150,7 +145,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
-					PX4_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
+					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 
 					_information_events.flags.reset_hgt_to_rng = true;
@@ -165,11 +160,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 			} else {
 				if (do_conditional_range_aid || do_range_aid) {
-					PX4_INFO("starting %s height fusion", HGT_SRC_NAME);
+					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
-					    && _rng_consistency_check.isKinematicallyConsistent()) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -186,7 +180,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
-					PX4_INFO("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
+					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);
@@ -202,18 +196,18 @@ 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()) {
-						PX4_INFO("stopping %s fusion, fusion failing", HGT_SRC_NAME);
+						ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
 						stopRngHgtFusion();
 						stopRngTerrFusion();
 
-					} else if (_rng_consistency_check.isKinematicallyConsistent()) {
+					} else {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
 				}
 
 			} else {
-				PX4_INFO("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
 				stopRngTerrFusion();
 			}
@@ -227,7 +221,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					}
 
 				} else {
-					if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
+					if (aid_src.innovation_rejected) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -240,7 +234,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.
-		PX4_INFO("stopping %s fusion, no data", HGT_SRC_NAME);
+		ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
 		stopRngTerrFusion();
 	}
@@ -256,7 +250,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.sample()->time_us, // sample timestamp
+			      _range_sensor.getSampleAddress()->time_us, // sample timestamp
 			      measurement,                               // observation
 			      measurement_variance,                      // observation variance
 			      getHagl() - measurement,                   // innovation
@@ -274,9 +268,11 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 float Ekf::getRngVar() const
 {
-	const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
-	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
-	return dist_var;
+	return fmaxf(
+		       P(State::pos.idx + 2, State::pos.idx + 2)
+		       + sq(_params.range_noise)
+		       + sq(_params.range_noise_scaler * _range_sensor.getRange()),
+		       0.f);
 }
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)

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

@@ -47,34 +47,10 @@ namespace estimator
 namespace sensor
 {
 
-void SensorRangeFinder::setSample(const rangeSample &sample)
+void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
 {
-	_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;
+	updateSensorToEarthRotation(R_to_earth);
+	updateValidity(current_time_us);
 }
 
 void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
@@ -84,5 +60,112 @@ 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;
+	}
+}
+
 } // namespace sensor
 } // namespace estimator

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

@@ -41,6 +41,8 @@
 #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>
 
@@ -55,62 +57,105 @@ 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
+class SensorRangeFinder : public Sensor
 {
 public:
 	SensorRangeFinder() = default;
-	~SensorRangeFinder() = default;
+	~SensorRangeFinder() override = default;
 
-	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 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 *sample() { return &_sample; }
+	rangeSample *getSampleAddress() { return &_sample; }
 
-	void setPitchOffset(float new_pitch_offset);
+	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 setLimits(float min_distance, float max_distance);
-
-	// void setRange(float rng) { _sample.rng = rng; }
-	// float getRange() const { return _sample.rng; }
+	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 updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
-
-	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
+	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{};
 
-	Parameters _parameters{};
+	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
@@ -127,6 +172,20 @@ 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 RngCtrl : uint8_t {
+enum class RngCtrl : uint8_t {
 	DISABLED    = 0,
 	CONDITIONAL = 1,
 	ENABLED     = 2
@@ -415,19 +415,17 @@ 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 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 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 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 range_kin_consistency_gate{1.0f}; ///< 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]
 
-	float ekf2_rng_pos_x{0.f};
-	float ekf2_rng_pos_y{0.f};
-	float ekf2_rng_pos_z{0.f};
-
+	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -572,6 +570,8 @@ 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,6 +582,8 @@ 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

src/modules/ekf2/EKF/covariance.cpp

@@ -227,10 +227,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (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.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;
 
@@ -383,18 +390,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)
-
-	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

@@ -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)
@@ -762,18 +762,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();
-	void stopRangeTerrainFusion();
-
+	void stopRngHgtFusion();
+	void stopRngTerrFusion();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)

src/modules/ekf2/EKF/estimator_interface.h

@@ -120,6 +120,8 @@ 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/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

@@ -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

src/modules/ekf2/EKF2.cpp

@@ -155,17 +155,18 @@ 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->ekf2_rng_sfe),
+	_param_ekf2_rng_sfe(_params->range_noise_scaler),
 	_param_ekf2_rng_gate(_params->range_innov_gate),
-	_param_ekf2_rng_pitch(_params->ekf2_rng_pitch),
+	_param_ekf2_rng_pitch(_params->rng_sens_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->ekf2_rng_pos_x),
-	_param_ekf2_rng_pos_y(_params->ekf2_rng_pos_y),
-	_param_ekf2_rng_pos_z(_params->ekf2_rng_pos_z),
+	_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)),
 #endif // CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_param_ekf2_ev_delay(_params->ev_delay_ms),
@@ -1600,16 +1601,8 @@ 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;
@@ -1917,12 +1910,14 @@ 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;

src/modules/ekf2/EKF2.hpp

@@ -619,6 +619,7 @@ 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.05
+      default: 0.1
       min: 0.01
       unit: m
       decimal: 2
@@ -93,6 +93,16 @@ 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
@@ -103,7 +113,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
@@ -112,7 +122,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/test/test_EKF_accelerometer.cpp

@@ -192,13 +192,12 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroRange)
 	_sensor_simulator.runSeconds(5);
 
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
 	// AND: an accelerometer with a really large Z bias
 	const float bias = CONSTANTS_ONE_G;
 	_sensor_simulator._imu.setAccelData(Vector3f(0.f, 0.f, -CONSTANTS_ONE_G + bias));
 	_sensor_simulator.runSeconds(2);
-	_sensor_simulator._imu.setAccelClipping(false, false, true);
-	_sensor_simulator.runSeconds(2);
 
 	// THEN: the bad vertical is detected because both sources agree
 	EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -123,6 +123,7 @@ 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);
@@ -178,6 +179,7 @@ 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_EKF_flow.cpp

@@ -171,7 +171,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
 	_sensor_simulator.startRangeFinder();
-	_sensor_simulator.runSeconds(2.0);
+	_sensor_simulator.runSeconds(1.0);
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ -103,7 +103,7 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
 	/* _ekf_wrapper.enableExternalVisionHeightFusion(); */ //TODO: this currently sets the reference to EV
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(1);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -125,6 +125,9 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus();
 	EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f);
 
+	const float terrain = _ekf->getTerrainVertPos();
+	EXPECT_NEAR(terrain, -baro_increment, 1.2f);
+
 	const BiasEstimator::status &ev_status = _ekf->getEvHgtBiasEstimatorStatus();
 	EXPECT_EQ(ev_status.bias, 0.f);
 
@@ -136,6 +139,7 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
 	// AND WHEN: the gps height increases
 	const float gps_increment = 1.f;
@@ -146,6 +150,9 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	EXPECT_EQ(gps_status.bias, _ekf->getGpsHgtBiasEstimatorStatus().bias);
 	// the estimated height follows the GPS height
 	EXPECT_NEAR(_ekf->getPosition()(2), -(baro_increment + gps_increment), 0.3f);
+	// and the range finder bias is adjusted to follow the new reference
+	const float terrain2 = _ekf->getTerrainVertPos();
+	EXPECT_NEAR(terrain2, -(baro_increment + gps_increment), 1.3f);
 }
 
 TEST_F(EkfHeightFusionTest, gpsRef)
@@ -155,7 +162,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(1);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -167,7 +174,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 
 	const BiasEstimator::status &baro_status_initial = _ekf->getBaroBiasEstimatorStatus();
 	const float baro_rel_initial = baro_initial - _sensor_simulator._gps.getData().alt;
-	EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.8f);
+	EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.6f);
 
 	// AND WHEN: the baro data increases
 	const float baro_increment = 5.f;
@@ -208,7 +215,7 @@ TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.disableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(1);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -235,7 +242,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
 
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(0.1);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
 
@@ -244,7 +251,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 
 	_sensor_simulator.stopGps();
-	_sensor_simulator.runSeconds(14);
+	_sensor_simulator.runSeconds(10);
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE);
 
 	_sensor_simulator.stopRangeFinder();
@@ -401,7 +408,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt)
 	reset_logging_checker.capturePostResetState();
 
 	// An origin reset doesn't change the baro bias as it is relative to the height reference (GNSS)
-	EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.4f);
+	EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.3f);
 
 	EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f);
 

src/modules/ekf2/test/test_EKF_terrain.cpp

@@ -207,7 +207,7 @@ TEST_F(EkfTerrainTest, testHeightReset)
 	const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
 	_sensor_simulator._baro.setData(new_baro_height);
 	_sensor_simulator.stopGps(); // prevent from switching to GNSS height
-	_sensor_simulator.runSeconds(100);
+	_sensor_simulator.runSeconds(10);
 
 	// THEN: a height reset occurred and the estimated distance to the ground remains constant
 	reset_logging_checker.capturePostResetState();
@@ -219,36 +219,11 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 {
 	// GIVEN: rng for terrain but not flow
 	_ekf_wrapper.disableFlowFusion();
-	const float rng_height = 16.f;
+	_ekf_wrapper.enableRangeHeightFusion();
 
-	_sensor_simulator.startGps();
-	_ekf->set_min_required_gps_health_time(1e6);
-	_ekf->set_in_air_status(false);
-	_ekf->set_vehicle_at_rest(true);
-	_ekf_wrapper.enableGpsFusion();
-	_sensor_simulator.runSeconds(1.5); // Run to pass the GPS checks
-	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
-
-	const Vector3f simulated_velocity(0.5f, -1.0f, 0.0f);
-
-	// Configure GPS simulator data
-	_sensor_simulator._gps.setVelocity(simulated_velocity);
-	_sensor_simulator._gps.setPositionRateNED(simulated_velocity);
-
-	// Simulate flight above max range finder distance
-	// run for a while to let terrain uncertainty increase
-	_sensor_simulator._rng.setData(30.f, 100);
-	_sensor_simulator._rng.setLimits(0.1f, 20.f);
-	_sensor_simulator.startRangeFinder();
-	_ekf->set_in_air_status(true);
-	_ekf->set_vehicle_at_rest(false);
-	_sensor_simulator.runSeconds(40);
-
-	// quick range finder change to bypass stuck-check
-	_sensor_simulator._rng.setData(rng_height - 1.f, 100);
-	_sensor_simulator.runSeconds(1);
-	_sensor_simulator._rng.setData(rng_height, 100);
-	_sensor_simulator.runSeconds(10);
+	const float rng_height = 18;
+	const float flow_height = 1.f;
+	runFlowAndRngScenario(rng_height, flow_height);
 
 	// THEN: the terrain should reset using rng
 	EXPECT_NEAR(rng_height, _ekf->getHagl(), 1e-3f);
@@ -259,13 +234,13 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 
 	const float corr_terrain_vz = P(State::vel.idx + 2,
 					State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain);
-	EXPECT_TRUE(fabsf(corr_terrain_vz) > 0.6f && fabsf(corr_terrain_vz) < 1.f);
+	EXPECT_NEAR(corr_terrain_vz, 0.6f, 0.03f);
 
 	const float corr_terrain_z = P(State::pos.idx + 2,
 				       State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain);
-	EXPECT_TRUE(fabsf(corr_terrain_z) > 0.8f && fabsf(corr_terrain_z) < 1.f);
+	EXPECT_NEAR(corr_terrain_z, 0.8f, 0.03f);
 
 	const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
 					     State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);
-	EXPECT_TRUE(corr_terrain_abias_z < -0.1f);
+	EXPECT_NEAR(corr_terrain_abias_z, -0.4f, 0.03f);
 }

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -51,6 +51,7 @@ 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
@@ -117,8 +118,9 @@ TEST_F(SensorRangeFinderTest, setRange)
 	sample.time_us = 1e6;
 	sample.quality = 9;
 
-	_range_finder.setSample(sample)
-
+	_range_finder.setRange(sample.rng);
+	_range_finder.setDataReadiness(true);
+	_range_finder.setValidity(true);
 	EXPECT_TRUE(_range_finder.isDataHealthy());
 	EXPECT_TRUE(_range_finder.isHealthy());
 }
@@ -361,7 +363,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.sample()->time_us;
+	uint64_t t_now_us = _range_finder.getSampleAddress()->time_us;
 	rangeSample sample{t_now_us, 1.f, 100};
 	updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
 
@@ -371,7 +373,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// WHEN: the sensor is not blocked by fog anymore
 	sample.rng = 5.f;
-	sample.time_us = _range_finder.sample()->time_us;
+	sample.time_us = _range_finder.getSampleAddress()->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,132 +97,94 @@ void FlightTaskManualAltitude::_scaleSticks()
 
 void FlightTaskManualAltitude::_updateAltitudeLock()
 {
-	// - 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)
+	// Depending on stick inputs and velocity, position is locked.
+	// If not locked, altitude setpoint is set to NAN.
 
-	// 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;
+	// 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));
+				}
+			}
+		}
+
+	}
+
+	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
+		// terrain following
+		_terrainFollowing(apply_brake, stopped);
+
+	} else {
+		// normal mode where height is dependent on local frame
+
+		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);
+
+			} else {
+				_dist_to_ground_lock = NAN;
+			}
+
+		} 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;
+			}
+
+		} else  {
+			// user demands velocity change
 			_position_setpoint(2) = NAN;
-			_dist_to_bottom_lock = NAN;
-			_constraints.lock_dist_bottom = false;
-		}
-
-		return;
-	}
-
-	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;
-		}
-
-	case AltitudeMode::TerrainFollow: {
-			// Terrain following - relative to ground which changes with terrain variation
-			// - distance sensor valid
-			// - No user throttle
-
-			// Cannot perform terrain follow without distance sensor
-			if (PX4_ISFINITE(_dist_to_bottom)) {
-				_terrain_follow_mode();
-
-			} else {
-				_altitude_follow_mode();
-				break;
-			}
-			break;
-		}
-
-	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;
-			}
-		}
-
-	case AltitudeMode::None: {
-			// Nothing to do
-			break;
+			// ensure that maximum altitude is respected
 		}
 	}
-}
-
-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()
@@ -236,7 +198,7 @@ void FlightTaskManualAltitude::_respectMinAltitude()
 
 void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
 {
-	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_bottom_lock)) {
+	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) {
 		// User wants to break and vehicle reached zero velocity. Lock height to ground.
 
 		// lock position
@@ -244,19 +206,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_bottom_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
+		_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
 
-	} else if (apply_brake && PX4_ISFINITE(_dist_to_bottom_lock)) {
+	} else if (apply_brake && PX4_ISFINITE(_dist_to_ground_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_bottom_lock - _dist_to_bottom;
+		const float delta_distance_to_ground = _dist_to_ground_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_bottom_lock = _position_setpoint(2) = NAN;
+		_dist_to_ground_lock = _position_setpoint(2) = NAN;
 	}
 }
 
@@ -307,8 +269,7 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
 
 void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl)
 {
-	PX4_INFO("_ekfResetHandlerHagl");
-	_dist_to_bottom_lock = NAN;
+	_dist_to_ground_lock = NAN;
 }
 
 void FlightTaskManualAltitude::_updateSetpoints()
@@ -317,7 +278,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,13 +45,6 @@
 #include "StickTiltXY.hpp"
 #include <uORB/Subscription.hpp>
 
-enum class AltitudeMode : int {
-	AltitudeFollow,
-	TerrainFollow,
-	TerrainHold,
-	None,
-};
-
 class FlightTaskManualAltitude : public FlightTask
 {
 public:
@@ -83,8 +76,7 @@ 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 */
-	AltitudeMode _current_mode = AltitudeMode::AltitudeFollow;
+	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
 
 	float _velocity_constraint_up{INFINITY};
 	float _velocity_constraint_down{INFINITY};
@@ -93,20 +85,15 @@ 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,
-					(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
+					(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 */
 				       )
 private:
-
-	void _terrain_hold_mode();
-	void _terrain_follow_mode();
-	void _altitude_follow_mode();
 	/**
 	 * Terrain following.
 	 * During terrain following, the position setpoint is adjusted
@@ -132,6 +119,8 @@ private:
 	 */
 	void _respectGroundSlowdown();
 
+	void setGearAccordingToSwitch();
+
 	bool _updateYawCorrection();
 
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
@@ -143,7 +132,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_bottom_lock.
+	 * _dist_to_ground_lock.
 	 */
-	float _dist_to_bottom_lock = NAN;
+	float _dist_to_ground_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 (_current_mode != AltitudeMode::TerrainHold) {
+	if (!_terrain_hold) {
 		if (_terrain_hold_previous) {
 			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
 			_smoothing.setCurrentPosition(_position(2));
@@ -116,18 +116,5 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 		_position_setpoint(2) = _smoothing.getCurrentPosition();
 	}
 
-	_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);
+	_terrain_hold_previous = _terrain_hold;
 }

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

@@ -42,9 +42,6 @@
 #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:
@@ -65,7 +62,12 @@ 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

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