Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Logger/LogStructure.h>
       4             : #include <AP_Param/AP_Param.h>
       5             : #include <AP_Vehicle/AP_FixedWing.h>
       6             : #include <Filter/SlewLimiter.h>
       7             : 
       8             : #include <Filter/ModeFilter.h>
       9             : 
      10             : class AP_AutoTune
      11             : {
      12             : public:
      13             :     struct ATGains {
      14             :         AP_Float tau;
      15             :         AP_Int16 rmax_pos;
      16             :         AP_Int16 rmax_neg;
      17             :         float FF, P, I, D, IMAX;
      18             :         float flt_T, flt_E, flt_D;
      19             :     };
      20             : 
      21             :     enum ATType {
      22             :         AUTOTUNE_ROLL  = 0,
      23             :         AUTOTUNE_PITCH = 1,
      24             :         AUTOTUNE_YAW = 2,
      25             :     };
      26             : 
      27             :     enum Options {
      28             :         DISABLE_FLTD_UPDATE = 0,
      29             :         DISABLE_FLTT_UPDATE = 1
      30             :     };
      31             : 
      32             :     struct PACKED log_ATRP {
      33             :         LOG_PACKET_HEADER;
      34             :         uint64_t time_us;
      35             :         uint8_t type;
      36             :         uint8_t state;
      37             :         float actuator;
      38             :         float P_slew;
      39             :         float D_slew;
      40             :         float FF_single;
      41             :         float FF;
      42             :         float P;
      43             :         float I;
      44             :         float D;
      45             :         uint8_t action;
      46             :         float rmax;
      47             :         float tau;
      48             :     };
      49             : 
      50             :     // constructor
      51             :     AP_AutoTune(ATGains &_gains, ATType type, const AP_FixedWing &parms, class AC_PID &rpid);
      52             : 
      53             :     // called when autotune mode is entered
      54             :     void start(void);
      55             : 
      56             :     // called to stop autotune and restore gains when user leaves
      57             :     // autotune
      58             :     void stop(void);
      59             : 
      60             :     // update called whenever autotune mode is active. This is
      61             :     // called at the main loop rate
      62             :     void update(struct AP_PIDInfo &pid_info, float scaler, float angle_err_deg);
      63             : 
      64             :     // are we running?
      65             :     bool running;
      66             : 
      67             :     static const char *axis_string(ATType _type);
      68             : 
      69             : private:
      70             :     // the current gains
      71             :     ATGains &current;
      72             :     class AC_PID &rpid;
      73             : 
      74             :     // what type of autotune is this
      75             :     ATType type;
      76             : 
      77             :     const AP_FixedWing &aparm;
      78             : 
      79             :     // values to restore if we leave autotune mode
      80             :     ATGains restore;
      81             :     ATGains last_save;
      82             : 
      83             :     // last logging time
      84             :     uint32_t last_log_ms;
      85             : 
      86             :     // the demanded/achieved state
      87             :     enum class ATState {IDLE,
      88             :                         DEMAND_POS,
      89             :                         DEMAND_NEG
      90             :                        };
      91             :     ATState state;
      92             : 
      93             :     // the demanded/achieved state
      94             :     enum class Action {NONE,
      95             :                        LOW_RATE,
      96             :                        SHORT,
      97             :                        RAISE_PD,
      98             :                        LOWER_PD,
      99             :                        IDLE_LOWER_PD,
     100             :                        RAISE_D,
     101             :                        RAISE_P,
     102             :                        LOWER_D,
     103             :                        LOWER_P
     104             :                       };
     105             :     Action action;
     106             : 
     107             :     // when we entered the current state
     108             :     uint32_t state_enter_ms;
     109             : 
     110             :     void check_state_exit(uint32_t state_time_ms);
     111             :     void save_gains(void);
     112             : 
     113             :     void save_float_if_changed(AP_Float &v, float value);
     114             :     void save_int16_if_changed(AP_Int16 &v, int16_t value);
     115             :     void state_change(ATState newstate);
     116             :     const char *axis_string(void) const;
     117             : 
     118             :     // get gains with PID components
     119             :     ATGains get_gains(void);
     120             :     void restore_gains(void);
     121             : 
     122             :     // update rmax and tau towards target
     123             :     void update_rmax();
     124             : 
     125         296 :     bool has_option(Options option) {
     126         296 :         return (aparm.autotune_options.get() & uint32_t(1<<uint32_t(option))) != 0;
     127             :     }
     128             : 
     129             :     // 5 point mode filter for FF estimate
     130             :     ModeFilterFloat_Size5 ff_filter;
     131             : 
     132             :     LowPassFilterConstDtFloat actuator_filter;
     133             :     LowPassFilterConstDtFloat rate_filter;
     134             :     LowPassFilterConstDtFloat target_filter;
     135             : 
     136             :     // separate slew limiters for P and D
     137             :     float slew_limit_max, slew_limit_tau;
     138             :     SlewLimiter slew_limiter_P{slew_limit_max, slew_limit_tau};
     139             :     SlewLimiter slew_limiter_D{slew_limit_max, slew_limit_tau};
     140             : 
     141             :     float max_actuator;
     142             :     float min_actuator;
     143             :     float max_rate;
     144             :     float min_rate;
     145             :     float max_target;
     146             :     float min_target;
     147             :     float max_P;
     148             :     float max_D;
     149             :     float min_Dmod;
     150             :     float max_Dmod;
     151             :     float max_SRate_P;
     152             :     float max_SRate_D;
     153             :     float FF_single;
     154             :     uint16_t ff_count;
     155             :     float dt;
     156             :     float D_limit;
     157             :     float P_limit;
     158             :     uint32_t D_set_ms;
     159             :     uint32_t P_set_ms;
     160             :     uint8_t done_count;
     161             : };