Line data    Source code

       1             : /// @file       AP_MotorsMatrix.h
       2             : /// @brief      Motor control class for Matrixcopters
       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 <RC_Channel/RC_Channel.h>     // RC Channel Library
       8             : #include "AP_MotorsMulticopter.h"
       9             : 
      10             : #define AP_MOTORS_MATRIX_YAW_FACTOR_CW   -1
      11             : #define AP_MOTORS_MATRIX_YAW_FACTOR_CCW   1
      12             : 
      13             : /// @class      AP_MotorsMatrix
      14             : class AP_MotorsMatrix : public AP_MotorsMulticopter {
      15             : public:
      16             : 
      17             :     /// Constructor
      18        1189 :     AP_MotorsMatrix(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
      19        1189 :         AP_MotorsMulticopter(speed_hz)
      20             :     {
      21        1189 :         if (_singleton != nullptr) {
      22           0 :             AP_HAL::panic("AP_MotorsMatrix must be singleton");
      23             :         }
      24        1189 :         _singleton = this;
      25        1189 :     };
      26             : 
      27             :     // get singleton instance
      28          67 :     static AP_MotorsMatrix *get_singleton() {
      29          67 :         return _singleton;
      30             :     }
      31             : 
      32             :     // init
      33             :     virtual void        init(motor_frame_class frame_class, motor_frame_type frame_type) override;
      34             : 
      35             : #if AP_SCRIPTING_ENABLED
      36             :     // Init to be called from scripting
      37             :     virtual bool        init(uint8_t expected_num_motors);
      38             : 
      39             :     // Set throttle factor from scripting
      40             :     bool                set_throttle_factor(int8_t motor_num, float throttle_factor);
      41             : 
      42             : #endif // AP_SCRIPTING_ENABLED
      43             : 
      44             :     // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
      45             :     void                set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override;
      46             : 
      47             :     // set update rate to motors - a value in hertz
      48             :     // you must have setup_motors before calling this
      49             :     void                set_update_rate(uint16_t speed_hz) override;
      50             : 
      51             :     // output_test_num - spin a motor connected to the specified output channel
      52             :     //  (should only be performed during testing)
      53             :     //  If a motor output channel is remapped, the mapped channel is used.
      54             :     //  Returns true if motor output is set, false otherwise
      55             :     //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
      56             :     bool                output_test_num(uint8_t motor, int16_t pwm);
      57             : 
      58             :     // output_to_motors - sends minimum values out to the motors
      59             :     virtual void        output_to_motors() override;
      60             : 
      61             :     // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
      62             :     //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
      63             :     uint32_t            get_motor_mask() override;
      64             : 
      65             :     // return number of motor that has failed.  Should only be called if get_thrust_boost() returns true
      66       10831 :     uint8_t             get_lost_motor() const override { return _motor_lost_index; }
      67             : 
      68             :     // return the roll factor of any motor, this is used for tilt rotors and tail sitters
      69             :     // using copter motors for forward flight
      70     1082534 :     float               get_roll_factor(uint8_t i) override { return _roll_factor[i]; }
      71             :     // return the pitch factor of any motor
      72           0 :     float               get_pitch_factor(uint8_t i) override { return _pitch_factor[i]; }
      73             : 
      74             :     // disable the use of motor torque to control yaw. Used when an external mechanism such
      75             :     // as vectoring is used for yaw control
      76             :     void                disable_yaw_torque(void) override;
      77             : 
      78             :     // add_motor using raw roll, pitch, throttle and yaw factors
      79             :     void                add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor = 1.0f);
      80             : 
      81             :     // same structure, but with floats.
      82             :     struct MotorDef {
      83             :         float angle_degrees;
      84             :         float yaw_factor;
      85             :         uint8_t testing_order;
      86             :     };
      87             : 
      88             :     // method to add many motors specified in a structure:
      89             :     void add_motors(const struct MotorDef *motors, uint8_t num_motors);
      90             : 
      91             :     // structure used for initialising motors that add have separate
      92             :     // roll/pitch/yaw factors.  Note that this does *not* include
      93             :     // the final parameter for the add_motor_raw call - throttle
      94             :     // factor as that is only used in the scripting binding, not in
      95             :     // the static motors at the moment.
      96             :     struct MotorDefRaw {
      97             :         float roll_fac;
      98             :         float pitch_fac;
      99             :         float yaw_fac;
     100             :         uint8_t testing_order;
     101             :     };
     102             :     void add_motors_raw(const struct MotorDefRaw *motors, uint8_t num_motors);
     103             : 
     104             :     // pull values direct, (examples only)
     105             :     float get_thrust_rpyt_out(uint8_t i) const;
     106             :     bool get_factors(uint8_t i, float &roll, float &pitch, float &yaw, float &throttle, uint8_t &testing_order) const;
     107             : 
     108             : protected:
     109             :     // output - sends commands to the motors
     110             :     void                output_armed_stabilizing() override;
     111             : 
     112             :     // check for failed motor
     113             :     void                check_for_failed_motor(float throttle_thrust_best);
     114             : 
     115             :     // add_motor using just position and yaw_factor (or prop direction)
     116             :     void                add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order);
     117             : 
     118             :     // add_motor using separate roll and pitch factors (for asymmetrical frames) and prop direction
     119             :     void                add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order);
     120             : 
     121             :     // remove_motor
     122             :     void                remove_motor(int8_t motor_num);
     123             : 
     124             :     // configures the motors for the defined frame_class and frame_type
     125             :     virtual void        setup_motors(motor_frame_class frame_class, motor_frame_type frame_type);
     126             : 
     127             :     // normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
     128             :     void                normalise_rpy_factors();
     129             : 
     130             :     // call vehicle supplied thrust compensation if set
     131             :     void                thrust_compensation(void) override;
     132             : 
     133        3270 :     const char*         _get_frame_string() const override { return _frame_class_string; }
     134        3208 :     const char*         get_type_string() const override { return _frame_type_string; }
     135             : 
     136             :     // output_test_seq - spin a motor at the pwm value specified
     137             :     //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
     138             :     //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
     139             :     virtual void        _output_test_seq(uint8_t motor_seq, int16_t pwm) override;
     140             : 
     141             :     float               _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
     142             :     float               _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
     143             :     float               _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to yaw (normally 1 or -1)
     144             :     float               _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to throttle 0~1
     145             :     float               _thrust_rpyt_out[AP_MOTORS_MAX_NUM_MOTORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
     146             :     uint8_t             _test_order[AP_MOTORS_MAX_NUM_MOTORS];  // order of the motors in the test sequence
     147             : 
     148             :     // motor failure handling
     149             :     float               _thrust_rpyt_out_filt[AP_MOTORS_MAX_NUM_MOTORS];    // filtered thrust outputs with 1 second time constant
     150             :     uint8_t             _motor_lost_index;  // index number of the lost motor
     151             : 
     152             :     motor_frame_class   _active_frame_class; // active frame class (i.e. quad, hexa, octa, etc)
     153             :     motor_frame_type    _active_frame_type;  // active frame type (i.e. plus, x, v, etc)
     154             : 
     155             :     const char*         _frame_class_string = ""; // string representation of frame class
     156             :     const char*         _frame_type_string = "";  //  string representation of frame type
     157             : 
     158             : private:
     159             : 
     160             :     // helper to return value scaled between boost and normal based on the value of _thrust_boost_ratio
     161             :     float boost_ratio(float boost_value, float normal_value) const;
     162             : 
     163             :     // setup motors matrix
     164             :     bool setup_quad_matrix(motor_frame_type frame_type);
     165             :     bool setup_hexa_matrix(motor_frame_type frame_type);
     166             :     bool setup_octa_matrix(motor_frame_type frame_type);
     167             :     bool setup_deca_matrix(motor_frame_type frame_type);
     168             :     bool setup_dodecahexa_matrix(motor_frame_type frame_type);
     169             :     bool setup_y6_matrix(motor_frame_type frame_type);
     170             :     bool setup_octaquad_matrix(motor_frame_type frame_type);
     171             : 
     172             :     static AP_MotorsMatrix *_singleton;
     173             : };