Line data    Source code

       1             : /// @file   AP_MotorsHeli_Dual.h
       2             : /// @brief  Motor control class for dual heli (tandem or transverse)
       3             : /// @author Fredrik Hedberg
       4             : 
       5             : #pragma once
       6             : 
       7             : #include <AP_Common/AP_Common.h>
       8             : #include <AP_Math/AP_Math.h>
       9             : #include <RC_Channel/RC_Channel.h>
      10             : 
      11             : #include "AP_MotorsHeli.h"
      12             : #include "AP_MotorsHeli_RSC.h"
      13             : #include "AP_MotorsHeli_Swash.h"
      14             : 
      15             : // tandem modes
      16             : #define AP_MOTORS_HELI_DUAL_MODE_TANDEM                0 // tandem mode (rotors front and aft)
      17             : #define AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE            1 // transverse mode (rotors side by side)
      18             : #define AP_MOTORS_HELI_DUAL_MODE_INTERMESHING          2 // intermeshing mode (rotors side by side)
      19             : 
      20             : // default differential-collective-pitch scaler
      21             : #define AP_MOTORS_HELI_DUAL_DCP_SCALER             0.25f
      22             : 
      23             : // maximum number of swashplate servos
      24             : #define AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS    6
      25             : 
      26             : // default collective min, max and midpoints for the rear swashplate
      27             : #define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN 1250
      28             : #define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX 1750
      29             : 
      30             : /// @class AP_MotorsHeli_Dual
      31             : class AP_MotorsHeli_Dual : public AP_MotorsHeli {
      32             : public:
      33             :     // constructor
      34           0 :     AP_MotorsHeli_Dual(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
      35           0 :         AP_MotorsHeli(speed_hz)
      36             :     {
      37           0 :         AP_Param::setup_object_defaults(this, var_info);
      38           0 :     };
      39             : 
      40             : 
      41             :     // set_update_rate - set update rate to motors
      42             :     void set_update_rate( uint16_t speed_hz ) override;
      43             : 
      44             :     // output_to_motors - sends values out to the motors
      45             :     void output_to_motors() override;
      46             : 
      47             :     // calculate_scalars - recalculates various scalars used
      48             :     void calculate_scalars() override;
      49             : 
      50             :     // servo_test - move servos through full range of movement
      51             :     void servo_test() override;
      52             : 
      53             :     // Run arming checks
      54             :     bool arming_checks(size_t buflen, char *buffer) const override;
      55             : 
      56             :     // Helper function for param conversions to be done in motors class
      57             :     void heli_motors_param_conversions(void) override;
      58             : 
      59             : #if HAL_LOGGING_ENABLED
      60             :     // heli motors logging - called at 10 Hz
      61             :     void Log_Write(void) override;
      62             : #endif
      63             : 
      64             :     // var_info for holding Parameter information
      65             :     static const struct AP_Param::GroupInfo var_info[];
      66             : 
      67             : protected:
      68             : 
      69             :     // init_outputs
      70             :     void init_outputs () override;
      71             : 
      72             :     // update_motor_controls - sends commands to motor controllers
      73             :     void update_motor_control(AP_MotorsHeli_RSC::DesiredRSCSpoolState state) override;
      74             : 
      75             :     // update_spool_state - updates the spool state based on the desired state
      76             :     AP_Motors::SpoolState update_spool_state(AP_MotorsHeli_RSC::DesiredRSCSpoolState state) override;
      77             : 
      78             :     // get_swashplate - calculate movement of each swashplate based on configuration
      79             :     float get_swashplate(int8_t swash_num, int8_t swash_axis, float pitch_input, float roll_input, float yaw_input, float coll_input);
      80             : 
      81             :     // move_actuators - moves swash plate to attitude of parameters passed in
      82             :     void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out)  override;
      83             : 
      84           0 :     const char* _get_frame_string() const override { return "HELI_DUAL"; }
      85             : 
      86             :     //  objects we depend upon
      87             :     AP_MotorsHeli_Swash _swashplate1 { CH_1, CH_2, CH_3, CH_7, 1U }; // swashplate1
      88             :     AP_MotorsHeli_Swash _swashplate2 { CH_4, CH_5, CH_6, CH_8, 2U }; // swashplate2
      89             : 
      90             :     // internal variables
      91             :     float _oscillate_angle = 0.0f;                  // cyclic oscillation angle, used by servo_test function
      92             :     float _servo_test_cycle_time = 0.0f;            // cycle time tracker, used by servo_test function
      93             :     float _collective_test = 0.0f;                  // over-ride for collective output, used by servo_test function
      94             :     float _roll_test = 0.0f;                        // over-ride for roll output, used by servo_test function
      95             :     float _pitch_test = 0.0f;                       // over-ride for pitch output, used by servo_test function
      96             :     float _servo_out[8];                            // output value sent to motor
      97             : 
      98             :     // parameters
      99             :     AP_Int16        _collective2_min;               // Lowest possible servo position for the rear swashplate
     100             :     AP_Int16        _collective2_max;               // Highest possible servo position for the rear swashplate
     101             :     AP_Int8         _dual_mode;                     // which dual mode the heli is
     102             :     AP_Float        _dcp_scaler;                    // scaling factor applied to the differential-collective-pitch
     103             :     AP_Float        _dcp_yaw_effect;                // feed-forward compensation to automatically add yaw input when differential collective pitch is applied.
     104             :     AP_Float        _yaw_scaler;                    // scaling factor applied to the yaw mixing
     105             :     AP_Float        _dcp_trim;                      // used to easily trim dcp axis
     106             :     AP_Float        _yaw_rev_expo;                  // yaw reverser smoothing exponent, for intermeshing mode only.
     107             : 
     108             :     // internal variables
     109             :     float _collective2_zero_thrst_pct;
     110             : 
     111             : private:
     112             : 
     113             :     // Mix and output swashplates for tandem
     114             :     void mix_tandem(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input);
     115             : 
     116             :     // Mix and output swashplates for transverse
     117             :     void mix_transverse(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input);
     118             : 
     119             :     // Mix and output swashplates for intermeshing
     120             :     void mix_intermeshing(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input);
     121             : 
     122             : };