FW Pos C: clearly define FW_AIRSPD_MIN as stall+margin, and automatically increase f(load_factor)

Previously the minimum airspeed setpoint was adjusted to the load_factor compensated
stall speed, which, when the stall speed was set without margin, gave the controller
no room for error (the vehicle would stall if the controller has even a small airspeed
error).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -425,10 +425,10 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 		}
 	}
 
-	float load_factor = 1.0f;
+	float load_factor_from_bank_angle = 1.0f;
 
 	if (PX4_ISFINITE(_att_sp.roll_body)) {
-		load_factor = 1.0f / cosf(_att_sp.roll_body);
+		load_factor_from_bank_angle = 1.0f / cosf(_att_sp.roll_body);
 	}
 
 	float weight_ratio = 1.0f;
@@ -438,16 +438,18 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 					       MAX_WEIGHT_RATIO);
 	}
 
-	// Adapt min airspeed setpoint based on wind estimate for more stability in higher winds.
+	// Here we make sure that the set minimum airspeed is automatically adapted to the current load factor.
-	if (_airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
+	// The minimum airspeed is the controller limit (FW_AIRSPD_MIN, unless either in takeoff or landing) that should
-		calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() * _wind_vel.length(),
+	// resemble the vehicles stall speed (CAS) with a 1g load plus some safety margin (as no controller tracks perfectly).
-						    _param_fw_airspd_max.get());
+	// Stall speed increases with the square root of the load factor: V_stall ~ sqrt(load_factor).
-	}
+	// The load_factor is composed of a term from the bank angle and a term from the weight ratio.
+	calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio);
 
-	// Stall speed increases with the square root of the load factor times the weight ratio
+	// Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds.
-	// Vs ~ sqrt(load_factor * weight_ratio)
+	if (_airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
-	calibrated_min_airspeed = constrain(_param_fw_airspd_stall.get() * sqrtf(load_factor * weight_ratio),
+		calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() *
-					    calibrated_min_airspeed, _param_fw_airspd_max.get());
+						    _wind_vel.length(), _param_fw_airspd_max.get());
+	}
 
 	calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
 				       _param_fw_airspd_max.get());

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -524,6 +524,10 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
  * The minimal airspeed (calibrated airspeed) the user is able to command.
  * Further, if the airspeed falls below this value, the TECS controller will try to
  * increase airspeed more aggressively.
+ * Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL),
+ * with some margin between the stall speed and minimum airspeed.
+ * This value corresponds to the desired minimum speed with the default load factor (level flight, default weight),
+ * and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE).
  *
  * @unit m/s
  * @min 0.5
@@ -537,7 +541,8 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
 /**
  * Maximum Airspeed (CAS)
  *
- * If the CAS (calibrated airspeed) is above this value, the TECS controller will try to decrease
+ * The maximal airspeed (calibrated airspeed) the user is able to command.
+ * Further, if the airspeed is above this value, the TECS controller will try to decrease
  * airspeed more aggressively.
  *
  * @unit m/s