fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 12:24:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added feedforward terms in fw att control

Browse Source
This commit is contained in:
Jaeyoung Lim 2022-08-11 17:47:35 +02:00 committed by Silvan Fuhrer
parent 958c61dbba
commit 33d068cfb2
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -558,7 +558,8 @@ void FixedwingAttitudeControl::Run()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed);
float roll_u = att_control(0) * _airspeed_scaling * _airspeed_scaling;
float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * rates_setpoint(0);
float roll_u = att_control(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
(PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
@ -566,7 +567,8 @@ void FixedwingAttitudeControl::Run()
_roll_ctrl.reset_integrator();
}
float pitch_u = att_control(1) * _airspeed_scaling * _airspeed_scaling;
float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * rates_setpoint(0);
float pitch_u = att_control(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
(PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;