fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

perfromance model: add FW_AIRSPD_FLP_SC to reduce with flaps

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan 2025-03-27 23:29:36 +01:00 committed by Silvan Fuhrer
parent 3e3f10f5bc
commit 0ea109ff5d
10 changed files with 42 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
docs/en/flight_modes_fw/land.md
View File

@ -45,6 +45,7 @@ Land mode behaviour can be configured using the parameters below.
| <a id="NAV_LOITER_RAD"></a>[NAV_LOITER_RAD](../advanced_config/parameter_reference.md#NAV_LOITER_RAD) | The loiter radius that the controller tracks for the whole landing sequence. |
| <a id="FW_LND_ANG"></a>[FW_LND_ANG](../advanced_config/parameter_reference.md#FW_LND_ANG) | The flight path angle setpoint. |
| <a id="FW_LND_AIRSPD"></a>[FW_LND_AIRSPD](../advanced_config/parameter_reference.md#FW_LND_AIRSPD) | The airspeed setpoint. |
| <a id="FW_AIRSPD_FLP_SC"></a>[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

1
docs/en/flight_modes_fw/takeoff.md
View File

@ -50,6 +50,7 @@ Parameters that affect both catapult/hand-launch and runway takeoffs:
| <a id="FW_TKO_PITCH_MIN"></a>[FW_TKO_PITCH_MIN](../advanced_config/parameter_reference.md#FW_TKO_PITCH_MIN) | This is the minimum pitch angle setpoint during the climbout phase |
| <a id="FW_T_CLMB_MAX"></a>[FW_T_CLMB_MAX](../advanced_config/parameter_reference.md#FW_T_CLMB_MAX) | Maximum climb rate. |
| <a id="FW_FLAPS_TO_SCL"></a>[FW_FLAPS_TO_SCL](../advanced_config/parameter_reference.md#FW_FLAPS_TO_SCL) | Flaps setpoint during takeoff |
| <a id="FW_AIRSPD_FLP_SC"></a>[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)).

11
src/lib/fw_performance_model/PerformanceModel.cpp
View File

@ -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

12
src/lib/fw_performance_model/PerformanceModel.hpp
View File

@ -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<px4::params::FW_THR_MAX>) _param_fw_thr_max,
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
(ParamFloat<px4::params::FW_THR_ASPD_MIN>) _param_fw_thr_aspd_min,
(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _param_fw_thr_aspd_max)
(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _param_fw_thr_aspd_max,
(ParamFloat<px4::params::FW_AIRSPD_FLP_SC>) _param_fw_airspd_flp_sc
)
/**
* Get the sea level trim throttle for a given calibrated airspeed setpoint.

13
src/lib/fw_performance_model/performance_model_params.c
View File

@ -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);

12
src/lib/npfg/CourseToAirspeedRefMapper.hpp
View File

@ -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.

11
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
View File

@ -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();

4
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
View File

@ -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)};

13
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -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[])
{

1
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -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.