dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/fw_pos_control/FixedwingPositionControl.cpp
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();
}