Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include <AP_Math/AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
       5             : #include <RC_Channel/RC_Channel.h>
       6             : #include <SRV_Channel/SRV_Channel.h>
       7             : #include <AP_Logger/AP_Logger_config.h>
       8             : #include <AC_Autorotation/RSC_Autorotation.h>
       9             : #include "AP_Motors_Class.h"
      10             : 
      11             : // rotor control modes
      12             : enum RotorControlMode {
      13             :     ROTOR_CONTROL_MODE_DISABLED = 0,
      14             :     ROTOR_CONTROL_MODE_PASSTHROUGH,
      15             :     ROTOR_CONTROL_MODE_SETPOINT,
      16             :     ROTOR_CONTROL_MODE_THROTTLECURVE,
      17             :     ROTOR_CONTROL_MODE_AUTOTHROTTLE,
      18             :     ROTOR_CONTROL_MODE_DDFP
      19             : };
      20             : 
      21             : class AP_MotorsHeli_RSC {
      22             : public:
      23             :     friend class AP_MotorsHeli_Single;
      24             :     friend class AP_MotorsHeli_Dual;
      25             :     friend class AP_MotorsHeli_Quad;
      26             : 
      27         163 :     AP_MotorsHeli_RSC(SRV_Channel::Function aux_fn,
      28             :                       uint8_t default_channel,
      29         163 :                       uint8_t inst) :
      30         163 :         _instance(inst),
      31         163 :         _aux_fn(aux_fn),
      32         978 :         _default_channel(default_channel)
      33             :     {
      34         163 :         AP_Param::setup_object_defaults(this, var_info);
      35         163 :     };
      36             : 
      37             :     using DesiredRSCSpoolState = AP_Motors::DesiredSpoolState;
      38             :     using RSCSpoolState = AP_Motors::SpoolState;
      39             : 
      40             :     // initialize - servo initialization and parameter setup on start-up
      41             :     void        initialize();
      42             : 
      43             :     // configure - configures RSC for current control mode, should be called on each update of control mode and on start-up after initialize() to setup parameters and scalars for the current control mode
      44             :     void        configure();
      45             : 
      46             :     // configure - configure the RSC with specific settings, allows caller to specify settings instead of using parameters.
      47             :     void        configure(RotorControlMode control_mode,
      48             :                           int8_t ramp_time,
      49             :                           int8_t runup_time,
      50             :                           float critical_speed,
      51             :                           float idle_output);
      52             : 
      53             :     // configure - configures the parameters for the armed state, should be called on each update of control mode and on start-up after initialize() to setup parameters and scalars for the current control mode
      54             :     void        configure_armed();
      55             : 
      56             :     // update - runs RSC logic and outputs to motors, returns current spool state for use in motor output logic
      57             :     void        update(float dt);
      58             : 
      59             :     // update_spool_state - updates the spool state machine based on the desired spool state and current spool state
      60             :     RSCSpoolState   update_spool_state(DesiredRSCSpoolState desired_spool_state, float dt);
      61             : 
      62             :     // output_to_servo - outputs pwm onto output rsc channel.
      63     6286516 :     void        output_to_servo() { write_rsc(_control_output);}
      64             : 
      65             :     // get_rsc_control_mode - gets RSC control mode
      66     3839649 :     uint8_t     get_rsc_control_mode() const { return _rsc_control_mode; }
      67             : 
      68             :     // get_desired_rotor_speed
      69     1970949 :     float       get_desired_rotor_speed() const { return _desired_rotor_speed; }
      70             : 
      71             :     // set_passthru_desired_rotor_speed - this requires input to be 0-1
      72           0 :     void        set_passthru_desired_rotor_speed(float desired_rotor_speed) { _passthru_desired_rotor_speed = desired_rotor_speed; }
      73             : 
      74             :     // set_setpoint_desired_rotor_speed - this requires input to be 0-1
      75       17503 :     void        set_setpoint_desired_rotor_speed(float desired_rotor_speed) { _setpoint_desired_rotor_speed = desired_rotor_speed * 0.01f; }
      76             : 
      77             :     // set_ddfp_desired_rotor_speed - this requires input to be 0-1
      78      151002 :     void        set_ddfp_desired_rotor_speed(float desired_rotor_speed) { _ddfp_desired_rotor_speed = desired_rotor_speed; }
      79             : 
      80             :     // set_collective. collective for throttle curve calculation
      81     3234119 :     void        set_collective(float collective) { _collective_in = collective; }
      82             : 
      83             :     // true if we are considered to be autorotating or bailing out of an autorotation
      84             :     bool        in_autorotation(void) const;
      85             : 
      86             :     // true if we are using a manual collective flight mode
      87     3233979 :     void        set_using_manual_collective_mode(bool using_manual_collective_mode) { _using_manual_collective_mode = using_manual_collective_mode; }
      88             : 
      89             :     // turbine start initialize sequence
      90     3233979 :     void        set_turbine_start(bool turbine_start) {_turbine_start = turbine_start; }
      91             : 
      92             :     // Return mask of output channels which the RSC is outputting on
      93             :     uint32_t    get_output_mask() const;
      94             : 
      95             : #if HAL_LOGGING_ENABLED
      96             :     // RSC logging
      97             :     void write_log(void) const;
      98             : #endif
      99             : 
     100             :     RSC_Autorotation autorotation;
     101             : 
     102             :     // var_info for holding Parameter information
     103             :     static const struct AP_Param::GroupInfo var_info[];
     104             : 
     105             :     // parameters
     106             :     AP_Int16        _rsc_setpoint;            // rotor speed when RSC mode is set to is enabled
     107             :     AP_Int8         _rsc_mode;                // Which main rotor ESC control mode is active
     108             :     AP_Int8         _ramp_time;               // Time in seconds for the output to the main rotor's ESC to reach setpoint
     109             :     AP_Int8         _runup_time;              // Time in seconds for the main rotor to reach full speed.  Must be longer than _rsc_ramp_time
     110             :     AP_Int16        _critical_speed;          // Rotor speed below which flight is not possible
     111             :     AP_Int16        _idle_output;             // Rotor control output while at idle
     112             : 
     113             : private:
     114             : 
     115             :     // set_rsc_control_mode - sets RSC control mode
     116     1772391 :     void            set_rsc_control_mode(RotorControlMode mode) { _rsc_control_mode = mode; }
     117             : 
     118             :     // reset_rsc_mode_param - resets rsc mode param to current control mode
     119           3 :     void            reset_rsc_mode_param() { _rsc_mode.set((uint8_t)_rsc_control_mode); }
     120             : 
     121             :     // set_critical_speed
     122     1772391 :     void            set_critical_speed(float critical_speed) { _critical_speed.set(critical_speed); }
     123             : 
     124             :     // functions for autothrottle, throttle curve, governor, idle speed, output to servo
     125             :     void            set_governor_output(float governor_output) {_governor_output = governor_output; }
     126             :     void            governor_reset();
     127     3182009 :     float           get_control_output() const { return _control_output; }
     128     1772391 :     void            set_idle_output(float idle_output) { _idle_output.set(idle_output); }
     129             :     void            autothrottle_run();
     130             :     void            set_throttle_curve();
     131             : 
     132             :     // functions for ramp and runup timers
     133     1772391 :     void            set_ramp_time(int8_t ramp_time) { _ramp_time.set(ramp_time); }
     134     1772391 :     void            set_runup_time(int8_t runup_time) { _runup_time.set(runup_time); }
     135             : 
     136             :     // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
     137             :     void            update_rotor_ramp(float rotor_ramp_input, float dt);
     138             : 
     139             :     // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
     140             :     void            update_rotor_runup(float dt);
     141             : 
     142             :     // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1
     143             :     void            write_rsc(float servo_out);
     144             : 
     145             :     // calculate_throttlecurve - uses throttle curve and collective input to determine throttle setting
     146             :     float           calculate_throttlecurve(float collective_in);
     147             : 
     148             :     // rotor_speed_above_critical - return true if rotor speed is above that critical for flight
     149     2250083 :     bool            rotor_speed_above_critical(void) const { return _rotor_runup_output >= get_critical_speed(); }
     150             : 
     151             :     uint64_t        _last_update_us;
     152             :     const uint8_t   _instance;
     153             : 
     154             :     // channel setup for aux function
     155             :     const SRV_Channel::Function _aux_fn;
     156             :     const uint8_t  _default_channel;
     157             : 
     158             :     RSCSpoolState          _spool_state;               // current spool state
     159             :     DesiredRSCSpoolState   _desired_spool_state;       // desired spool state
     160             : 
     161             :     // internal variables
     162             :     RotorControlMode _rsc_control_mode = ROTOR_CONTROL_MODE_DISABLED;   // RSC control mode, Passthrough, Setpoint, Throttle Curve or Autothrottle
     163             :     float           _passthru_desired_rotor_speed;// latest pilot desired rotor speed, used for passthrough mode
     164             :     float           _setpoint_desired_rotor_speed;// latest setpoint desired rotor speed
     165             :     float           _ddfp_desired_rotor_speed;    // latest ddfp desired rotor speed
     166             :     float           _desired_rotor_speed;         // latest desired rotor speed
     167             :     float           _control_output;              // latest logic controlled output
     168             :     float           _rotor_ramp_output;           // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f)
     169             :     float           _rotor_runup_output;          // scalar used to store status of rotor run-up time (0.0-1.0f)
     170             :     bool            _runup_complete;              // flag for determining if runup is complete
     171             :     float           _thrcrv_poly[4][4];           // spline polynomials for throttle curve interpolation
     172             :     float           _collective_in;               // collective in for throttle curve calculation, range 0-1.0f
     173             :     float           _rotor_rpm;                   // rotor rpm from speed sensor for governor
     174             :     bool            _turbine_start;               // initiates starting sequence
     175             :     bool            _starting;                    // tracks if starting sequence has been used
     176             :     float           _governor_output;             // governor output for rotor speed control
     177             :     bool            _governor_engage;             // RSC governor status flag
     178             :     bool            _autothrottle;                // autothrottle status flag
     179             :     bool            _governor_fault;              // governor fault status flag
     180             :     bool            _spooldown_complete;          // flag for determining if spooldown is complete
     181             :     float           _fast_idle_timer;             // cooldown timer variable
     182             :     uint8_t         _governor_fault_count;        // variable for tracking governor speed sensor faults
     183             :     float           _governor_torque_reference;   // governor reference for load calculations
     184             :     float           _idle_throttle;               // current idle throttle setting
     185             :     bool            _save_rsc_mode;               // flag to determine if we should save RSC mode changes to EEPROM, used to prevent saving changes to RSC mode param while armed.
     186             :     bool            _using_manual_collective_mode; // flag to determine if we are using a manual collective flight mode
     187             : 
     188             :     // parameters
     189             :     AP_Int16        _power_slewrate;            // throttle slew rate (percentage per second)
     190             :     AP_Int16        _thrcrv[5];                 // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective
     191             :     AP_Int16        _governor_rpm;              // governor reference for speed calculations
     192             :     AP_Float        _governor_torque;           // governor torque rise setting
     193             :     AP_Float        _governor_compensator;      // governor torque compensator variable
     194             :     AP_Float        _governor_droop_response;   // governor response to droop under load
     195             :     AP_Float        _governor_ff;               // governor feedforward variable
     196             :     AP_Float        _governor_range;            // RPM range +/- governor rpm reference setting where governor is operational
     197             :     AP_Int16        _cooldown_time;             // cooldown time to provide a fast idle
     198             : 
     199             :     // parameter accessors to allow conversions
     200     2277141 :     float           get_critical_speed() const { return _critical_speed * 0.01; }
     201     4271552 :     float           get_idle_output() const { return _idle_output * 0.01; }
     202        3858 :     float           get_governor_torque() const { return _governor_torque * 0.01; }
     203       13326 :     float           get_governor_compensator() const { return _governor_compensator * 0.000001; }
     204             : 
     205             : };