Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: added D_FF support for fixed wing

Browse Source
This commit is contained in:
Andrew Tridgell 2023-11-12 17:34:27 +11:00
parent 60d5e65a1d
commit 69bfe9b837
4 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/APM_Control/AP_AutoTune.cpp
View File

@ -179,7 +179,7 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg)
// filter actuator without I term so we can take ratios without // filter actuator without I term so we can take ratios without
// accounting for trim offsets. We first need to include the I and // accounting for trim offsets. We first need to include the I and
// clip to 45 degrees to get the right value of the real surface // clip to 45 degrees to get the right value of the real surface
const float clipped_actuator = constrain_float(pinfo.FF + pinfo.P + pinfo.D + pinfo.I, -45, 45) - pinfo.I; const float clipped_actuator = constrain_float(pinfo.FF + pinfo.P + pinfo.D + pinfo.DFF + pinfo.I, -45, 45) - pinfo.I;
const float actuator = actuator_filter.apply(clipped_actuator); const float actuator = actuator_filter.apply(clipped_actuator);
const float actual_rate = rate_filter.apply(pinfo.actual); const float actual_rate = rate_filter.apply(pinfo.actual);

3
libraries/APM_Control/AP_PitchController.cpp
View File

@ -218,13 +218,14 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
pinfo.P *= deg_scale; pinfo.P *= deg_scale;
pinfo.I *= deg_scale; pinfo.I *= deg_scale;
pinfo.D *= deg_scale; pinfo.D *= deg_scale;
pinfo.DFF *= deg_scale;
// fix the logged target and actual values to not have the scalers applied // fix the logged target and actual values to not have the scalers applied
pinfo.target = desired_rate; pinfo.target = desired_rate;
pinfo.actual = degrees(rate_y); pinfo.actual = degrees(rate_y);
// sum components // sum components
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D; float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
if (ground_mode) { if (ground_mode) {
// when on ground suppress D and half P term to prevent oscillations // when on ground suppress D and half P term to prevent oscillations
out -= pinfo.D + 0.5*pinfo.P; out -= pinfo.D + 0.5*pinfo.P;

3
libraries/APM_Control/AP_RollController.cpp
View File

@ -206,13 +206,14 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
pinfo.P *= deg_scale; pinfo.P *= deg_scale;
pinfo.I *= deg_scale; pinfo.I *= deg_scale;
pinfo.D *= deg_scale; pinfo.D *= deg_scale;
pinfo.DFF *= deg_scale;
// fix the logged target and actual values to not have the scalers applied // fix the logged target and actual values to not have the scalers applied
pinfo.target = desired_rate; pinfo.target = desired_rate;
pinfo.actual = degrees(rate_x); pinfo.actual = degrees(rate_x);
// sum components // sum components
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D; float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
if (ground_mode) { if (ground_mode) {
// when on ground suppress D term to prevent oscillations // when on ground suppress D term to prevent oscillations
out -= pinfo.D + 0.5*pinfo.P; out -= pinfo.D + 0.5*pinfo.P;

3
libraries/APM_Control/AP_YawController.cpp
View File

@ -344,6 +344,7 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa
pinfo.P *= deg_scale; pinfo.P *= deg_scale;
pinfo.I *= deg_scale; pinfo.I *= deg_scale;
pinfo.D *= deg_scale; pinfo.D *= deg_scale;
pinfo.DFF *= deg_scale;
pinfo.limit = limit_I; pinfo.limit = limit_I;
// fix the logged target and actual values to not have the scalers applied // fix the logged target and actual values to not have the scalers applied
@ -351,7 +352,7 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa
pinfo.actual = degrees(rate_z); pinfo.actual = degrees(rate_z);
// sum components // sum components
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D; float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
// remember the last output to trigger the I limit // remember the last output to trigger the I limit
_last_out = out; _last_out = out;