Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file    AC_AttitudeControl_Sub.h
       4             : /// @brief   ArduSub attitude control library
       5             : 
       6             : #include "AC_AttitudeControl.h"
       7             : #include <AP_Motors/AP_MotorsMulticopter.h>
       8             : 
       9             : // default angle controller PID gains
      10             : // (Sub-specific defaults for parent class)
      11             : #define AC_ATC_SUB_ANGLE_P             6.0f
      12             : #define AC_ATC_SUB_ACCEL_Y_MAX_DEGSS   1100.0f
      13             : 
      14             : // default rate controller PID gains
      15             : #define AC_ATC_SUB_RATE_RP_P           0.135f
      16             : #define AC_ATC_SUB_RATE_RP_I           0.090f
      17             : #define AC_ATC_SUB_RATE_RP_D           0.0036f
      18             : #define AC_ATC_SUB_RATE_RP_IMAX        0.444f
      19             : #define AC_ATC_SUB_RATE_RP_FILT_HZ     30.0f
      20             : #define AC_ATC_SUB_RATE_YAW_P          0.180f
      21             : #define AC_ATC_SUB_RATE_YAW_I          0.018f
      22             : #define AC_ATC_SUB_RATE_YAW_D          0.0f
      23             : #define AC_ATC_SUB_RATE_YAW_IMAX       0.222f
      24             : #define AC_ATC_SUB_RATE_YAW_FILT_HZ    5.0f
      25             : 
      26             : #define MAX_YAW_ERROR                  radians(5)
      27             : 
      28             : class AC_AttitudeControl_Sub : public AC_AttitudeControl {
      29             : public:
      30             :     AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, AP_MotorsMulticopter& motors);
      31             : 
      32             :     // empty destructor to suppress compiler warning
      33          14 :     virtual ~AC_AttitudeControl_Sub() {}
      34             : 
      35             :     // pid accessors
      36     3072088 :     AC_PID& get_rate_roll_pid() override { return _pid_rate_roll; }
      37     3072084 :     AC_PID& get_rate_pitch_pid() override { return _pid_rate_pitch; }
      38     3072084 :     AC_PID& get_rate_yaw_pid() override { return _pid_rate_yaw; }
      39           0 :     const AC_PID& get_rate_roll_pid() const override { return _pid_rate_roll; }
      40           0 :     const AC_PID& get_rate_pitch_pid() const override { return _pid_rate_pitch; }
      41     2026868 :     const AC_PID& get_rate_yaw_pid() const override { return _pid_rate_yaw; }
      42             : 
      43             :     // Update Alt_Hold angle maximum
      44             :     void update_althold_lean_angle_max(float throttle_in) override;
      45             : 
      46             :     // Set output throttle
      47             :     void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
      48             : 
      49             :     // Calculate body-frame throttle required to produce the given earth-frame throttle input (accounts for vehicle tilt)
      50             :         float get_throttle_boosted(float throttle_in);
      51             : 
      52             :     // Set desired throttle vs attitude mixing (actual mix is slewed toward this value over 1~2 seconds)
      53             :     // Low values favor pilot/autopilot throttle over attitude control; high values prioritize attitude control
      54             :     // Has no effect when throttle is above hover throttle
      55           0 :     void set_throttle_mix_min() override { _throttle_rpy_mix_desired = _thr_mix_min; }
      56           0 :     void set_throttle_mix_man() override { _throttle_rpy_mix_desired = _thr_mix_man; }
      57           0 :     void set_throttle_mix_max(float ratio) override { _throttle_rpy_mix_desired = _thr_mix_max; }
      58             : 
      59             :     // Returns true if throttle mix is near minimum (i.e., attitude control is deprioritised)
      60           0 :     bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f*_thr_mix_min); }
      61             : 
      62             :     // run lowest level body-frame rate controller and send outputs to the motors
      63             :     void rate_controller_run() override;
      64             : 
      65             :     // sanity check parameters.  should be called once before take-off
      66             :     void parameter_sanity_check() override;
      67             : 
      68             :     // set the PID notch sample rates
      69             :     void set_notch_sample_rate(float sample_rate) override;
      70             : 
      71             :     // Sets desired roll, pitch, and yaw angles (in centidegrees), with yaw slewing.
      72             :     // Slews toward target yaw at a fixed rate (in centidegrees/s) until the error is within 5 degrees.
      73             :     // Used to enforce consistent heading changes without large instantaneous yaw errors.
      74             :     void input_euler_angle_roll_pitch_slew_yaw_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float slew_yaw_rate_cds);
      75             : 
      76             :     // user settable parameters
      77             :     static const struct AP_Param::GroupInfo var_info[];
      78             : 
      79             : protected:
      80             : 
      81             :     // Slews the throttle-to-attitude mix ratio (_throttle_rpy_mix) toward the requested value (_throttle_rpy_mix_desired).
      82             :     // Increases rapidly and decreases more slowly to ensure stability during transitions.
      83             :     void update_throttle_rpy_mix();
      84             : 
      85             :     // Returns a throttle value that accounts for the priority of attitude control over throttle.
      86             :     // This allows graceful reduction of control authority as thrust approaches its minimum.
      87             :     // returns a throttle including compensation for roll/pitch angle
      88             :     // throttle value should be 0 ~ 1
      89             :     float get_throttle_avg_max(float throttle_in);
      90             : 
      91             :     AP_MotorsMulticopter& _motors_multi;
      92             : 
      93             :     // Roll and Pitch rate PIDs share the same defaults:
      94             :     const AC_PID::Defaults rp_defaults {
      95             :         AC_PID::Defaults{
      96             :             .p         = AC_ATC_SUB_RATE_RP_P,
      97             :             .i         = AC_ATC_SUB_RATE_RP_I,
      98             :             .d         = AC_ATC_SUB_RATE_RP_D,
      99             :             .ff        = 0.0f,
     100             :             .imax      = AC_ATC_SUB_RATE_RP_IMAX,
     101             :             .filt_T_hz = AC_ATC_SUB_RATE_RP_FILT_HZ,
     102             :             .filt_E_hz = 0.0,
     103             :             .filt_D_hz = AC_ATC_SUB_RATE_RP_FILT_HZ,
     104             :             .srmax     = 0,
     105             :             .srtau     = 1.0
     106             :         }
     107             :     };
     108             :     AC_PID                _pid_rate_roll { rp_defaults };
     109             :     AC_PID                _pid_rate_pitch { rp_defaults };
     110             : 
     111             :     AC_PID                _pid_rate_yaw {
     112             :         AC_PID::Defaults{
     113             :             .p         = AC_ATC_SUB_RATE_YAW_P,
     114             :             .i         = AC_ATC_SUB_RATE_YAW_I,
     115             :             .d         = AC_ATC_SUB_RATE_YAW_D,
     116             :             .ff        = 0.0f,
     117             :             .imax      = AC_ATC_SUB_RATE_YAW_IMAX,
     118             :             .filt_T_hz = AC_ATC_SUB_RATE_YAW_FILT_HZ,
     119             :             .filt_E_hz = 0.0f,
     120             :             .filt_D_hz = AC_ATC_SUB_RATE_YAW_FILT_HZ,
     121             :             .srmax     = 0,
     122             :             .srtau     = 1.0
     123             :         }
     124             :     };
     125             : 
     126             :     AP_Float              _thr_mix_man;     // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle)
     127             :     AP_Float              _thr_mix_min;     // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
     128             :     AP_Float              _thr_mix_max;     // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
     129             : };