LCOV - code coverage report
Current view: top level - APM_Control - AP_AutoTune.cpp (source / functions) Hit Total Coverage
Test: lcov.info Lines: 272 300 90.7 %
Date: 2026-09-09 11:02:08 Functions: 13 13 100.0 %

          Line data    Source code
       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /**
      17             :    The strategy for roll/pitch autotune is to give the user a AUTOTUNE
      18             :    flight mode which behaves just like FBWA, but does automatic
      19             :    tuning.
      20             : */
      21             : 
      22             : #include "AP_AutoTune.h"
      23             : 
      24             : #include <AP_Common/AP_Common.h>
      25             : #include <AP_HAL/AP_HAL.h>
      26             : #include <AP_Logger/AP_Logger.h>
      27             : #include <AP_Math/AP_Math.h>
      28             : #include <AC_PID/AC_PID.h>
      29             : #include <AP_Scheduler/AP_Scheduler.h>
      30             : #include <GCS_MAVLink/GCS.h>
      31             : #include <AP_InertialSensor/AP_InertialSensor.h>
      32             : 
      33             : extern const AP_HAL::HAL& hal;
      34             : 
      35             : // step size for changing FF gains, percentage
      36             : #define AUTOTUNE_INCREASE_FF_STEP 12
      37             : #define AUTOTUNE_DECREASE_FF_STEP 15
      38             : 
      39             : // limits on IMAX
      40             : #define AUTOTUNE_MIN_IMAX 0.4
      41             : #define AUTOTUNE_MAX_IMAX 0.9
      42             : 
      43             : // ratio of I to P
      44             : #define AUTOTUNE_I_RATIO 0.75
      45             : 
      46             : // time constant of rate trim loop
      47             : #define TRIM_TCONST 1.0f
      48             : 
      49             : // constructor
      50           6 : AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
      51             :                          const AP_FixedWing &parms,
      52           6 :                          AC_PID &_rpid) :
      53           6 :     current(_gains),
      54           6 :     rpid(_rpid),
      55           6 :     type(_type),
      56           6 :     aparm(parms),
      57           6 :     ff_filter(2)
      58           6 : {}
      59             : 
      60             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      61             : #include <stdio.h>
      62             : # define Debug(fmt, args ...)  do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
      63             : #else
      64             : # define Debug(fmt, args ...)
      65             : #endif
      66             : 
      67             : /*
      68             :   auto-tuning table. This table gives the starting values for key
      69             :   tuning parameters based on a user chosen AUTOTUNE_LEVEL parameter
      70             :   from 1 to 10. Level 1 is a very soft tune. Level 10 is a very
      71             :   aggressive tune.
      72             :   Level 0 means use the existing RMAX and TCONST parameters
      73             :  */
      74             : static const struct {
      75             :     float tau;
      76             :     float rmax;
      77             : } tuning_table[] = {
      78             :     { 1.00, 20 },   // level 1
      79             :     { 0.90, 30 },   // level 2
      80             :     { 0.80, 40 },   // level 3
      81             :     { 0.70, 50 },   // level 4
      82             :     { 0.60, 60 },   // level 5
      83             :     { 0.50, 75 },   // level 6
      84             :     { 0.30, 90 },   // level 7
      85             :     { 0.2, 120 },   // level 8
      86             :     { 0.15, 160 },   // level 9
      87             :     { 0.1, 210 },   // level 10
      88             :     { 0.1, 300 },   // (yes, it goes to 11)
      89             : };
      90             : 
      91             : /*
      92             :   start an autotune session
      93             : */
      94           8 : void AP_AutoTune::start(void)
      95             : {
      96           8 :     running = true;
      97           8 :     state = ATState::IDLE;
      98             : 
      99           8 :     current = restore = last_save = get_gains();
     100             : 
     101             :     // do first update of rmax and tau now
     102           8 :     update_rmax();
     103             : 
     104           8 :     dt = AP::scheduler().get_loop_period_s();
     105             : 
     106           8 :     rpid.kIMAX().set(constrain_float(rpid.kIMAX(), AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX));
     107             : 
     108             :     // use 0.75Hz filters on the actuator, rate and target to reduce impact of noise
     109           8 :     actuator_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 0.75);
     110           8 :     rate_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 0.75);
     111             : 
     112             :     // target filter is a bit broader
     113           8 :     target_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 4);
     114             : 
     115           8 :     ff_filter.reset();
     116           8 :     actuator_filter.reset();
     117           8 :     rate_filter.reset();
     118           8 :     D_limit = 0;
     119           8 :     P_limit = 0;
     120           8 :     ff_count = 0;
     121           8 :     D_set_ms = 0;
     122           8 :     P_set_ms = 0;
     123           8 :     done_count = 0;
     124             : 
     125           8 :     if (!is_positive(rpid.slew_limit())) {
     126             :         // we must have a slew limit, default to 150 deg/s
     127           0 :         rpid.slew_limit().set_and_save(150);
     128             :     }
     129             : 
     130           8 :     if (current.FF < 0.01) {
     131             :         // don't allow for zero FF
     132           0 :         current.FF = 0.01;
     133           0 :         rpid.ff().set(current.FF);
     134             :     }
     135             : 
     136           8 :     Debug("START FF -> %.3f\n", rpid.ff().get());
     137           8 : }
     138             : 
     139             : /*
     140             :   called when we change state to see if we should change gains
     141             :  */
     142          64 : void AP_AutoTune::stop(void)
     143             : {
     144          64 :     if (running) {
     145           8 :         running = false;
     146           8 :         if (is_positive(D_limit) && is_positive(P_limit)) {
     147           4 :             save_gains();
     148             :         } else {
     149           4 :             restore_gains();
     150             :         }
     151             :     }
     152          64 : }
     153             : 
     154          13 : const char *AP_AutoTune::axis_string(void) const
     155             : {
     156          13 :     return axis_string(type);
     157             : }
     158             : 
     159          13 : const char *AP_AutoTune::axis_string(ATType _type)
     160             : {
     161          13 :     switch (_type) {
     162           7 :     case AUTOTUNE_ROLL:
     163           7 :         return "Roll";
     164           6 :     case AUTOTUNE_PITCH:
     165           6 :         return "Pitch";
     166           0 :     case AUTOTUNE_YAW:
     167           0 :         return "Yaw";
     168             :     }
     169           0 :     return "";
     170             : }
     171             : 
     172             : /*
     173             :   one update cycle of the autotuner
     174             :  */
     175       37088 : void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg)
     176             : {
     177       37088 :     if (!running) {
     178       36940 :         return;
     179             :     }
     180             : 
     181             :     // see what state we are in
     182       37088 :     ATState new_state = state;
     183       37088 :     const float desired_rate = target_filter.apply(pinfo.target);
     184             : 
     185             :     // filter actuator without I term so we can take ratios without
     186             :     // accounting for trim offsets. We first need to include the I and
     187             :     // clip to 45 degrees to get the right value of the real surface
     188       37088 :     const float clipped_actuator = constrain_float(pinfo.FF + pinfo.P + pinfo.D + pinfo.DFF + pinfo.I, -45, 45) - pinfo.I;
     189       37088 :     const float actuator = actuator_filter.apply(clipped_actuator);
     190       37088 :     const float actual_rate = rate_filter.apply(pinfo.actual);
     191             : 
     192       37088 :     max_actuator = MAX(max_actuator, actuator);
     193       37088 :     min_actuator = MIN(min_actuator, actuator);
     194       37088 :     max_rate = MAX(max_rate, actual_rate);
     195       37088 :     min_rate = MIN(min_rate, actual_rate);
     196       37088 :     max_target = MAX(max_target, desired_rate);
     197       37088 :     min_target = MIN(min_target, desired_rate);
     198       37088 :     max_P = MAX(max_P, fabsf(pinfo.P));
     199       37088 :     max_D = MAX(max_D, fabsf(pinfo.D));
     200       37088 :     min_Dmod = MIN(min_Dmod, pinfo.Dmod);
     201       37088 :     max_Dmod = MAX(max_Dmod, pinfo.Dmod);
     202             : 
     203             :     // update the P and D slew rates, using P and D values from before Dmod was applied
     204       37088 :     const float slew_limit_scale = 45.0 / degrees(1);
     205       37088 :     slew_limit_max = rpid.slew_limit();
     206       37088 :     slew_limit_tau = 1.0;
     207       37088 :     slew_limiter_P.modifier((pinfo.P/pinfo.Dmod)*slew_limit_scale, dt);
     208       37088 :     slew_limiter_D.modifier((pinfo.D/pinfo.Dmod)*slew_limit_scale, dt);
     209             : 
     210             :     // remember maximum slew rates for this cycle
     211       37088 :     max_SRate_P = MAX(max_SRate_P, slew_limiter_P.get_slew_rate());
     212       37088 :     max_SRate_D = MAX(max_SRate_D, slew_limiter_D.get_slew_rate());
     213             : 
     214       37088 :     float att_limit_deg = 0;
     215       37088 :     switch (type) {
     216       18544 :     case AUTOTUNE_ROLL:
     217       18544 :         att_limit_deg = aparm.roll_limit;
     218       18544 :         break;
     219       18544 :     case AUTOTUNE_PITCH:
     220       18544 :         att_limit_deg = MIN(abs(aparm.pitch_limit_max*100),abs(aparm.pitch_limit_min*100))*0.01;
     221       18544 :         break;
     222           0 :     case AUTOTUNE_YAW:
     223             :         // arbitrary value for yaw angle
     224           0 :         att_limit_deg = 20;
     225           0 :         break;
     226             :     }
     227             : 
     228             : 
     229             :     // thresholds for when we consider an event to start and end
     230       37088 :     const float rate_threshold1 = 0.4 * MIN(att_limit_deg / current.tau.get(), current.rmax_pos);
     231       37088 :     const float rate_threshold2 = 0.25 * rate_threshold1;
     232       37088 :     bool in_att_demand = fabsf(angle_err_deg) >= 0.3 * att_limit_deg;
     233             : 
     234       37088 :     switch (state) {
     235       23183 :     case ATState::IDLE:
     236       23183 :         if (desired_rate > rate_threshold1 && in_att_demand) {
     237          74 :             new_state = ATState::DEMAND_POS;
     238       23109 :         } else if (desired_rate < -rate_threshold1 && in_att_demand) {
     239          75 :             new_state = ATState::DEMAND_NEG;
     240             :         }
     241       23183 :         break;
     242        6910 :     case ATState::DEMAND_POS:
     243        6910 :         if (desired_rate < rate_threshold2) {
     244          73 :             new_state = ATState::IDLE;
     245             :         }
     246        6910 :         break;
     247        6995 :     case ATState::DEMAND_NEG:
     248        6995 :         if (desired_rate > -rate_threshold2) {
     249          75 :             new_state = ATState::IDLE;
     250             :         }
     251        6995 :         break;
     252             :     }
     253             : 
     254       37088 :     const uint32_t now = AP_HAL::millis();
     255             : 
     256             : #if HAL_LOGGING_ENABLED
     257       37088 :     if (now - last_log_ms >= 40) {
     258             :         // log at 25Hz
     259       18368 :         const struct log_ATRP pkt {
     260             :             LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
     261       18368 :             time_us : AP_HAL::micros64(),
     262       18368 :             type : uint8_t(type),
     263             :             state: uint8_t(new_state),
     264             :             actuator : actuator,
     265       18368 :             P_slew : max_SRate_P,
     266       18368 :             D_slew : max_SRate_D,
     267       18368 :             FF_single: FF_single,
     268       18368 :             FF: current.FF,
     269       18368 :             P: current.P,
     270       18368 :             I: current.I,
     271       18368 :             D: current.D,
     272       18368 :             action: uint8_t(action),
     273       36736 :             rmax: float(current.rmax_pos.get()),
     274       36736 :             tau: current.tau.get()
     275       18368 :         };
     276       18368 :         AP::logger().WriteBlock(&pkt, sizeof(pkt));
     277       18368 :         last_log_ms = now;
     278             :     }
     279             : #endif
     280             : 
     281       37088 :     if (new_state == state) {
     282       36791 :         if (state == ATState::IDLE &&
     283       23034 :             now - state_enter_ms > 500 &&
     284       21110 :             max_Dmod < 0.9) {
     285             :             // we've been oscillating while idle, reduce P or D
     286           0 :             const float slew_sum = max_SRate_P + max_SRate_D;
     287           0 :             const float gain_mul = 0.5;
     288           0 :             current.P *= linear_interpolate(gain_mul, 1.0,
     289             :                                             max_SRate_P,
     290             :                                             slew_sum, 0);
     291           0 :             current.D *= linear_interpolate(gain_mul, 1.0,
     292             :                                             max_SRate_D,
     293             :                                             slew_sum, 0);
     294           0 :             rpid.kP().set(current.P);
     295           0 :             rpid.kD().set(current.D);
     296           0 :             action = Action::IDLE_LOWER_PD;
     297           0 :             P_limit = MIN(P_limit, current.P);
     298           0 :             D_limit = MIN(D_limit, current.D);
     299           0 :             state_change(state);
     300             :         }
     301       36791 :         return;
     302             :     }
     303             : 
     304         297 :     if (new_state != ATState::IDLE) {
     305             :         // starting an event
     306         149 :         min_actuator = max_actuator = min_rate = max_rate = 0;
     307         149 :         state_enter_ms = now;
     308         149 :         state = new_state;
     309         149 :         return;
     310             :     }
     311             : 
     312         296 :     if ((state == ATState::DEMAND_POS && max_rate < 0.01 * current.rmax_pos) ||
     313         148 :         (state == ATState::DEMAND_NEG && min_rate > -0.01 * current.rmax_neg)) {
     314             :         // we didn't get enough rate
     315           0 :         action = Action::LOW_RATE;
     316           0 :         state_change(ATState::IDLE);
     317           0 :         return;
     318             :     }
     319             : 
     320         148 :     if (now - state_enter_ms < 100) {
     321             :         // not long enough sample
     322           0 :         action = Action::SHORT;
     323           0 :         state_change(ATState::IDLE);
     324           0 :         return;
     325             :     }
     326             : 
     327             :     // we've finished an event. calculate the single-event FF value
     328         148 :     if (state == ATState::DEMAND_POS) {
     329          73 :         FF_single = max_actuator / (max_rate * scaler);
     330             :     } else {
     331          75 :         FF_single = min_actuator / (min_rate * scaler);
     332             :     }
     333             : 
     334             :     // apply median filter
     335         148 :     float FF = ff_filter.apply(FF_single);
     336         148 :     ff_count++;
     337             : 
     338         148 :     const float old_FF = rpid.ff();
     339             : 
     340             :     // limit size of change in FF
     341         148 :     FF = constrain_float(FF,
     342             :                          old_FF*(1-AUTOTUNE_DECREASE_FF_STEP*0.01),
     343             :                          old_FF*(1+AUTOTUNE_INCREASE_FF_STEP*0.01));
     344             : 
     345             :     // adjust P and D
     346         148 :     float D = rpid.kD();
     347         148 :     float P = rpid.kP();
     348             : 
     349         148 :     if (ff_count == 1) {
     350             :         // apply minimum D and P values
     351           4 :         D = MAX(D, 0.0005);
     352           4 :         P = MAX(P, 0.01);
     353         144 :     } else if (ff_count == 4) {
     354             :         // we got a good ff estimate, halve P ready to start raising D
     355           4 :         P *= 0.5;
     356             :     }
     357             : 
     358             :     // see if the slew limiter kicked in
     359         148 :     if (min_Dmod < 1.0 && !is_positive(D_limit)) {
     360             :         // oscillation, without D_limit set
     361           6 :         if (max_P > 0.5 * max_D) {
     362             :             // lower P and D to get us to a non-oscillating state
     363           2 :             P *= 0.35;
     364           2 :             D *= 0.75;
     365           2 :             action = Action::LOWER_PD;
     366             :         } else {
     367             :             // set D limit to 30% of current D, remember D limit and start to work on P
     368           4 :             D *= 0.3;
     369           4 :             D_limit = D;
     370           4 :             D_set_ms = now;
     371           4 :             action = Action::LOWER_D;
     372           4 :             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", axis_string(), D_limit);
     373             :         }
     374         142 :     } else if (min_Dmod < 1.0) {
     375             :         // oscillation, with D_limit set
     376           5 :         if (now - D_set_ms > 2000) {
     377             :             // leave 2s for Dmod to settle after lowering D
     378           5 :             if (max_D > 0.8 * max_P) {
     379             :                 // lower D limit some more
     380           1 :                 D *= 0.35;
     381           1 :                 D_limit = D;
     382           1 :                 D_set_ms = now;
     383           1 :                 action = Action::LOWER_D;
     384           1 :                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", axis_string(), D_limit);
     385           1 :                 done_count = 0;
     386           4 :             } else if (now - P_set_ms > 2500) {
     387           4 :                 if (is_positive(P_limit)) {
     388             :                     // if we've already got a P estimate then don't
     389             :                     // reduce as quickly, stopping small spikes at the
     390             :                     // later part of the tune from giving us a very
     391             :                     // low P gain
     392           0 :                     P *= 0.7;
     393             :                 } else {
     394           4 :                     P *= 0.35;
     395             :                 }
     396           4 :                 P_limit = P;
     397           4 :                 P_set_ms = now;
     398           4 :                 action = Action::LOWER_P;
     399           4 :                 done_count = 0;
     400           4 :                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sP: %.4f", axis_string(), P_limit);
     401             :             }
     402             :         }
     403         137 :     } else if (ff_count < 4) {
     404             :         // we don't have a good FF estimate yet, keep going
     405             : 
     406         127 :     } else if (!is_positive(D_limit)) {
     407             :         /* we haven't detected D oscillation yet, keep raising D */
     408          49 :         D *= 1.3;
     409          49 :         action = Action::RAISE_D;
     410          78 :     } else if (!is_positive(P_limit)) {
     411             :         /* not oscillating, increase P gain */
     412          63 :         P *= 1.3;
     413          63 :         action = Action::RAISE_PD;
     414             :     } else {
     415             :         // after getting P_limit we consider the tune done when we
     416             :         // have done 3 cycles without reducing P
     417          15 :         if (done_count < 3) {
     418          12 :             if (++done_count == 3) {
     419           4 :                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s: Finished", axis_string());
     420           4 :                 save_gains();
     421             :             }
     422             :         }
     423             :     }
     424             : 
     425         148 :     rpid.ff().set(FF);
     426         148 :     rpid.kP().set(P);
     427         148 :     rpid.kD().set(D);
     428         148 :     if (type == AUTOTUNE_ROLL) {  // for roll set I = smaller of FF or P
     429          67 :         rpid.kI().set(MIN(P, (FF / TRIM_TCONST)));
     430             :     } else {                      // for pitch/yaw naturally damped axes) set I usually = FF to get 1 sec I closure
     431          81 :         rpid.kI().set(MAX(P*AUTOTUNE_I_RATIO, (FF / TRIM_TCONST)));
     432             :     }
     433             : 
     434             :     // setup filters to be suitable for time constant and gyro filter
     435             :     // filtering T can  prevent P/D oscillation being seen, so allow the
     436             :     // user to switch it off
     437         148 :     if (!has_option(DISABLE_FLTT_UPDATE)) {
     438          80 :         rpid.filt_T_hz().set(10.0/(current.tau * 2 * M_PI));
     439             :     }
     440         148 :     rpid.filt_E_hz().set(0);
     441             :     // filtering D at the same level as VTOL can allow unwanted oscillations to be seen,
     442             :     // so allow the user to switch it off and select their own (usually lower) value
     443         148 :     if (!has_option(DISABLE_FLTD_UPDATE)) {
     444          80 :         rpid.filt_D_hz().set(AP::ins().get_gyro_filter_hz()*0.5);
     445             :     }
     446             : 
     447         148 :     current.FF = FF;
     448         148 :     current.P = P;
     449         148 :     current.I = rpid.kI().get();
     450         148 :     current.D = D;
     451             : 
     452         148 :     Debug("FPID=(%.3f, %.3f, %.3f, %.3f) Dmod=%.2f\n",
     453             :           rpid.ff().get(),
     454             :           rpid.kP().get(),
     455             :           rpid.kI().get(),
     456             :           rpid.kD().get(),
     457             :           min_Dmod);
     458             : 
     459             :     // move rmax and tau towards target
     460         148 :     update_rmax();
     461             : 
     462         148 :     state_change(new_state);
     463             : }
     464             : 
     465             : /*
     466             :   record a state change
     467             :  */
     468         148 : void AP_AutoTune::state_change(ATState new_state)
     469             : {
     470         148 :     min_Dmod = 1;
     471         148 :     max_Dmod = 0;
     472         148 :     max_SRate_P = 1;
     473         148 :     max_SRate_D = 1;
     474         148 :     max_P = max_D = 0;
     475         148 :     state = new_state;
     476         148 :     state_enter_ms = AP_HAL::millis();
     477         148 : }
     478             : 
     479             : /*
     480             :   save a float if it has changed
     481             :  */
     482          72 : void AP_AutoTune::save_float_if_changed(AP_Float &v, float old_value)
     483             : {
     484          72 :     if (!is_equal(old_value, v.get())) {
     485          28 :         v.save();
     486             :     }
     487          72 : }
     488             : 
     489             : /*
     490             :   save a int16_t if it has changed
     491             :  */
     492          16 : void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t old_value)
     493             : {
     494          16 :     if (old_value != v.get()) {
     495           7 :         v.save();
     496             :     }
     497          16 : }
     498             : 
     499             : 
     500             : /*
     501             :   save a set of gains
     502             :  */
     503           8 : void AP_AutoTune::save_gains(void)
     504             : {
     505           8 :     const auto &v = last_save;
     506           8 :     save_float_if_changed(current.tau, v.tau);
     507           8 :     save_int16_if_changed(current.rmax_pos, v.rmax_pos);
     508           8 :     save_int16_if_changed(current.rmax_neg, v.rmax_neg);
     509           8 :     save_float_if_changed(rpid.ff(), v.FF);
     510           8 :     save_float_if_changed(rpid.kP(), v.P);
     511           8 :     save_float_if_changed(rpid.kI(), v.I);
     512           8 :     save_float_if_changed(rpid.kD(), v.D);
     513           8 :     save_float_if_changed(rpid.kIMAX(), v.IMAX);
     514           8 :     save_float_if_changed(rpid.filt_T_hz(), v.flt_T);
     515           8 :     save_float_if_changed(rpid.filt_E_hz(), v.flt_E);
     516           8 :     save_float_if_changed(rpid.filt_D_hz(), v.flt_D);
     517           8 :     last_save = get_gains();
     518           8 : }
     519             : 
     520             : /*
     521             :   get gains with PID components
     522             :  */
     523          16 : AP_AutoTune::ATGains AP_AutoTune::get_gains(void)
     524             : {
     525          16 :     ATGains ret = current;
     526          16 :     ret.FF = rpid.ff();
     527          16 :     ret.P = rpid.kP();
     528          16 :     ret.I = rpid.kI();
     529          16 :     ret.D = rpid.kD();
     530          16 :     ret.IMAX = rpid.kIMAX();
     531          16 :     ret.flt_T = rpid.filt_T_hz();
     532          16 :     ret.flt_E = rpid.filt_E_hz();
     533          16 :     ret.flt_D = rpid.filt_D_hz();
     534          16 :     return ret;
     535             : }
     536             : 
     537             : /*
     538             :   set gains with PID components
     539             :  */
     540           4 : void AP_AutoTune::restore_gains(void)
     541             : {
     542           4 :     current = restore;
     543           4 :     rpid.ff().set(restore.FF);
     544           4 :     rpid.kP().set(restore.P);
     545           4 :     rpid.kI().set(restore.I);
     546           4 :     rpid.kD().set(restore.D);
     547           4 :     rpid.kIMAX().set(restore.IMAX);
     548           4 :     rpid.filt_T_hz().set(restore.flt_T);
     549           4 :     rpid.filt_E_hz().set(restore.flt_E);
     550           4 :     rpid.filt_D_hz().set(restore.flt_D);
     551           4 : }
     552             : 
     553             : /*
     554             :   update RMAX and TAU parameters on each step. We move them gradually
     555             :   towards the target to allow for a user going straight to a level 10
     556             :   tune while starting with a poorly tuned plane
     557             :  */
     558         156 : void AP_AutoTune::update_rmax(void)
     559             : {
     560         156 :     uint8_t level = constrain_int32(aparm.autotune_level, 0, ARRAY_SIZE(tuning_table));
     561             : 
     562             :     int16_t target_rmax;
     563             :     float target_tau;
     564             : 
     565         156 :     if (level == 0) {
     566             :         // this level means to keep current values of RMAX and TCONST
     567           0 :         target_rmax = constrain_float(current.rmax_pos, 20, 720);
     568           0 :         target_tau = constrain_float(current.tau, 0.1, 2);
     569             :     } else {
     570         156 :         target_rmax = tuning_table[level-1].rmax;
     571         156 :         target_tau = tuning_table[level-1].tau;
     572         156 :         if (type == AUTOTUNE_PITCH) {
     573             :             // 50% longer time constant on pitch
     574          85 :             target_tau *= 1.5;
     575             :         }
     576             :     }
     577             : 
     578         156 :     if (level > 0 && is_positive(current.FF)) {
     579         156 :         const float invtau = ((1.0f / target_tau) + (current.I / current.FF));
     580         156 :         if (is_positive(invtau)) {
     581         156 :             target_tau = MAX(target_tau,1.0f / invtau);
     582             :         }
     583             :     }
     584             : 
     585         156 :     if (current.rmax_pos == 0) {
     586             :         // conservative initial value
     587           2 :         current.rmax_pos.set(75);
     588             :     }
     589             :     // move RMAX by 20 deg/s per step
     590         156 :     current.rmax_pos.set(constrain_int32(target_rmax,
     591         156 :                                          current.rmax_pos.get()-20,
     592         156 :                                          current.rmax_pos.get()+20));
     593             : 
     594         156 :     if (level != 0 || current.rmax_neg.get() == 0) {
     595         156 :         current.rmax_neg.set(current.rmax_pos.get());
     596             :     }
     597             : 
     598             :     // move tau by max 15% per loop
     599         156 :     current.tau.set(constrain_float(target_tau,
     600             :                                     current.tau*0.85,
     601             :                                     current.tau*1.15));
     602         156 : }

Generated by: LCOV version 1.14