diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index e07888adb3..751ef2cb82 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -226,6 +226,7 @@ set(msg_files
 	VehicleImu.msg
 	VehicleImuStatus.msg
 	VehicleLocalPositionSetpoint.msg
+	TaskLocalPositionSetpoint.msg
 	VehicleMagnetometer.msg
 	VehicleOpticalFlow.msg
 	VehicleOpticalFlowVel.msg
diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg
index 3de2bfd722..921063db13 100644
--- a/msg/EstimatorStatus.msg
+++ b/msg/EstimatorStatus.msg
@@ -39,14 +39,12 @@ uint8 CS_FIXED_WING = 17	# 17 - true when thought to be operating as a fixed win
 uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetometer has been declared faulty and is no longer being used
 uint8 CS_ASPD = 19		# 19 - true when airspeed measurements are being fused
 uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect induced static pressure rise is active
-uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected
 uint8 CS_GPS_YAW = 22		# 22 - true when yaw (not ground course) data from a GPS receiver is being fused
 uint8 CS_MAG_ALIGNED = 23	# 23 - true when the in-flight mag field alignment has been completed
 uint8 CS_EV_VEL = 24		# 24 - true when local frame velocity data fusion from external vision measurements is intended
 uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measurement for the magnetometer Z component
 uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
 uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
-uint8 CS_RNG_FAULT = 28		# 28 - true when the range finder has been declared faulty and is no longer being used
 uint8 CS_GNSS_VEL = 44		# 44 - true if GNSS velocity measurements are being fused
 
 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults
diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg
index 7fed72cc28..7fae5312a8 100644
--- a/msg/EstimatorStatusFlags.msg
+++ b/msg/EstimatorStatusFlags.msg
@@ -25,14 +25,12 @@ bool cs_fixed_wing            # 17 - true when the vehicle is operating as a fix
 bool cs_mag_fault             # 18 - true when the magnetometer has been declared faulty and is no longer being used
 bool cs_fuse_aspd             # 19 - true when airspeed measurements are being fused
 bool cs_gnd_effect            # 20 - true when protection from ground effect induced static pressure rise is active
-bool cs_rng_stuck             # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 bool cs_gnss_yaw              # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed
 bool cs_ev_vel                # 24 - true when local frame velocity data fusion from external vision measurements is intended
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gnss_yaw_fault        # 27 - true when the GNSS heading has been declared faulty and is no longer being used
-bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
@@ -49,7 +47,6 @@ bool cs_valid_fake_pos          # 41 - true if a valid constant position is bein
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
 bool cs_gnss_vel                # 44 - true if GNSS velocity measurement fusion is intended
-bool cs_rng_kin_unknown      	# 45 - true when the range finder kinematic consistency check is not running
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes
diff --git a/msg/PositionSetpoint.msg b/msg/PositionSetpoint.msg
index 51bfaa3da0..e7cb29d17f 100644
--- a/msg/PositionSetpoint.msg
+++ b/msg/PositionSetpoint.msg
@@ -36,3 +36,5 @@ float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
+
+# TOPICS position_setpoint task_position_setpoint
diff --git a/msg/TaskLocalPositionSetpoint.msg b/msg/TaskLocalPositionSetpoint.msg
new file mode 100644
index 0000000000..0093d52d77
--- /dev/null
+++ b/msg/TaskLocalPositionSetpoint.msg
@@ -0,0 +1,19 @@
+# Local position setpoint in NED frame
+# Telemetry of PID position controller to monitor tracking.
+# NaN means the state was not controlled
+
+uint64 timestamp	# time since system start (microseconds)
+
+float32 x		# in meters NED
+float32 y		# in meters NED
+float32 z		# in meters NED
+
+float32 vx		# in meters/sec
+float32 vy		# in meters/sec
+float32 vz		# in meters/sec
+
+float32[3] acceleration # in meters/sec^2
+float32[3] thrust	# normalized thrust vector in NED
+
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec
diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp
index 4610f7d615..dcaf984bb2 100644
--- a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp
@@ -47,97 +47,262 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		return;
 	}
 
-	bool sample_valid = false;
-	bool kinematically_consistent = false;
-
 	// Pop rangefinder measurement from buffer of samples into active sample
 	sensor::rangeSample sample = {};
