Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file    AC_AttitudeControl_Heli.h
       4             : /// @brief   ArduCopter attitude control library for traditional helicopters
       5             : 
       6             : #include "AC_AttitudeControl.h"
       7             : #include <AP_Motors/AP_MotorsHeli.h>
       8             : #include <AC_PID/AC_HELI_PID.h>
       9             : #include <Filter/Filter.h>
      10             : 
      11             : // default rate controller PID gains
      12             : #define AC_ATC_HELI_RATE_RP_P                       0.024f
      13             : #define AC_ATC_HELI_RATE_RP_I                       0.15f
      14             : #define AC_ATC_HELI_RATE_RP_D                       0.001f
      15             : #define AC_ATC_HELI_RATE_RP_IMAX                    0.4f
      16             : #define AC_ATC_HELI_RATE_RP_FF                      0.15f
      17             : #define AC_ATC_HELI_RATE_RP_FILT_HZ                 20.0f
      18             : #define AC_ATC_HELI_RATE_RP_ILMI                    0.05f  // Default I-term Leak Minimum
      19             : #define AC_ATC_HELI_RATE_YAW_P                      0.18f
      20             : #define AC_ATC_HELI_RATE_YAW_I                      0.12f
      21             : #define AC_ATC_HELI_RATE_YAW_D                      0.003f
      22             : #define AC_ATC_HELI_RATE_YAW_IMAX                   0.4f
      23             : #define AC_ATC_HELI_RATE_YAW_FF                     0.024f
      24             : #define AC_ATC_HELI_RATE_YAW_FILT_HZ                20.0f
      25             : 
      26             : #define AC_ATTITUDE_HELI_ANGLE_LIMIT_THROTTLE_MAX   0.95f    // Heli's use 95% of max collective before limiting frame angle
      27             : #define AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE  0.02f
      28             : #define AC_ATTITUDE_HELI_RATE_RP_FF_FILTER          20.0f
      29             : #define AC_ATTITUDE_HELI_RATE_Y_FF_FILTER          20.0f
      30             : #define AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT    300
      31             : #define AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD   radians(30.0f)
      32             : #define AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME    3.0f
      33             : 
      34             : class AC_AttitudeControl_Heli : public AC_AttitudeControl {
      35             : public:
      36             :     AC_AttitudeControl_Heli( AP_AHRS_View &ahrs,
      37             :                         AP_MotorsHeli& motors);
      38             : 
      39             :     // pid accessors
      40     1946411 :     AC_PID& get_rate_roll_pid() override { return _pid_rate_roll; }
      41     1946400 :     AC_PID& get_rate_pitch_pid() override { return _pid_rate_pitch; }
      42    10622285 :     AC_PID& get_rate_yaw_pid() override { return _pid_rate_yaw; }
      43           0 :     const AC_PID& get_rate_roll_pid() const override { return _pid_rate_roll; }
      44           0 :     const AC_PID& get_rate_pitch_pid() const override { return _pid_rate_pitch; }
      45     6397068 :     const AC_PID& get_rate_yaw_pid() const override { return _pid_rate_yaw; }
      46             : 
      47             :         // rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
      48             :         // should be called at 100hz or more
      49             :         virtual void rate_controller_run() override;
      50             : 
      51             :     // Update Alt_Hold angle maximum
      52             :     void update_althold_lean_angle_max(float throttle_in) override;
      53             : 
      54             :         // use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage
      55     3233979 :         void use_leaky_i(bool leaky_i) override {  _flags_heli.leaky_i = leaky_i; }
      56             :     
      57             :     // set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition.
      58     3233979 :     void set_hover_roll_trim_scalar(float scalar) override {_hover_roll_trim_scalar = constrain_float(scalar, 0.0f, 1.0f);}
      59             : 
      60             :     // get_roll_trim - angle in centi-degrees to be added to roll angle for learn hover collective. Used by helicopter to counter tail rotor thrust in hover
      61             :     float get_roll_trim_cd() override;
      62             : 
      63             :     // Set output throttle
      64             :     void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
      65             : 
      66             :     // calculate total body frame throttle required to produce the given earth frame throttle
      67             :     float get_throttle_boosted(float throttle_in);
      68             : 
      69             :     // Sets desired roll and pitch angles (in radians) and yaw rate (in radians/s).
      70             :     // Used when roll/pitch stabilization is needed with manual or autonomous yaw rate control.
      71             :     // Applies acceleration-limited input shaping for smooth transitions and computes body-frame angular velocity targets.
      72             :     void input_euler_angle_roll_pitch_euler_rate_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_rate_rads) override;
      73             : 
      74             :     // Sets desired roll, pitch, and yaw angles (in radians).
      75             :     // Used to follow an absolute attitude setpoint. Input shaping and yaw slew limits are applied.
      76             :     // Outputs are passed to the rate controller via shaped angular velocity targets.
      77             :     void input_euler_angle_roll_pitch_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_angle_rad, bool slew_yaw) override;
      78             :     
      79             :     // Sets desired thrust vector and heading rate (in radians/s).
      80             :     // Used for tilt-based navigation with independent yaw control.
      81             :     // The thrust vector defines the desired orientation (e.g., pointing direction for vertical thrust),
      82             :     // while the heading rate adjusts yaw. The input is shaped by acceleration and slew limits.
      83             :     void input_thrust_vector_rate_heading_rads(const Vector3f& thrust_vector, float heading_rate_rads, bool slew_yaw = true) override;
      84             :     
      85             :     // Sets desired thrust vector and heading (in radians) with heading rate (in radians/s).
      86             :     // Used for advanced attitude control where thrust direction is separated from yaw orientation.
      87             :     // Heading slew is constrained based on configured limits.
      88             :     void input_thrust_vector_heading_rad(const Vector3f& thrust_vector, float heading_angle_rad, float heading_rate_rads) override;
      89             : 
      90             :     // enable/disable inverted flight
      91          50 :     void set_inverted_flight(bool inverted) override { _inverted_flight = inverted; }
      92             : 
      93             :     // accessor for inverted flight flag
      94     3868725 :     bool get_inverted_flight() override { return _inverted_flight; }
      95             : 
      96             :     // set the PID notch sample rates
      97             :     void set_notch_sample_rate(float sample_rate) override;
      98             : 
      99             :     // user settable parameters
     100             :     static const struct AP_Param::GroupInfo var_info[];
     101             : 
     102             : private:
     103             : 
     104             :     // To-Do: move these limits flags into the heli motors class
     105             :     struct AttControlHeliFlags {
     106             :         uint8_t leaky_i             :   1;  // 1 if we should use leaky i term for body-frame rate to motor stage
     107             :     } _flags_heli;
     108             : 
     109             :     // true in inverted flight mode
     110             :     bool _inverted_flight;
     111             : 
     112             :     //
     113             :     // body-frame rate controller
     114             :     //
     115             :         // rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in radians/sec) for roll, pitch and yaw
     116             :     // outputs are sent directly to motor class
     117             :     void rate_bf_to_motor_roll_pitch(const Vector3f &rate_rads, float rate_roll_target_rads, float rate_pitch_target_rads);
     118             :     float rate_target_to_motor_yaw(float rate_yaw_actual_rads, float rate_yaw_rads);
     119             : 
     120             :     //
     121             :     // throttle methods
     122             :     //
     123             :     
     124             :     // internal variables
     125             :     float _hover_roll_trim_scalar = 0;              // scalar used to suppress Hover Roll Trim
     126             : 
     127             : 
     128             :     // This represents an euler axis-angle rotation vector from the vehicles
     129             :     // estimated attitude to the reference (setpoint) attitude used in the attitude
     130             :     // controller, in radians in the vehicle body frame of reference.
     131             :     Vector3f            _att_error_rot_vec_rad;
     132             : 
     133             :     // parameters
     134             :     AP_Int8         _piro_comp_enabled;             // Flybar present or not.  Affects attitude controller used during ACRO flight mode
     135             :     AP_Int16        _hover_roll_trim_cd;               // Angle in centi-degrees used to counter tail rotor thrust in hover
     136             : 
     137             :     // Roll and Pitch rate PIDs share the same defaults:
     138             :     const AC_PID::Defaults rp_defaults {
     139             :         AC_PID::Defaults{
     140             :             .p         = AC_ATC_HELI_RATE_RP_P,
     141             :             .i         = AC_ATC_HELI_RATE_RP_I,
     142             :             .d         = AC_ATC_HELI_RATE_RP_D,
     143             :             .ff        = AC_ATC_HELI_RATE_RP_FF,
     144             :             .imax      = AC_ATC_HELI_RATE_RP_IMAX,
     145             :             .filt_T_hz = AC_ATTITUDE_HELI_RATE_RP_FF_FILTER,
     146             :             .filt_E_hz = AC_ATC_HELI_RATE_RP_FILT_HZ,
     147             :             .filt_D_hz = 0.0,
     148             :             .srmax     = 0,
     149             :             .srtau     = 1.0
     150             :         }
     151             :     };
     152             :     AC_HELI_PID     _pid_rate_roll { rp_defaults, AC_ATC_HELI_RATE_RP_ILMI };
     153             :     AC_HELI_PID     _pid_rate_pitch { rp_defaults, AC_ATC_HELI_RATE_RP_ILMI };
     154             : 
     155             :     AC_HELI_PID     _pid_rate_yaw {
     156             :         AC_PID::Defaults{
     157             :             .p         = AC_ATC_HELI_RATE_YAW_P,
     158             :             .i         = AC_ATC_HELI_RATE_YAW_I,
     159             :             .d         = AC_ATC_HELI_RATE_YAW_D,
     160             :             .ff        = AC_ATC_HELI_RATE_YAW_FF,
     161             :             .imax      = AC_ATC_HELI_RATE_YAW_IMAX,
     162             :             .filt_T_hz = AC_ATTITUDE_HELI_RATE_Y_FF_FILTER,
     163             :             .filt_E_hz = AC_ATC_HELI_RATE_YAW_FILT_HZ,
     164             :             .filt_D_hz = 0.0,
     165             :             .srmax     = 0,
     166             :             .srtau     = 1.0
     167             :         }
     168             :         , 0.0f  // default_ilmi
     169             :     };
     170             :     
     171             : };