fw_tecs: Support tighter altitude tracking during low-height flight

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

@@ -647,6 +647,14 @@ class TECS
 	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()
@@ -420,6 +423,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;
@@ -1236,7 +1240,9 @@ 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());
+
+		_is_low_height = true; // In low-height flight, TECS will control altitude tighter
+
 		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();
@@ -1270,7 +1276,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 			_att_sp.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,
@@ -1657,8 +1663,8 @@ 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());
+
+	_is_low_height = true; // In low-height flight, TECS will control altitude tighter
 
 	// now handle position
 	const Vector2f local_position{_local_pos.x, _local_pos.y};
@@ -1885,8 +1891,7 @@ 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());
+	_is_low_height = true; // In low-height flight, TECS will control altitude tighter
 
 	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);
@@ -2464,9 +2469,11 @@ FixedwingPositionControl::Run()
 
 		update_in_air_states(_local_pos.timestamp);
 
+		// check if generic low-height flight conditions are satisfied
+		_is_low_height = checkLowHeightConditions();
+
 		// 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());
@@ -2594,6 +2601,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();
@@ -2682,6 +2691,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	/* No underspeed protection in landing mode */
 	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
 
+	/* Update altitude time constant */
+	updateTECSAltitudeTimeConstant(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;
@@ -2804,6 +2816,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 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
@@ -287,6 +289,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	bool _landed{true};
 
+	bool _is_low_height{false};
+
 	// MANUAL MODES
 
 	// indicates whether we have completed a manual takeoff in a position control mode
@@ -401,6 +405,9 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	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};
@@ -825,6 +832,21 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	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 control flag.
+	 * For low-height flight conditions,
+	 *
+	 * @param dt Update time step [s]
+	 */
+	void updateTECSAltitudeTimeConstant(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 +975,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 		(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,7 +412,7 @@ 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.
@@ -426,6 +426,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
+ * 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
  *