-	bool sample_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, &sample);
-	if (sample_ready) {
-		// 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);
-
-		// Gate sample consumption on these checks
-		// - Quality is OK
-		bool quality_ok = sample.quality > 0;
-		// - Tilt is OK
-		bool tilt_ok = _range_sensor.isTiltOk();
-		// - Value is IN range
-		bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal();
-		// - Not stuck value
-		// - Not fog detected
-
-		if (quality_ok && tilt_ok && range_ok) {
-
-			// 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);
-
-			// 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);
-
-			kinematically_consistent = _rng_consistency_check.isKinematicallyConsistent();
-
-			if (kinematically_consistent) {
-				sample_valid = true;
+	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) {
+				ECL_INFO("stopping RNG fusion, sensor timed out");
+				stopRangeAltitudeFusion();
+				stopRangeTerrainFusion();
 			}
-
-			// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
-			updateRangeHagl(_aid_src_rng_hgt);
 		}
-	}
-
-	// Check if rangefinder has timed out
-	if (_range_sensor.timedOut(imu_sample.time_us)) {
-		// TODO: timeout value?
-		// TODO: handle timeout
-	}
-
-	// If the sample is valid conditionally fuse into Altitude and Terrain estimates
-	if (sample_valid) {
-
-	} else {
-
-		// Sample is invalid
-		// - Timeout
-		// - Quality
-		// - Tilt
-		// - Range
-		// - KinematicConsistency
-		// - RangeAidChecks...
-		//   - Horizontal Velocity
-		//   - aid_src_rng_hgt.test_ratio
-
-		// If sample is invalid
-		// - Don't fuse
-		// -
-
 		return;
 	}
 
+	// 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);
 
-	// Conditionally enable/disable rangefinder fusion for Altitude Estimate and Terrain Estimate
+	// Update sensor to earth rotation
+	_range_sensor.updateSensorToEarthRotation(_R_to_earth);
+
+	// Gate sample consumption on these checks
+	bool quality_ok = sample.quality > 0;
+	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) {
+				ECL_INFO("stopping RNG fusion, sensor data invalid");
+				stopRangeAltitudeFusion();
+				stopRangeTerrainFusion();
+			}
+		}
+		return;
+	}
+
+	// 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);
+
+	// 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)) {
+		ECL_INFO("INFINIFY OBSERVATION INVALID");
+	}
+
+	if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
+		fuseRangeAsHeightSource();
+	} else {
+		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
+			_control_status.flags.rng_hgt = true;
+
+			// TODO: review for correctness
+			if (innovation_rejected && kinematically_consistent) {
+				// Reset aid source
+				ECL_INFO("range alt fusion, resetting aid source");
+				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+			}
+		}
+
+		// Fuse
+		fuse_measurement = true;
+
+	} else {
+		// Stop fusion
+		ECL_INFO("stopping RNG Altitude fusion:");
+		if (!range_aid_conditions_passed) {
+			ECL_INFO("range aid conditions failed");
+		}
+		if (!kinematically_consistent) {
+			ECL_INFO("kinematically inconsistent");
+		}
+		stopRangeAltitudeFusion();
+	}
+
+	// Fuse Range into Terrain if:
+	// - kinematically consistent
+	if (kinematically_consistent) {
+
+		// Start fusion
+		if (!_control_status.flags.rng_terrain) {
+			// Fusion init logic
+			_control_status.flags.rng_terrain = true;
+
+			if (!optical_flow_for_terrain && innovation_rejected
+				&& kinematically_consistent) {
+				// Reset aid source and then reset terrain estimate
+				ECL_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
+		ECL_INFO("stopping RNG Terrain fusion, kinematically inconsistent");
+		stopRangeTerrainFusion();
+	}
+
+	if (fuse_measurement) {
+		fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
+	}
+}
+
+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)
@@ -172,3 +337,16 @@ float Ekf::getRngVar() const
 	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
 	return dist_var;
 }
+
+void Ekf::stopRangeAltitudeFusion()
+{
+	_control_status.flags.rng_hgt = false;
+	if (_height_sensor_ref == HeightSensor::RANGE) {
+		_height_sensor_ref = HeightSensor::UNKNOWN;
+	}
+}
+
+void Ekf::stopRangeTerrainFusion()
+{
+	_control_status.flags.rng_terrain = false;
+}
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index 6d275d588f..3831a3ecdf 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -148,7 +148,7 @@ enum class GnssCtrl : uint8_t {
 	YAW  = (1 << 3)
 };
 
-enum class RngCtrl : uint8_t {
+enum RngCtrl : uint8_t {
 	DISABLED    = 0,
 	CONDITIONAL = 1,
 	ENABLED     = 2
@@ -572,8 +572,6 @@ uint64_t mag_fault               :
 		uint64_t fuse_aspd               : 1; ///< 19 - true when airspeed measurements are being fused
 uint64_t gnd_effect              :
 		1; ///< 20 - true when protection from ground effect induced static pressure rise is active
-uint64_t rng_stuck               :
-		1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 uint64_t gnss_yaw                 :
 		1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 		uint64_t mag_aligned_in_flight   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
@@ -584,8 +582,6 @@ uint64_t synthetic_mag_z         :
 		uint64_t vehicle_at_rest         : 1; ///< 26 - true when the vehicle is at rest
 uint64_t gnss_yaw_fault           :
 		1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
-uint64_t rng_fault               :
-		1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
 uint64_t inertial_dead_reckoning :
 		1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 		uint64_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
@@ -606,7 +602,6 @@ uint64_t mag_heading_consistent  :
 		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
 		uint64_t baro_fault              : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
 		uint64_t gnss_vel                : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
-		uint64_t rng_kin_unknown	 : 1; ///< 45 - true when the range finder kinematic consistency check is not running
 	} flags;
 	uint64_t value;
 };
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 602b94bdfd..40ba2a633e 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -762,9 +762,18 @@ private:
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
 	void controlRangeHaglFusion(const imuSample &imu_delayed);
