fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position control: add roll slewrate also in manual Position flight mode

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-10-05 18:06:49 -04:00
committed by Daniel Agar
parent 2edd97102b
commit 98c7f664c1
1 changed files with 11 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+11 -1
View File
@@ -1106,7 +1106,17 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
// do slew rate limiting on roll if enabled
float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());
if (dt > 0.f && roll_rate_slew_rad > 0.f) {
roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * dt,
_att_sp.roll_body + roll_rate_slew_rad * dt);
}
_att_sp.roll_body = roll_sp_new;
_att_sp.yaw_body = 0;
}