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

FW attitude controller: constrain rates correctly

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-08-19 09:40:48 +02:00
parent 15c95a7b6a
commit cd080289c6
6 changed files with 9 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/ecl_yaw_controller.cpp
+3 -2
View File
@@ -85,8 +85,9 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
/* Transform setpoint to body angular rates (jacobian) */
_body_rate_setpoint = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
_body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate);
}
if (!PX4_ISFINITE(_body_rate_setpoint)) {