aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_Control: fixed handling of clipped actuator in autotune

Browse Source
This commit is contained in:
Andrew Tridgell 2021-04-03 12:00:44 +11:00
parent 9997b191a8
commit 316541017a
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/APM_Control/AP_AutoTune.cpp
View File

@ -177,9 +177,11 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
ATState new_state = state;
const float desired_rate = pinfo.target;
// filter actuator without I term so we can take ratios without accounting
// for trim offsets
const float actuator = actuator_filter.apply(pinfo.FF + pinfo.P + pinfo.D);
// filter actuator without I term so we can take ratios without
// 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
const float clipped_actuator = constrain_float(pinfo.FF + pinfo.P + pinfo.D + pinfo.I, -45, 45) - pinfo.I;
const float actuator = actuator_filter.apply(clipped_actuator);
const float actual_rate = rate_filter.apply(pinfo.actual);
max_actuator = MAX(max_actuator, actuator);