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

add FF to FW rate controllers

Browse Source
This commit is contained in:
bresch
2021-10-25 13:41:31 +02:00
committed by Mathieu Bresciani
parent 55f0860c31
commit 6af0856558
8 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/ecl_yaw_controller.cpp
+2 -2
View File
@@ -142,11 +142,11 @@ float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);