tecs: Applied smoothed-out altitude TC transition to landings

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -1240,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();
@@ -1274,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,
@@ -1661,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};
@@ -1889,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);
@@ -2468,10 +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());
-		updateAltitudeTimeConstant(control_interval);
-		_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());
 
 		// restore lateral-directional guidance parameters (changed in takeoff mode)
 		_npfg.setPeriod(_param_npfg_period.get());
@@ -2599,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();
@@ -2687,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;
@@ -2809,21 +2816,25 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
 	}
 }
 
-void FixedwingPositionControl::updateAltitudeTimeConstant(float dt)
+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();
 
-	// Are conditions for low-height altitude tracking satisfied?
-	const bool is_low_height = _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();
-
-	if (is_low_height) {
+	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

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -289,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
@@ -830,10 +832,20 @@ 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);
 
-	/**
-	 * @brief Updates the value of the TECS altitude tracking time constant.
-	*/
-	void updateAltitudeTimeConstant(float dt);
+	/*
+	 * 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