Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include <AP_Param/AP_Param.h>
       5             : #include <AP_Math/AP_Math.h>
       6             : #include <AP_Motors/AP_Motors.h>
       7             : #include <AP_Motors/AP_MotorsHeli_RSC.h>
       8             : #include <Filter/LowPassFilter.h>
       9             : #include <AC_PID/AC_P.h>
      10             : #include <AC_PID/AC_PID_Basic.h>
      11             : #include <AC_AttitudeControl/AC_AttitudeControl.h>
      12             : 
      13             : class AC_Autorotation
      14             : {
      15             : public:
      16             : 
      17             :     //Constructor
      18             :     AC_Autorotation(AP_MotorsHeli*& motors, AC_AttitudeControl*& att_crtl);
      19             : 
      20             :     void init(void);
      21             : 
      22        2340 :     bool enabled(void) const { return _param_enable.get() > 0; }
      23             : 
      24             :     // Init and run entry phase controller
      25             :     void init_entry(void);
      26             :     void run_entry(float pilot_norm_accel);
      27             : 
      28             :     // Init and run the glide phase controller
      29             :     void init_glide(void);
      30             :     void run_glide(float pilot_norm_accel);
      31             : 
      32             :     // Run the landed phase controller to zero the desired vels and accels
      33             :     void run_landed(void);
      34             : 
      35             :     // Arming checks for autorotation, mostly checking for miss-configurations
      36             :     bool arming_checks(size_t buflen, char *buffer) const;
      37             : 
      38             :     // Logging of lower rate autorotation specific variables
      39             :     void log_write_autorotation(void) const;
      40             : 
      41             :     // Returns true if we have met the autorotation-specific reasons to think we have landed
      42             :     bool check_landed(void);
      43             : 
      44             :     // Dynamically update time step used in autorotation controllers
      45             :     void set_dt(float delta_sec);
      46             : 
      47             :     // Helper to get measured head speed that has been normalised by head speed set point
      48             :     bool get_norm_head_speed(float& norm_rpm) const;
      49             : 
      50             :     // User Settable Parameters
      51             :     static const struct AP_Param::GroupInfo var_info[];
      52             : 
      53             :     static const uint32_t entry_time_ms = 2000; // (ms) Number of milliseconds that the entry phase operates for
      54             : 
      55             : private:
      56             : 
      57             :     // References to other libraries
      58             :     AP_MotorsHeli*&    _motors_heli;
      59             :     AC_AttitudeControl*& _attitude_control;
      60             : 
      61             :     // Calculates the forward ground speed in the horizontal plane
      62             :     float get_speed_forward_ms(void) const;
      63             : 
      64             :     // (s) Time step, updated dynamically from vehicle
      65             :     float _dt; 
      66             : 
      67             :     // Parameter values
      68             :     AP_Int8  _param_enable;
      69          83 :     AC_P _p_hs{1.0};
      70             :     AP_Float _param_head_speed_set_point;
      71             :     AP_Float _param_target_speed_ms;
      72             :     AP_Float _param_col_entry_cutoff_freq;
      73             :     AP_Float _param_col_glide_cutoff_freq;
      74             :     AP_Float _param_accel_max_mss;
      75             :     AP_Int8  _param_rpm_instance;
      76             :     AP_Float _param_fwd_k_ff;
      77             : 
      78             :     // Forward speed controller
      79             :     void update_forward_speed_controller(float pilot_norm_accel);
      80             :     AC_PID_Basic _fwd_speed_pid{2.0, 2.0, 0.2, 0.1, 4.0, 0.0, 10.0};  // PID object for vel to accel controller, Default values for kp, ki, kd, kff, imax, filt E Hz, filt D Hz
      81             :     bool _limit_accel;      // flag used for limiting integrator wind up if vehicle is against an accel or angle limit
      82             :     float _desired_vel_ms;  // (m/s) This is the velocity that we want.  This is the variable that is set by the invoking function to request a certain speed
      83             :     float _target_vel_ms;   // (m/s) This is the acceleration constrained velocity that we are allowed
      84             : 
      85             :     // Head speed controller variables
      86             :     void update_headspeed_controller(void);  // Update controller used to drive head speed with collective
      87             :     float _hs_accel;                         // The head speed target acceleration during the entry phase
      88             :     float _head_speed_error;                 // Error between target head speed and current head speed. Normalised by head speed set point RPM.
      89             :     float _target_head_speed;                // Normalised target head speed.  Normalised by head speed set point RPM.
      90             :     float _p_term_hs;                        // Proportional contribution to collective setting.
      91             :     float _ff_term_hs;                       // Following trim feed forward contribution to collective setting.
      92             :     LowPassFilterFloat col_trim_lpf;         // Low pass filter for collective trim
      93             : 
      94             :     // Flags used to check if we believe the aircraft has landed
      95             :     struct {
      96             :         bool min_speed;
      97             :         bool land_col;
      98             :         bool is_still;
      99             :     } _landed_reason;
     100             : 
     101             :     // Parameter accessors that provide value constraints
     102       57604 :     float get_accel_max_mss(void) const { return MAX(_param_accel_max_mss.get(), 0.5); }
     103             : };