Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dynamically update ff gain for fw rate control #24120

Merged
merged 1 commit into from
Dec 17, 2024
Merged

Dynamically update ff gain for fw rate control #24120

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 5 additions & 8 deletions src/modules/fw_rate_control/FixedwingRateControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,10 +84,6 @@ FixedwingRateControl::parameters_update()
_rate_control.setIntegratorLimit(
Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get()));

_rate_control.setFeedForwardGain(
// set FF gains to 0 as we add the FF control outside of the rate controller
Vector3f(0.f, 0.f, 0.f));

if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) {
param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en);
}
Expand Down Expand Up @@ -354,14 +350,15 @@ void FixedwingRateControl::Run()
body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll);
}

const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get());
const Vector3f scaled_gain_ff = gain_ff / _airspeed_scaling;
_rate_control.setFeedForwardGain(scaled_gain_ff);

// Run attitude RATE controllers which need the desired attitudes from above, add trim.
const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
_landed);

const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get());
const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling;

Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward;
Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling;

// Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw
if (!_vcontrol_mode.flag_control_attitude_enabled && !_param_fw_acro_yaw_en.get()) {
Expand Down
Loading