fw_tecs: Support tighter altitude tracking during low-height flight (#23519)

* fw_tecs: Support tighter altitude tracking during low-height flight. Added FW_T_THR_LOW_HGT defining low-height flight threshold

* tecs: Applied smoothed-out altitude TC transition to landings

* fw_tecs: modified tighter altitude control for low-height implementation

* addressed PR comments

* addressed PR comments

---------

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

msg/TecsStatus.msg

@@ -2,6 +2,7 @@ uint64 timestamp		# time since system start (microseconds)
 
 float32 altitude_sp			# Altitude setpoint AMSL [m]
 float32 altitude_reference		# Altitude setpoint reference AMSL [m]
+float32 altitude_time_constant		# Time constant of the altitude tracker [s]
 float32 height_rate_reference		# Height rate setpoint reference [m/s]
 float32 height_rate_direct		# Direct height rate setpoint from velocity reference generator [m/s]
 float32 height_rate_setpoint		# Height rate setpoint [m/s]

src/lib/tecs/TECS.hpp

@@ -651,6 +651,14 @@ public:
 	float get_pitch_setpoint() {return _control.getPitchSetpoint();}
 	float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
 
+	/**
+	 * Returns the altitude tracking time constant
+	 */
+	float get_altitude_error_time_constant() const
+	{
+		return 1.0f / math::max(_control_param.altitude_error_gain, 0.01f);
+	}
+
 	uint64_t timestamp() { return _update_timestamp; }
 	float get_underspeed_ratio() { return _control.getRatioUndersped(); }
 

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -80,6 +80,9 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
 	_roll_slew_rate.setForcedValue(0.f);
 
+	_tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE);
+	_tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get());
+
 }
 
 FixedwingPositionControl::~FixedwingPositionControl()
@@ -419,6 +422,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
 
 	tecs_status.altitude_sp = alt_sp;
 	tecs_status.altitude_reference = debug_output.altitude_reference;
+	tecs_status.altitude_time_constant = _tecs.get_altitude_error_time_constant();
 	tecs_status.height_rate_reference = debug_output.height_rate_reference;
 	tecs_status.height_rate_direct = debug_output.height_rate_direct;
 	tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
@@ -940,8 +944,9 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 void
 FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
 {
-	// only control altitude and airspeed ("fixed-bank loiter")
+	const bool is_low_height = checkLowHeightConditions();
 
+	// only control altitude and airspeed ("fixed-bank loiter")
 	tecs_update_pitch_throttle(control_interval,
 				   _current_altitude,
 				   _performance_model.getCalibratedTrimAirspeed(),
@@ -950,7 +955,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 				   _param_fw_thr_min.get(),
 				   _param_fw_thr_max.get(),
 				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+				   _param_climbrate_target.get(),
+				   is_low_height);
 
 	const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
 	const float yaw_body = 0.f;
@@ -976,6 +982,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 	const float descend_rate = -0.5f;
 	const bool disable_underspeed_handling = false;
 
+	const bool is_low_height = checkLowHeightConditions();
+
 	tecs_update_pitch_throttle(control_interval,
 				   _current_altitude,
 				   _performance_model.getCalibratedTrimAirspeed(),
@@ -985,6 +993,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 				   _param_fw_thr_max.get(),
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get(),
+				   is_low_height,
 				   disable_underspeed_handling,
 				   descend_rate);
 
@@ -1125,6 +1134,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 
 	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
+	const bool is_low_height = checkLowHeightConditions();
+
 	tecs_update_pitch_throttle(control_interval,
 				   position_sp_alt,
 				   target_airspeed,
@@ -1133,7 +1144,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+				   _param_climbrate_target.get(),
+				   is_low_height);
+
 	const float pitch_body = get_tecs_pitch();
 	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
 	attitude_setpoint.copyTo(_att_sp.q_d);
@@ -1178,6 +1191,8 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	float yaw_body = _yaw;
 	const bool disable_underspeed_handling = false;
 
+	const bool is_low_height = checkLowHeightConditions();
+
 	tecs_update_pitch_throttle(control_interval,
 				   position_sp_alt,
 				   target_airspeed,
@@ -1187,6 +1202,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get(),
+				   is_low_height,
 				   disable_underspeed_handling,
 				   pos_sp_curr.vz);
 	const float pitch_body = get_tecs_pitch();
@@ -1200,6 +1216,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
 		const position_setpoint_s &pos_sp_next)
 {
+
 	Vector2d curr_wp{0, 0};
 	Vector2d prev_wp{0, 0};
 
@@ -1248,6 +1265,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 	Vector2f curr_wp_local{_global_local_proj_ref.project(curr_wp(0), curr_wp(1))};
 	Vector2f vehicle_to_loiter_center{curr_wp_local - curr_pos_local};
 
+	bool is_low_height = checkLowHeightConditions();
+
 	const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _npfg.switchDistance(500);
 
 	if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid
@@ -1255,7 +1274,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
 		// landing airspeed and potentially tighter altitude control) already such that we don't
 		// have to do this switch (which can cause significant altitude errors) close to the ground.
-		_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
+
+		// Just before landing, enforce low-height flight conditions for tighter altitude control
+		is_low_height = true;
+
 		airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() :
 			      _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
 		_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
@@ -1289,7 +1311,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 			roll_body = 0.0f;
 		}
 
