Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file    AC_AttitudeControl_Multi.h
       4             : /// @brief   ArduCopter attitude control library
       5             : 
       6             : #include "AC_AttitudeControl.h"
       7             : #include <AP_Motors/AP_MotorsMulticopter.h>
       8             : 
       9             : // default rate controller PID gains
      10             : #ifndef AC_ATC_MULTI_RATE_RP_P
      11             :   # define AC_ATC_MULTI_RATE_RP_P           0.135f
      12             : #endif
      13             : #ifndef AC_ATC_MULTI_RATE_RP_I
      14             :   # define AC_ATC_MULTI_RATE_RP_I           0.135f
      15             : #endif
      16             : #ifndef AC_ATC_MULTI_RATE_RP_D
      17             :   # define AC_ATC_MULTI_RATE_RP_D           0.0036f
      18             : #endif
      19             : #ifndef AC_ATC_MULTI_RATE_RP_IMAX
      20             :  # define AC_ATC_MULTI_RATE_RP_IMAX         0.5f
      21             : #endif
      22             : #ifndef AC_ATC_MULTI_RATE_RPY_FILT_HZ
      23             :  # define AC_ATC_MULTI_RATE_RPY_FILT_HZ      20.0f
      24             : #endif
      25             : #ifndef AC_ATC_MULTI_RATE_YAW_P
      26             :  # define AC_ATC_MULTI_RATE_YAW_P           0.180f
      27             : #endif
      28             : #ifndef AC_ATC_MULTI_RATE_YAW_I
      29             :  # define AC_ATC_MULTI_RATE_YAW_I           0.018f
      30             : #endif
      31             : #ifndef AC_ATC_MULTI_RATE_YAW_D
      32             :  # define AC_ATC_MULTI_RATE_YAW_D           0.0f
      33             : #endif
      34             : #ifndef AC_ATC_MULTI_RATE_YAW_IMAX
      35             :  # define AC_ATC_MULTI_RATE_YAW_IMAX        0.5f
      36             : #endif
      37             : #ifndef AC_ATC_MULTI_RATE_YAW_FILT_HZ
      38             :  # define AC_ATC_MULTI_RATE_YAW_FILT_HZ     2.5f
      39             : #endif
      40             : 
      41             : 
      42             : class AC_AttitudeControl_Multi : public AC_AttitudeControl {
      43             : public:
      44             :         AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, AP_MotorsMulticopter& motors);
      45             : 
      46             :         // empty destructor to suppress compiler warning
      47           0 :         virtual ~AC_AttitudeControl_Multi() {}
      48             : 
      49             :     // pid accessors
      50    77185011 :     AC_PID& get_rate_roll_pid() override { return _pid_rate_roll; }
      51    77184913 :     AC_PID& get_rate_pitch_pid() override { return _pid_rate_pitch; }
      52   151938477 :     AC_PID& get_rate_yaw_pid() override { return _pid_rate_yaw; }
      53           0 :     const AC_PID& get_rate_roll_pid() const override { return _pid_rate_roll; }
      54           0 :     const AC_PID& get_rate_pitch_pid() const override { return _pid_rate_pitch; }
      55    57505212 :     const AC_PID& get_rate_yaw_pid() const override { return _pid_rate_yaw; }
      56             : 
      57             :     // Update Alt_Hold angle maximum
      58             :     void update_althold_lean_angle_max(float throttle_in) override;
      59             : 
      60             :     // Set output throttle
      61             :     void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
      62             : 
      63             :     // Calculate body-frame throttle required to produce the given earth-frame throttle input (accounts for vehicle tilt)
      64             :     float get_throttle_boosted(float throttle_in);
      65             : 
      66             :     // Set desired throttle vs attitude mixing (actual mix is slewed toward this value over 1~2 seconds)
      67             :     // Low values favor pilot/autopilot throttle over attitude control; high values prioritize attitude control
      68             :     // Has no effect when throttle is above hover throttle
      69     2316276 :     void set_throttle_mix_min() override { _throttle_rpy_mix_desired = _thr_mix_min; }
      70       24805 :     void set_throttle_mix_man() override { _throttle_rpy_mix_desired = _thr_mix_man; }
      71             :     void set_throttle_mix_max(float ratio) override;
      72      199088 :     void set_throttle_mix_value(float value) override { _throttle_rpy_mix_desired = _throttle_rpy_mix = value; }
      73      205625 :     float get_throttle_mix(void) const override { return _throttle_rpy_mix; }
      74             : 
      75             :     // Returns true if throttle mix is near minimum (i.e., attitude control is deprioritised)
      76    17055492 :     bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f * _thr_mix_min); }
      77             : 
      78             :     // run lowest level body-frame rate controller and send outputs to the motors
      79             :     void rate_controller_run_dt(const Vector3f& gyro_rads, float dt) override;
      80             :     void rate_controller_run() override;
      81             : 
      82             :     // sanity check parameters.  should be called once before take-off
      83             :     void parameter_sanity_check() override;
      84             : 
      85             :     // set the PID notch sample rates
      86             :     void set_notch_sample_rate(float sample_rate) override;
      87             : 
      88             :     // user settable parameters
      89             :     static const struct AP_Param::GroupInfo var_info[];
      90             : 
      91             : protected:
      92             : 
      93             :     // Boosts angle controller gains during rapid throttle changes to improve responsiveness
      94             :     // boost angle_p/pd each cycle on high throttle slew
      95             :     void update_throttle_gain_boost();
      96             : 
      97             :     // Slews the current throttle-to-attitude mix ratio toward the target (_throttle_rpy_mix_desired)
      98             :     void update_throttle_rpy_mix();
      99             : 
     100             :     // Get throttle limit based on priority of attitude vs throttle control (used for blending during low thrust)
     101             :     float get_throttle_avg_max(float throttle_in);
     102             : 
     103             :     AP_MotorsMulticopter& _motors_multi;
     104             : 
     105             :     // Roll rate PID controller (used for body-frame angular rate control)
     106             :     AC_PID                _pid_rate_roll {
     107             :         AC_PID::Defaults{
     108             :             .p         = AC_ATC_MULTI_RATE_RP_P,
     109             :             .i         = AC_ATC_MULTI_RATE_RP_I,
     110             :             .d         = AC_ATC_MULTI_RATE_RP_D,
     111             :             .ff        = 0.0f,
     112             :             .imax      = AC_ATC_MULTI_RATE_RP_IMAX,
     113             :             .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
     114             :             .filt_E_hz = 0.0f,
     115             :             .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
     116             :             .srmax     = 0,
     117             :             .srtau     = 1.0
     118             :         }
     119             :     };
     120             :     // Pitch rate PID controller (used for body-frame angular rate control)
     121             :     AC_PID                _pid_rate_pitch{
     122             :         AC_PID::Defaults{
     123             :             .p         = AC_ATC_MULTI_RATE_RP_P,
     124             :             .i         = AC_ATC_MULTI_RATE_RP_I,
     125             :             .d         = AC_ATC_MULTI_RATE_RP_D,
     126             :             .ff        = 0.0f,
     127             :             .imax      = AC_ATC_MULTI_RATE_RP_IMAX,
     128             :             .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
     129             :             .filt_E_hz = 0.0f,
     130             :             .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
     131             :             .srmax     = 0,
     132             :             .srtau     = 1.0
     133             :         }
     134             :     };
     135             :     // Yaw rate PID controller (used for body-frame angular rate control)
     136             :     AC_PID                _pid_rate_yaw{
     137             :         AC_PID::Defaults{
     138             :             .p         = AC_ATC_MULTI_RATE_YAW_P,
     139             :             .i         = AC_ATC_MULTI_RATE_YAW_I,
     140             :             .d         = AC_ATC_MULTI_RATE_YAW_D,
     141             :             .ff        = 0.0f,
     142             :             .imax      = AC_ATC_MULTI_RATE_YAW_IMAX,
     143             :             .filt_T_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
     144             :             .filt_E_hz = AC_ATC_MULTI_RATE_YAW_FILT_HZ,
     145             :             .filt_D_hz = AC_ATC_MULTI_RATE_RPY_FILT_HZ,
     146             :             .srmax     = 0,
     147             :             .srtau     = 1.0
     148             :         }
     149             :     };
     150             : 
     151             :     AP_Float              _thr_mix_man;     // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle)
     152             :     AP_Float              _thr_mix_min;     // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
     153             :     AP_Float              _thr_mix_max;     // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
     154             : 
     155             :     // angle_p/pd boost multiplier
     156             :     AP_Float              _throttle_gain_boost;
     157             : };