+
+
+	void fuseRangeAsHeightSource();
+	void fuseRangeAsHeightAiding();
+
 	bool isConditionalRangeAidSuitable();
-	void stopRngHgtFusion();
-	void stopRngTerrFusion();
+	bool rangeAidConditionsPassed();
+
+
+	void stopRangeAltitudeFusion();
+	void stopRangeTerrainFusion();
+
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp
index d005022289..6a8ae625af 100644
--- a/src/modules/ekf2/EKF/terrain_control.cpp
+++ b/src/modules/ekf2/EKF/terrain_control.cpp
@@ -54,7 +54,6 @@ void Ekf::controlTerrainFakeFusion()
 	// If we are on ground, store the local position and time to use as a reference
 	if (!_control_status.flags.in_air) {
 		_last_on_ground_posD = -_gpos.altitude();
-		_control_status.flags.rng_fault = false;
 
 	} else if (!_control_status_prev.flags.in_air) {
 		// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index fe5fadce5e..bbe4038831 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -1917,14 +1917,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_mag_fault             = _ekf.control_status_flags().mag_fault;
 		status_flags.cs_fuse_aspd             = _ekf.control_status_flags().fuse_aspd;
 		status_flags.cs_gnd_effect            = _ekf.control_status_flags().gnd_effect;
-		status_flags.cs_rng_stuck             = _ekf.control_status_flags().rng_stuck;
 		status_flags.cs_gnss_yaw               = _ekf.control_status_flags().gnss_yaw;
 		status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
 		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
 		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
 		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
 		status_flags.cs_gnss_yaw_fault         = _ekf.control_status_flags().gnss_yaw_fault;
-		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
 		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
@@ -1941,7 +1939,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_constant_pos        = _ekf.control_status_flags().constant_pos;
 		status_flags.cs_baro_fault	    = _ekf.control_status_flags().baro_fault;
 		status_flags.cs_gnss_vel            = _ekf.control_status_flags().gnss_vel;
-		status_flags.cs_rng_kin_unknown     = _ekf.control_status_flags().rng_kin_unknown;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp
index 396d50ff76..6f24b80fc6 100644
--- a/src/modules/ekf2/test/test_EKF_basics.cpp
+++ b/src/modules/ekf2/test/test_EKF_basics.cpp
@@ -123,7 +123,6 @@ TEST_F(EkfBasicsTest, initialControlMode)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
@@ -179,7 +178,6 @@ TEST_F(EkfBasicsTest, gpsFusion)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index 1eaba10692..b01432e836 100644
--- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -173,7 +173,7 @@ void FlightTaskManualAltitude::_terrain_hold_mode()
 		if (_current_mode != AltitudeMode::AltitudeFollow) {
 			_current_mode = AltitudeMode::AltitudeFollow;
 
-			// PX4_INFO("Locking to Z estimate");
+			PX4_INFO("Locking to Z estimate");
 			_position_setpoint(2) = _position(2);
 			_dist_to_bottom_lock = NAN;
 			_constraints.lock_dist_bottom = false;
@@ -186,6 +186,7 @@ void FlightTaskManualAltitude::_terrain_hold_mode()
 		// Set velocity setpoint to 0, switch into TerrainHold
 		_current_mode = AltitudeMode::TerrainHold;
 		_velocity_setpoint(2) = 0.f;
+		PX4_INFO("setting terrain hold");
 		return;
 	}
 
@@ -195,7 +196,7 @@ void FlightTaskManualAltitude::_terrain_hold_mode()
 			_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);
+			PX4_INFO("Locking distance to %f", (double)_dist_to_bottom);
 		}
 
 		return;
diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
index cbb4e7c234..afdcde1b79 100644
--- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
+++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
@@ -117,4 +117,17 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 	}
 
 	_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);
 }
diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
index 5ce710b812..6fafe628a0 100644
--- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
+++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
@@ -42,6 +42,9 @@
 #include "FlightTaskManualAltitude.hpp"
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
+#include <uORB/Publication.hpp>
+#include <uORB/topics/task_local_position_setpoint.h>
+
 class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 {
 public:
@@ -63,5 +66,6 @@ protected:
 	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
 
 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 */
 };
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index 1f7ab5cdac..8ef6f0610c 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -142,6 +142,8 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_local_position", 100);
 	add_topic("vehicle_local_position_setpoint", 100);
+	// DEBUG
+	add_topic("task_local_position_setpoint");
 	add_topic("vehicle_magnetometer", 200);
 	add_topic("vehicle_rates_setpoint", 20);
 	add_topic("vehicle_roi", 1000);