perfromance model: add FW_AIRSPD_FLP_SC to reduce with flaps

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

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

@@ -92,7 +92,7 @@ FwLateralLongitudinalControl::parameters_update()
 	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
 	_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
 	_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
-	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
+	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed(getLoadFactor(), _flaps_setpoint));
 	_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
 	_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get());
@@ -168,6 +168,13 @@ void FwLateralLongitudinalControl::Run()
 
 		_vehicle_status_sub.update();
 		_control_mode_sub.update();
+
+		if (_flaps_setpoint_sub.updated()) {
+			normalized_unsigned_setpoint_s flaps_setpoint{};
+			_flaps_setpoint_sub.copy(&flaps_setpoint);
+			_flaps_setpoint = flaps_setpoint.normalized_setpoint;
+		}
+
 		update_control_state();
 
 		if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled
@@ -622,7 +629,7 @@ float
 FwLateralLongitudinalControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
 		float calibrated_min_airspeed_guidance, float wind_speed)
 {
-	float system_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
+	float system_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor(), _flaps_setpoint);
 
 	const float system_max_airspeed = _performance_model.getMaximumCalibratedAirspeed();
 

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp

@@ -66,6 +66,7 @@
 #include <uORB/topics/fixed_wing_lateral_setpoint.h>
 #include <uORB/topics/fixed_wing_lateral_status.h>
 #include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
 #include <uORB/topics/flight_phase_estimation.h>
 #include <uORB/topics/lateral_control_configuration.h>
 #include <uORB/topics/longitudinal_control_configuration.h>
@@ -110,6 +111,7 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
 	uORB::Subscription _wind_sub{ORB_ID(wind)};
 	uORB::SubscriptionData<vehicle_control_mode_s> _control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::SubscriptionData<vehicle_air_data_s> _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
@@ -127,6 +129,8 @@ private:
 	fixed_wing_lateral_setpoint_s _lat_control_sp{empty_lateral_control_setpoint};
 	lateral_control_configuration_s _lateral_configuration{};
 
+	float _flaps_setpoint{0.f};
+
 	uORB::Publication <vehicle_attitude_setpoint_s> _attitude_sp_pub;
 	uORB::Publication <tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
 	uORB::PublicationData <flight_phase_estimation_s> _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)};

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -2591,19 +2591,6 @@ fw_pos_control is the fixed-wing position controller.
 
 	return 0;
 }
-float FixedwingPositionControl::getLoadFactor() const
-{
-	float load_factor_from_bank_angle = 1.0f;
-
-	float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi();
-
-	if (PX4_ISFINITE(roll_body)) {
-		load_factor_from_bank_angle = 1.0f / math::max(cosf(roll_body), FLT_EPSILON);
-	}
-
-	return load_factor_from_bank_angle;
-
-}
 
 extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[])
 {

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -424,7 +424,6 @@ private:
 	void landing_status_publish();
 
 	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
-	float getLoadFactor() const;
 
 	/**
 	 * @brief Sets the landing abort status and publishes landing status.