-		_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
+		is_low_height = true; // In low-height flight, TECS will control altitude tighter
 	}
 
 	tecs_update_pitch_throttle(control_interval,
@@ -1300,7 +1322,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+				   _param_climbrate_target.get(),
+				   is_low_height);
 
 	const float pitch_body = get_tecs_pitch();
 
@@ -1350,6 +1373,8 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
 		tecs_fw_thr_max = _param_fw_thr_max.get();
 	}
 
+	const bool is_low_height = checkLowHeightConditions();
+
 	tecs_update_pitch_throttle(control_interval,
 				   pos_sp_curr.alt,
 				   target_airspeed,
@@ -1358,7 +1383,8 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+				   _param_climbrate_target.get(),
+				   is_low_height);
 
 	// Yaw
 	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
@@ -1388,7 +1414,6 @@ void
 FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
 {
-
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
 
@@ -1425,6 +1450,8 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 
 	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
+	const bool is_low_height = checkLowHeightConditions();
+
 	tecs_update_pitch_throttle(control_interval,
 				   pos_sp_curr.alt,
 				   target_airspeed,
@@ -1433,7 +1460,8 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+				   _param_climbrate_target.get(),
+				   is_low_height);
 
 	_att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max);
 	const float pitch_body = get_tecs_pitch();
@@ -1471,6 +1499,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		adjusted_min_airspeed = takeoff_airspeed;
 	}
 
+	const bool is_low_height = checkLowHeightConditions();
+
 	if (_runway_takeoff.runwayTakeoffEnabled()) {
 		if (!_runway_takeoff.isInitialized()) {
 			_runway_takeoff.init(now, _yaw, global_position);
@@ -1559,6 +1589,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 					   _param_fw_thr_max.get(),
 					   _param_sinkrate_target.get(),
 					   _performance_model.getMaximumClimbRate(_air_density),
+					   is_low_height,
 					   disable_underspeed_handling);
 
 		_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
@@ -1646,6 +1677,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 						   max_takeoff_throttle,
 						   _param_sinkrate_target.get(),
 						   _performance_model.getMaximumClimbRate(_air_density),
+						   is_low_height,
 						   disable_underspeed_handling);
 
 			if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
@@ -1709,8 +1741,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 
 	float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed,
 				ground_speed);
-	// Enable tighter altitude control for landings
-	_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
+
 
 	// now handle position
 	const Vector2f local_position{_local_pos.x, _local_pos.y};
@@ -1758,6 +1789,9 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 	// flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude
 	const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get());
 
+	// During landing, enforce low-height flight conditions for tighter altitude control
+	const bool is_low_height = true;
+
 	// the terrain estimate (if enabled) is always used to determine the flaring altitude
 	if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) {
 		// flare and land with minimal speed
@@ -1834,6 +1868,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 					   throttle_max,
 					   _param_sinkrate_target.get(),
 					   _param_climbrate_target.get(),
+					   is_low_height,
 					   disable_underspeed_handling,
 					   height_rate_setpoint);
 
