UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(fw_latlon): align airspeed load factor (#27519)

Browse Source
This commit is contained in:
Eurus
2026-05-31 02:36:56 +08:00
committed by GitHub
parent a2be9197b5
commit 9a2b57ca82
2 changed files with 5 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -607,13 +607,13 @@ void FwLateralLongitudinalControl::updateAttitude() {
_long_control_state.pitch_rad = euler_angles.theta();
_yaw = euler_angles.psi();
const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON);
_load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON);
// Used to compensate for higher induced drag during banking
_tecs.set_load_factor(load_factor_from_bank_angle);
_tecs.set_load_factor(_load_factor_from_bank_angle);
// Used to give underspeed mitigation the correct minimum airspeed
_tecs.set_equivalent_airspeed_min(
_performance_model.getMinimumCalibratedAirspeed(load_factor_from_bank_angle, _flaps_setpoint)
_performance_model.getMinimumCalibratedAirspeed(_load_factor_from_bank_angle, _flaps_setpoint)
);
}
}
@@ -649,7 +649,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(), _flaps_setpoint);
float system_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(_load_factor_from_bank_angle, _flaps_setpoint);
const float system_max_airspeed = _performance_model.getMaximumCalibratedAirspeed();
@@ -853,19 +853,6 @@ void FwLateralLongitudinalControl::updateLongitudinalControlConfiguration(const
}
}
float FwLateralLongitudinalControl::getLoadFactor() const
{
float load_factor_from_bank_angle = 1.f;
const float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi();
if (PX4_ISFINITE(roll_body)) {
load_factor_from_bank_angle = 1.f / math::max(cosf(roll_body), FLT_EPSILON);
}
return load_factor_from_bank_angle;
}
extern "C" __EXPORT int fw_lat_lon_control_main(int argc, char *argv[])
{
return ModuleBase::main(FwLateralLongitudinalControl::desc, argc, argv);

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

@@ -201,6 +201,7 @@ private:
vehicle_attitude_setpoint_s _att_sp{};
bool _landed{false};
float _can_run_factor{0.f};
float _load_factor_from_bank_angle{1.f};
SlewRate<float> _airspeed_slew_rate_controller;
perf_counter_t _loop_perf; // loop performance counter
@@ -248,8 +249,6 @@ private:
void updateControllerConfiguration(hrt_abstime now);
float getLoadFactor() const;
/**
* @brief Returns an adapted calibrated airspeed setpoint
*