diff --git a/docs/en/flight_modes_fw/land.md b/docs/en/flight_modes_fw/land.md index 3c294e95f0..43d8ff9294 100644 --- a/docs/en/flight_modes_fw/land.md +++ b/docs/en/flight_modes_fw/land.md @@ -45,6 +45,7 @@ Land mode behaviour can be configured using the parameters below. | [NAV_LOITER_RAD](../advanced_config/parameter_reference.md#NAV_LOITER_RAD) | The loiter radius that the controller tracks for the whole landing sequence. | | [FW_LND_ANG](../advanced_config/parameter_reference.md#FW_LND_ANG) | The flight path angle setpoint. | | [FW_LND_AIRSPD](../advanced_config/parameter_reference.md#FW_LND_AIRSPD) | The airspeed setpoint. | +| [FW_AIRSPD_FLP_SC](../advanced_config/parameter_reference.md#FW_AIRSPD_FLP_SC) | Factor applied to the minimum airspeed when flaps are fully deployed. Necessary if FW_LND_AIRSPD is below FW_AIRSPD_MIN. ## See Also diff --git a/docs/en/flight_modes_fw/takeoff.md b/docs/en/flight_modes_fw/takeoff.md index b9e6d2e703..a59413394d 100644 --- a/docs/en/flight_modes_fw/takeoff.md +++ b/docs/en/flight_modes_fw/takeoff.md @@ -50,6 +50,7 @@ Parameters that affect both catapult/hand-launch and runway takeoffs: | [FW_TKO_PITCH_MIN](../advanced_config/parameter_reference.md#FW_TKO_PITCH_MIN) | This is the minimum pitch angle setpoint during the climbout phase | | [FW_T_CLMB_MAX](../advanced_config/parameter_reference.md#FW_T_CLMB_MAX) | Maximum climb rate. | | [FW_FLAPS_TO_SCL](../advanced_config/parameter_reference.md#FW_FLAPS_TO_SCL) | Flaps setpoint during takeoff | +| [FW_AIRSPD_FLP_SC](../advanced_config/parameter_reference.md#FW_AIRSPD_FLP_SC) | Factor applied to the minimum airspeed when flaps are fully deployed. Necessary if FW_TKO_AIRSPD is below FW_AIRSPD_MIN. | ::: info The vehicle always respects normal FW max/min throttle settings during takeoff ([FW_THR_MIN](../advanced_config/parameter_reference.md#FW_THR_MIN), [FW_THR_MAX](../advanced_config/parameter_reference.md#FW_THR_MAX)). diff --git a/src/lib/fw_performance_model/PerformanceModel.cpp b/src/lib/fw_performance_model/PerformanceModel.cpp index 6fb315cb89..e946347a44 100644 --- a/src/lib/fw_performance_model/PerformanceModel.cpp +++ b/src/lib/fw_performance_model/PerformanceModel.cpp @@ -144,17 +144,18 @@ float PerformanceModel::getCalibratedTrimAirspeed() const return math::constrain(_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()), _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); } -float PerformanceModel::getMinimumCalibratedAirspeed(float load_factor) const +float PerformanceModel::getMinimumCalibratedAirspeed(float load_factor, float flaps_setpoint) const { - load_factor = math::max(load_factor, FLT_EPSILON); - return _param_fw_airspd_min.get() * sqrtf(getWeightRatio() * load_factor); + const float airspeed_flap_factor = math::lerp(1.f, _param_fw_airspd_flp_sc.get(), flaps_setpoint); + return (_param_fw_airspd_min.get() * airspeed_flap_factor) * sqrtf(getWeightRatio() * load_factor); } -float PerformanceModel::getCalibratedStallAirspeed(float load_factor) const +float PerformanceModel::getCalibratedStallAirspeed(float load_factor, float flaps_setpoint) const { load_factor = math::max(load_factor, FLT_EPSILON); - return _param_fw_airspd_stall.get() * sqrtf(getWeightRatio() * load_factor); + const float airspeed_flap_factor = math::lerp(1.f, _param_fw_airspd_flp_sc.get(), flaps_setpoint); + return (_param_fw_airspd_stall.get() * airspeed_flap_factor) * sqrtf(getWeightRatio() * load_factor); } float PerformanceModel::getMaximumCalibratedAirspeed() const diff --git a/src/lib/fw_performance_model/PerformanceModel.hpp b/src/lib/fw_performance_model/PerformanceModel.hpp index fbdd8693c2..dece1bb3e0 100644 --- a/src/lib/fw_performance_model/PerformanceModel.hpp +++ b/src/lib/fw_performance_model/PerformanceModel.hpp @@ -94,11 +94,12 @@ public: float getCalibratedTrimAirspeed() const; /** - * Get the minimum airspeed compensated for weight and load factor due to bank angle. + * Get the minimum airspeed compensated for weight, load factor due to bank angle and flaps. * @param load_factor due to banking + * @param flaps_setpoint Flaps setpoint, normalized in range [0,1] * @return calibrated minimum airspeed in m/s */ - float getMinimumCalibratedAirspeed(float load_factor = 1.0f) const; + float getMinimumCalibratedAirspeed(float load_factor, float flaps_setpoint) const; /** * Get the maximum airspeed. @@ -109,9 +110,10 @@ public: /** * get the stall airspeed compensated for load factor due to bank angle. * @param load_factor load factor due to banking + * @param flaps_setpoint Flaps setpoint, normalized in range [0,1] * @return calibrated stall airspeed in m/s */ - float getCalibratedStallAirspeed(float load_factor) const; + float getCalibratedStallAirspeed(float load_factor, float flaps_setpoint) const; /** * Run some checks on parameters and detect unfeasible combinations. @@ -134,7 +136,9 @@ private: (ParamFloat) _param_fw_thr_max, (ParamFloat) _param_fw_thr_min, (ParamFloat) _param_fw_thr_aspd_min, - (ParamFloat) _param_fw_thr_aspd_max) + (ParamFloat) _param_fw_thr_aspd_max, + (ParamFloat) _param_fw_airspd_flp_sc + ) /** * Get the sea level trim throttle for a given calibrated airspeed setpoint. diff --git a/src/lib/fw_performance_model/performance_model_params.c b/src/lib/fw_performance_model/performance_model_params.c index 0c39d2ed39..2223f60139 100644 --- a/src/lib/fw_performance_model/performance_model_params.c +++ b/src/lib/fw_performance_model/performance_model_params.c @@ -212,3 +212,16 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); * @group FW Performance */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); + +/** + * Airspeed scale with full flaps + * + * Factor applied to the minimum and stall airspeed when flaps are fully deployed. + * + * @min 0.5 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group FW Performance + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_FLP_SC, 1.f); diff --git a/src/lib/npfg/CourseToAirspeedRefMapper.hpp b/src/lib/npfg/CourseToAirspeedRefMapper.hpp index 70f3312e9b..65d8e7ae81 100644 --- a/src/lib/npfg/CourseToAirspeedRefMapper.hpp +++ b/src/lib/npfg/CourseToAirspeedRefMapper.hpp @@ -77,18 +77,6 @@ public: const matrix::Vector2f &wind_vel, float max_airspeed, float min_ground_speed) const; private: - /* - * Determines a reference air velocity *without curvature compensation, but - * including "optimal" true airspeed reference compensation in excess wind conditions. - * Nominal and maximum true airspeed member variables must be set before using this method. - * - * @param[in] wind_vel Wind velocity vector [m/s] - * @param[in] bearing_setpoint Bearing - * @param[in] airspeed_true True airspeed setpoint[m/s] - * @return Air velocity vector [m/s] - */ - matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const float bearing_setpoint, - float airspeed_true, float min_ground_speed) const; /* * Projection of the air velocity vector onto the bearing line considering * a connected wind triangle. diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index eb92670ffa..77e0ae5b23 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/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(); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index 703fb722e9..590054ae89 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -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 _control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::SubscriptionData _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 _attitude_sp_pub; uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; uORB::PublicationData _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)}; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 328a556be2..672a718dc8 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/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[]) { diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index e7526dca40..aea0dd5a89 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -424,7 +424,6 @@ private: void landing_status_publish(); void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); - float getLoadFactor() const; /** * @brief Sets the landing abort status and publishes landing status.