Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_HAL/AP_HAL_Boards.h>
       4             : 
       5             : #ifndef AP_QUICKTUNE_ENABLED
       6             : #define AP_QUICKTUNE_ENABLED 1 // NOTE: may be disabled by vehicle header
       7             : #endif
       8             : 
       9             : #if AP_QUICKTUNE_ENABLED
      10             : 
      11             : #include <AC_AttitudeControl/AC_AttitudeControl.h>
      12             : #include <AP_Param/AP_Param.h>
      13             : #include <RC_Channel/RC_Channel.h>
      14             : 
      15             : class AP_Quicktune {
      16             : public:
      17         692 :     AP_Quicktune()
      18         692 :     {
      19         692 :         AP_Param::setup_object_defaults(this, var_info);
      20         692 :     }
      21             : 
      22             :     // Empty destructor to suppress compiler warning
      23         192 :     virtual ~AP_Quicktune() {}
      24             : 
      25             :     /* Do not allow copies */
      26             :     CLASS_NO_COPY(AP_Quicktune);
      27             : 
      28             :     // Parameter block
      29             :     static const struct AP_Param::GroupInfo var_info[];
      30             : 
      31             :     void update(bool mode_supports_quicktune);
      32             :     void update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag);
      33             : 
      34             : private:
      35             : 
      36             :     // Parameters
      37             :     AP_Int8 enable;
      38             :     AP_Int8 axes_enabled;
      39             :     AP_Float double_time;
      40             :     AP_Float gain_margin;
      41             :     AP_Float osc_smax;
      42             :     AP_Float yaw_p_max;
      43             :     AP_Float yaw_d_max;
      44             :     AP_Float rp_pi_ratio;
      45             :     AP_Float y_pi_ratio;
      46             :     AP_Int8 auto_filter;
      47             :     AP_Float auto_save;
      48             :     AP_Float reduce_max;
      49             :     AP_Int16 options;
      50             :     AP_Int8 angle_max;
      51             : 
      52             :     // Low, Mid and High must be in the same positions as they are in RC_Channel::AuxSwitchPos
      53             :     enum class SwitchPos : uint8_t {
      54             :         LOW,
      55             :         MID,
      56             :         HIGH,
      57             :         NONE,
      58             :     };
      59             : 
      60             : 
      61             :     enum class AxisName : uint8_t {
      62             :         RLL = 0,
      63             :         PIT,
      64             :         YAW,
      65             :         DONE,
      66             :         END,
      67             :     };
      68             : 
      69             :     /*
      70             :       note! we rely on the enum being in the same order between axes
      71             :      */
      72             :     enum class Param : uint8_t {
      73             :         RLL_P = 0,
      74             :         RLL_I,
      75             :         RLL_D,
      76             :         RLL_SMAX,
      77             :         RLL_FLTT,
      78             :         RLL_FLTD,
      79             :         RLL_FLTE,
      80             :         RLL_FF,
      81             : 
      82             :         PIT_P,
      83             :         PIT_I,
      84             :         PIT_D,
      85             :         PIT_SMAX,
      86             :         PIT_FLTT,
      87             :         PIT_FLTD,
      88             :         PIT_FLTE,
      89             :         PIT_FF,
      90             : 
      91             :         YAW_P,
      92             :         YAW_I,
      93             :         YAW_D,
      94             :         YAW_SMAX,
      95             :         YAW_FLTT,
      96             :         YAW_FLTD,
      97             :         YAW_FLTE,
      98             :         YAW_FF,
      99             :         END,
     100             :     };
     101             : 
     102             :     static const uint8_t param_per_axis = uint8_t(Param::PIT_P) - uint8_t(Param::RLL_P);
     103             :     static_assert(uint8_t(Param::END) == 3*param_per_axis, "AP_Quicktune Param error");
     104             : 
     105             :     // Also the gains
     106             :     enum class Stage : uint8_t {
     107             :         D,
     108             :         P,
     109             :         DONE,
     110             :         I,
     111             :         FF,
     112             :         SMAX,
     113             :         FLTT,
     114             :         FLTD,
     115             :         FLTE,
     116             :         END,
     117             :     };
     118             : 
     119             :     // Time keeping
     120             :     uint32_t last_stage_change_ms;
     121             :     uint32_t last_gain_report_ms;
     122             :     uint32_t last_pilot_input_ms;
     123             :     uint32_t last_warning_ms;
     124             :     uint32_t tune_done_time_ms;
     125             : 
     126             :     // Bitmasks
     127             :     uint32_t axes_done;
     128             :     uint32_t filters_done;
     129             :     uint32_t param_changed; //Bitmask of changed parameters
     130             : 
     131             :     Stage current_stage = Stage::D;
     132             :     Param slew_parm = Param::END;
     133             :     uint8_t slew_steps;
     134             :     float slew_delta;
     135             :     SwitchPos sw_pos; //Switch pos to be set by aux func
     136             :     bool need_restore;
     137             :     float param_saved[uint8_t(Param::END)]; //Saved values of the parameters
     138             : 
     139             :     void reset_axes_done();
     140             :     void setup_filters(AxisName axis);
     141             :     bool have_pilot_input() const;
     142             :     AxisName get_current_axis() const;
     143             :     float get_slew_rate(AxisName axis) const;
     144             :     void advance_stage(AxisName axis);
     145             :     void adjust_gain(Param param, float value);
     146             :     void adjust_gain_limited(Param param, float value);
     147             :     float get_gain_mul() const;
     148             :     void restore_all_params();
     149             :     void save_all_params();
     150             :     Param get_pname(AxisName axis, Stage stage) const;
     151             :     AP_Float *get_param_pointer(Param param) const;
     152             :     float get_param_value(Param param) const;
     153             :     void set_param_value(Param param, float value);
     154             :     void set_and_save_param_value(Param param, float value);
     155             :     float gain_limit(Param param) const;
     156             :     AxisName get_axis(Param param) const;
     157             :     float limit_gain(Param param, float value);
     158             :     const char* get_param_name(Param param) const;
     159             :     Stage get_stage(Param param) const;
     160             :     const char* get_axis_name(AxisName axis) const;
     161             :     AC_PID *get_axis_pid(AxisName axis) const;
     162             :     void Write_QWIK(float SRate, float Gain, Param param);
     163             : 
     164             :     void abort_tune(void);
     165             : };
     166             : 
     167             : #endif  // AP_QUICKTUNE_ENABLED