Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: move rmax and tau more slowly

Browse Source
This commit is contained in:
Andrew Tridgell 2021-04-02 13:31:45 +11:00
parent 3cb32a18f0
commit 0b76a8018f
2 changed files with 32 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/APM_Control/AP_AutoTune.cpp
View File

@ -126,9 +126,12 @@ void AP_AutoTune::start(void)
level = 1;
}
current.rmax_pos.set(tuning_table[level-1].rmax);
current.rmax_neg.set(tuning_table[level-1].rmax);
current.tau.set(tuning_table[level-1].tau);
target_rmax = tuning_table[level-1].rmax;
target_tau = tuning_table[level-1].tau;
// do first update of rmax and tau now
update_rmax();
rpid.kIMAX().set(constrain_float(rpid.kIMAX(), AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX));
next_save = current;
@ -354,6 +357,9 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
rpid.kI().get(),
rpid.kD().get());
// move rmax and tau towards target
update_rmax();
min_Dmod = 1;
max_P = max_D = 0;
state = new_state;
@ -460,3 +466,21 @@ void AP_AutoTune::set_gains(const ATGains &v)
rpid.kD().set(v.D);
rpid.kIMAX().set(v.IMAX);
}
void AP_AutoTune::update_rmax(void)
{
if (current.rmax_pos == 0) {
// conservative initial value
current.rmax_pos.set(75);
}
// move by 20 deg/s per step
current.rmax_pos.set(constrain_int32(target_rmax,
current.rmax_pos.get()-20,
current.rmax_pos.get()+20));
current.rmax_neg.set(current.rmax_pos.get());
// move tau by max 15% per loop
current.tau.set(constrain_float(target_tau,
current.tau*0.85,
current.tau*1.15));
}

5
libraries/APM_Control/AP_AutoTune.h
View File

@ -99,6 +99,9 @@ private:
ATGains get_gains(const ATGains &current);
void set_gains(const ATGains &v);
// update rmax and tau towards target
void update_rmax();
// 5 point mode filter for FF estimate
ModeFilterFloat_Size5 ff_filter;
@ -115,4 +118,6 @@ private:
float max_D;
float min_Dmod;
float FF_single;
int16_t target_rmax;
float target_tau;
};