@@ -1894,7 +1929,8 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 					   _param_fw_thr_min.get(),
 					   _param_fw_thr_max.get(),
 					   desired_max_sinkrate,
-					   _param_climbrate_target.get());
+					   _param_climbrate_target.get(),
+					   is_low_height);
 
 		/* set the attitude and throttle commands */
 
@@ -1947,8 +1983,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 	float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed,
 				ground_speed);
 
-	// Enable tighter altitude control for landings
-	_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 
 	const Vector2f local_position{_local_pos.x, _local_pos.y};
 	Vector2f local_landing_orbit_center = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
@@ -1972,6 +2006,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 		loiter_radius = _param_nav_loiter_rad.get();
 	}
 
+	// During landing, enforce low-height flight conditions for tighter altitude control
+	const bool is_low_height = true;
+
 	// the terrain estimate (if enabled) is always used to determine the flaring altitude
 	float roll_body;
 	float yaw_body;
@@ -2051,6 +2088,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 					   throttle_max,
 					   _param_sinkrate_target.get(),
 					   _param_climbrate_target.get(),
+					   is_low_height,
 					   disable_underspeed_handling,
 					   height_rate_setpoint);
 
@@ -2108,6 +2146,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 					   _param_fw_thr_max.get(),
 					   desired_max_sinkrate,
 					   _param_climbrate_target.get(),
+					   is_low_height,
 					   disable_underspeed_handling,
 					   -glide_slope_sink_rate); // heightrate = -sinkrate
 
@@ -2165,6 +2204,9 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 		throttle_max = 0.0f;
 	}
 
+
+	const bool is_low_height = checkLowHeightConditions();
+
 	const bool disable_underspeed_handling = false;
 
 	tecs_update_pitch_throttle(control_interval,
@@ -2176,6 +2218,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 				   throttle_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get(),
+				   is_low_height,
 				   disable_underspeed_handling,
 				   height_rate_sp);
 
@@ -2267,8 +2310,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 		}
 	}
 
+	const bool is_low_height = checkLowHeightConditions();
+
 	const bool disable_underspeed_handling = false;
 
+
 	tecs_update_pitch_throttle(control_interval,
 				   _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude.
 				   calibrated_airspeed_sp,
@@ -2278,6 +2324,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 				   throttle_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get(),
+				   is_low_height,
 				   disable_underspeed_handling,
 				   height_rate_sp);
 
@@ -2302,6 +2349,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
 		const position_setpoint_s &pos_sp_curr)
 {
+	const bool is_low_height = checkLowHeightConditions();
+
 	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
 				_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed);
 
@@ -2332,7 +2381,8 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
 				   _param_fw_thr_min.get(),
 				   _param_fw_thr_max.get(),
 				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+				   _param_climbrate_target.get(),
+				   is_low_height);
 
 	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
 	const float pitch_body = get_tecs_pitch();
@@ -2555,7 +2605,6 @@ FixedwingPositionControl::Run()
 
 		// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
 		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-		_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
 
 		// restore lateral-directional guidance parameters (changed in takeoff mode)
 		_npfg.setPeriod(_param_npfg_period.get());
@@ -2687,6 +2736,8 @@ FixedwingPositionControl::Run()
 			_roll_slew_rate.setForcedValue(_roll);
 		}
 
+
+
 		// Publish estimate of level flight
 		_flight_phase_estimation_pub.get().timestamp = hrt_absolute_time();
 		_flight_phase_estimation_pub.update();
@@ -2755,7 +2806,7 @@ FixedwingPositionControl::reset_landing_state()
 void
 FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
 		float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max,
-		const float desired_max_sinkrate, const float desired_max_climbrate,
+		const float desired_max_sinkrate, const float desired_max_climbrate, const bool is_low_height,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
 	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
@@ -2775,6 +2826,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	/* No underspeed protection in landing mode */
 	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
 
