handle low height condition properly

Signed-off-by: RomanBapst <[email protected]>

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

@@ -144,7 +144,8 @@ void FwLateralLongitudinalControl::Run()
 		_lateral_control_limits_sub.update();
 
 		_tecs.set_speed_weight(_long_control_limits_sub.get().speed_weight);
-		updateTECSAltitudeTimeConstant(checkLowHeightConditions(), control_interval);
+		updateTECSAltitudeTimeConstant(checkLowHeightConditions()
+					       || _long_control_limits_sub.get().enforce_low_height_condition, control_interval);
 		_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());
 
 		if (_vehicle_air_data_sub.updated()) {

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -1082,12 +1082,14 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 	const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _directional_guidance.switchDistance(
 					     500);
 
+	bool enforce_low_height{false};
+
 	if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid
 	    && close_to_circle && _param_fw_lnd_earlycfg.get()) {
 		// 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.
-
+		enforce_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();
@@ -1124,6 +1126,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 			lateral_limits.lateral_accel_max = 0.0f;
 		}
 
+		enforce_low_height = true;
 	}
 
 	_lateral_ctrl_limits_pub.publish(lateral_limits);
@@ -1141,6 +1144,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()};
 	setDefaultLongControlLimits(longitudinal_control_limits);
+	longitudinal_control_limits.enforce_low_height_condition = enforce_low_height;
 
 	if (pos_sp_curr.gliding_enabled) {
 		longitudinal_control_limits.throttle_min = 0.f;
@@ -1535,6 +1539,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 
 	longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()};
 	setDefaultLongControlLimits(longitudinal_control_limits);
+	longitudinal_control_limits.enforce_low_height_condition = true;
 
 	if (airspeed_land < adjusted_min_airspeed) {
 		// adjust underspeed detection bounds for landing airspeed