fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 06:30:36 +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-12 14:17:43 +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_pitch_controller.cpp
+3 -2
View File
@@ -62,8 +62,9 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
_euler_rate_setpoint = pitch_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
_body_rate_setpoint = cosf(ctl_data.roll) * _euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate);
return _body_rate_setpoint;
}