+	updateTECSAltitudeTimeConstant(is_low_height, control_interval);
+
 	// HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases
 	// when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0.
 	const float airspeed_rate_estimate = 0.f;
@@ -2897,6 +2950,27 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
 	}
 }
 
+bool FixedwingPositionControl::checkLowHeightConditions()
+{
+	// Are conditions for low-height
+	return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid
+	       && _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get();
+}
+
+void FixedwingPositionControl::updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt)
+{
+	// Target time constant for the TECS altitude tracker
+	float alt_tracking_tc = _param_fw_t_h_error_tc.get();
+
+	if (is_low_height) {
+		// If low-height conditions satisfied, compute target time constant for altitude tracking
+		alt_tracking_tc *= _param_fw_thrtc_sc.get();
+	}
+
+	_tecs_alt_time_const_slew_rate.update(alt_tracking_tc, dt);
+	_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());
+}
+
 Vector2f
 FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
 {

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -175,6 +175,8 @@ static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;
 // [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
 static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;
 
+// [s] slew rate with which we change altitude time constant
+static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f;
 
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
@@ -401,6 +403,9 @@ private:
 
 	bool _tecs_is_running{false};
 
+	// Smooths changes in the altitude tracking error time constant value
+	SlewRate<float> _tecs_alt_time_const_slew_rate;
+
 	// VTOL / TRANSITION
 	bool _is_vtol_tailsitter{false};
 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
@@ -761,12 +766,13 @@ private:
 	 * @param throttle_max Maximum throttle command [0,1]
 	 * @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s]
 	 * @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s]
+	 * @param is_low_height Define whether we are in low-height flight for tighter altitude tracking
 	 * @param disable_underspeed_detection True if underspeed detection should be disabled
 	 * @param hgt_rate_sp Height rate setpoint [m/s]
 	 */
 	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad,
 					float pitch_max_rad, float throttle_min, float throttle_max,
-					const float desired_max_sink_rate, const float desired_max_climb_rate,
+					const float desired_max_sink_rate, const float desired_max_climb_rate, const bool is_low_height,
 					bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
 
 	/**
@@ -825,6 +831,21 @@ private:
 	void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
 				   const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point);
 
+	/*
+	 * Checks if the vehicle satisfies conditions for low-height flight
+	 *
+	 * @return bool True if conditions are satisfied, false otherwise
+	 */
+	bool checkLowHeightConditions();
+
+	/*
+	 * Updates TECS altitude time constant according to the is_low_height parameter.
+	 *
+	 * @param is_low_height Boolean flag defining whether we are in low-height flight
+	 * @param dt Update time step [s]
+	 */
+	void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);
+
 	/*
 	 * Waypoint handling logic following closely to the ECL_L1_Pos_Controller
 	 * method of the same name. Takes two waypoints, steering the vehicle to track
@@ -953,6 +974,7 @@ private:
 		(ParamFloat<px4::params::FW_LND_FL_PMIN>) _param_fw_lnd_fl_pmin,
 		(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
 		(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
+		(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt,
 		(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
 		(ParamInt<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
 

src/modules/fw_pos_control/fw_path_navigation_params.c

@@ -412,10 +412,9 @@ PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f);
 PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f);
 
 /**
- * Altitude time constant factor for landing
+ * Altitude time constant factor for landing and low-height flight
  *
- * During landing, the TECS altitude time constant (FW_T_ALT_TC)
- * is multiplied by this value.
+ * The TECS altitude time constant (FW_T_ALT_TC) is multiplied by this value.
  *
  * @unit
  * @min 0.2
@@ -426,6 +425,24 @@ PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f);
  */
 PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
 
+/**
+ * Low-height threshold for tighter altitude tracking
+ *
+ * Defines the height (distance to bottom) threshold below which tighter altitude
+ * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly
+ * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC
+ * to FW_LND_THRTC_SC*FW_T_ALT_TC.
+ *
+ * If equal to -1, low-height traking is disabled.
+ *
+ * @unit m
+ * @min -1
+ * @decimal 0
+ * @increment 1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f);
+
 /*
  * TECS parameters
  *