Line data    Source code

       1             : /// @file       AP_MotorsHeli_Swash.h
       2             : /// @brief      Swashplate Library for traditional heli
       3             : #pragma once
       4             : 
       5             : #include <AP_Common/AP_Common.h>
       6             : #include <AP_Math/AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
       7             : #include <AP_Param/AP_Param.h>
       8             : #include <AP_Logger/AP_Logger_config.h>
       9             : 
      10             : // swashplate types
      11             : enum SwashPlateType {
      12             :     SWASHPLATE_TYPE_H3 = 0,
      13             :     SWASHPLATE_TYPE_H1,
      14             :     SWASHPLATE_TYPE_H3_140,
      15             :     SWASHPLATE_TYPE_H3_120,
      16             :     SWASHPLATE_TYPE_H4_90,
      17             :     SWASHPLATE_TYPE_H4_45
      18             : };
      19             : 
      20             : class AP_MotorsHeli_Swash {
      21             : public:
      22             : 
      23             :     AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t instance);
      24             : 
      25             :     // configure - configure the swashplate settings for any updated parameters
      26             :     void configure();
      27             : 
      28             :     // get_swash_type - gets swashplate type
      29          80 :     SwashPlateType get_swash_type() const { return _swash_type; }
      30             : 
      31             :     // calculates servo output
      32             :     void calculate(float roll, float pitch, float collective);
      33             : 
      34             :     // Output calculated values to servos
      35             :     void output();
      36             : 
      37             :     // get_phase_angle - returns the rotor phase angle
      38        4412 :     float get_phase_angle() const { return _phase_angle; }
      39             : 
      40             :     // Get function output mask
      41             :     uint32_t get_output_mask() const;
      42             : 
      43             : #if HAL_LOGGING_ENABLED
      44             :     // Write SWSH log for this instance of swashplate
      45             :     void write_log(float cyclic_scaler, float col_ang_min, float col_ang_max, int16_t col_min, int16_t col_max) const;
      46             : #endif
      47             : 
      48             :     // var_info
      49             :     static const struct AP_Param::GroupInfo var_info[];
      50             : 
      51             : private:
      52             : 
      53             :     // linearize mechanical output of swashplate servo
      54             :     float get_linear_servo_output(float input) const;
      55             : 
      56             :     // CCPM Mixers - calculate mixing scale factors by swashplate type
      57             :     void calculate_roll_pitch_collective_factors();
      58             : 
      59             :     // Setup a servo
      60             :     void add_servo_angle(uint8_t num, float scale, float angle, float collective);
      61             :     void add_servo_raw(uint8_t num, float roll, float pitch, float collective);
      62             : 
      63             :     // write to a swash servo. output value is pwm
      64             :     void rc_write(uint8_t chan, float swash_in);
      65             : 
      66             :     enum CollectiveDirection {
      67             :         COLLECTIVE_DIRECTION_NORMAL = 0,
      68             :         COLLECTIVE_DIRECTION_REVERSED
      69             :     };
      70             : 
      71             :     static const uint8_t _max_num_servos {4};
      72             : 
      73             :     // Currently configured setup
      74             :     SwashPlateType       _swash_type;                 // Swashplate type
      75             :     CollectiveDirection  _collective_direction;       // Collective control direction, normal or reversed
      76             :     bool                 _make_servo_linear;          // Sets servo output to be linearized
      77             : 
      78             :     // Internal variables
      79             :     bool                 _enabled[_max_num_servos];                 // True if this output servo is enabled
      80             :     float                _rollFactor[_max_num_servos];              // Roll axis scaling of servo output based on servo position
      81             :     float                _pitchFactor[_max_num_servos];             // Pitch axis scaling of servo output based on servo position
      82             :     float                _collectiveFactor[_max_num_servos];        // Collective axis scaling of servo output based on servo position
      83             :     float                _output[_max_num_servos];                  // Servo output value
      84             :     const uint8_t        _motor_num[_max_num_servos];               // Motor function to use for output
      85             :     const uint8_t        _instance;                                 // Swashplate instance. Used for logging.
      86             : 
      87             :     // Variables stored for logging
      88             :     float _roll_input;
      89             :     float _pitch_input;
      90             :     float _collective_input_scaled;
      91             : 
      92             :     // parameters
      93             :     AP_Int8  _swashplate_type;                   // Swash Type Setting
      94             :     AP_Int8  _swash_coll_dir;                    // Collective control direction, normal or reversed
      95             :     AP_Int8  _linear_swash_servo;                // linearize swashplate output
      96             :     AP_Int8  enable;
      97             :     AP_Int16 _servo1_pos;                        // servo1 azimuth position on swashplate with front of heli being 0 deg
      98             :     AP_Int16 _servo2_pos;                        // servo2 azimuth position on swashplate with front of heli being 0 deg
      99             :     AP_Int16 _servo3_pos;                        // servo3 azimuth position on swashplate with front of heli being 0 deg
     100             :     AP_Float _phase_angle;                       // Phase angle correction for rotor head.  If pitching the swash forward induces 
     101             :                                                  // a roll, this can be negative depending on mechanics.
     102             : 
     103             : };
     104             :