Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: Ensure adequate trim speed

Browse Source
This commit is contained in:
Paul Riseborough 2021-04-05 10:25:05 +10:00 committed by Andrew Tridgell
parent 4622e33255
commit 9e6561e6b5
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/APM_Control/AP_AutoTune.cpp
View File

@ -58,6 +58,9 @@ extern const AP_HAL::HAL& hal;
// ratio of I to P
#define AUTOTUNE_I_RATIO 0.75
// time constant of rate trim loop
#define TRIM_TCONST 1.0f
// overshoot threshold
#define AUTOTUNE_OVERSHOOT 1.1
@ -365,7 +368,8 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
rpid.ff().set(FF);
rpid.kP().set(P);
rpid.kD().set(D);
rpid.kI().set(P*AUTOTUNE_I_RATIO);
rpid.kI().set(MAX(P*AUTOTUNE_I_RATIO, (FF / TRIM_TCONST)));
current.FF = FF;
current.P = P;
current.I = rpid.kI().get();
@ -509,6 +513,10 @@ void AP_AutoTune::update_rmax(void)
target_tau = tuning_table[level-1].tau;
}
if (level > 0) {
target_tau = 1.0f / ((1.0f / target_tau) + (current.I / current.FF));
}
if (current.rmax_pos == 0) {
// conservative initial value
current.rmax_pos.set(75);