fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 14:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FixedwingPositionControl: initialize the airspeed slew rate controller with trim airspeed in the constructor

Browse Source
This commit is contained in:
Thomas Stastny
2023-09-13 14:25:26 +02:00
committed by Daniel Agar
parent a1cd4fd5df
commit 28fe15d829
1 changed files with 10 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+10 -8
View File
@@ -76,6 +76,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
/* fetch initial parameter values */
parameters_update();
_airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_trim.get());
}
FixedwingPositionControl::~FixedwingPositionControl()
@@ -429,14 +431,6 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
_wind_vel.length(), _param_fw_airspd_max.get());
}
if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) {
_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
}
if (_airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get()) {
_airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_max.get());
}
// --- airspeed *setpoint adjustments ---
if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
@@ -470,6 +464,14 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
_airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);
}
if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) {
_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
}
if (_airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get()) {
_airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_max.get());
}
return _airspeed_slew_rate_controller.getState();
}