Line data    Source code

       1             : /// @file       AP_MotorsMulticopter.h
       2             : /// @brief      Motor control class for Multicopters
       3             : #pragma once
       4             : 
       5             : #include "AP_Motors_Class.h"
       6             : #include "AP_Motors_Thrust_Linearization.h"
       7             : 
       8             : #define AP_MOTORS_YAW_HEADROOM_DEFAULT  200
       9             : #define AP_MOTORS_THST_EXPO_DEFAULT     0.65f   // set to 0 for linear and 1 for second order approximation
      10             : #define AP_MOTORS_THST_HOVER_DEFAULT    0.35f   // the estimated hover throttle, 0 ~ 1
      11             : #define AP_MOTORS_THST_HOVER_TC         10.0f   // time constant used to update estimated hover throttle, 0 ~ 1
      12             : #define AP_MOTORS_THST_HOVER_MIN        0.125f  // minimum possible hover throttle
      13             : #define AP_MOTORS_THST_HOVER_MAX        0.6875f // maximum possible hover throttle
      14             : #define AP_MOTORS_SPIN_MIN_DEFAULT      0.15f   // throttle out ratio which produces the minimum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
      15             : #define AP_MOTORS_SPIN_MAX_DEFAULT      0.95f   // throttle out ratio which produces the maximum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
      16             : #define AP_MOTORS_SPIN_ARM_DEFAULT      0.10f   // throttle out ratio which produces the armed spin rate.  (i.e. 0 ~ 1 ) of the full throttle range
      17             : #define AP_MOTORS_BAT_VOLT_MAX_DEFAULT  0.0f    // voltage limiting max default
      18             : #define AP_MOTORS_BAT_VOLT_MIN_DEFAULT  0.0f    // voltage limiting min default (voltage dropping below this level will have no effect)
      19             : #define AP_MOTORS_BAT_CURR_MAX_DEFAULT  0.0f    // current limiting max default
      20             : #define AP_MOTORS_BAT_CURR_TC_DEFAULT   5.0f    // Time constant used to limit the maximum current
      21             : #define AP_MOTORS_SLEW_TIME_DEFAULT     0.0f    // slew rate limit for thrust output
      22             : #define AP_MOTORS_SAFE_TIME_DEFAULT     1.0f    // Time for the esc when transitioning between zero pwm to minimum
      23             : 
      24             : // spool definition
      25             : #define AP_MOTORS_SPOOL_UP_TIME_DEFAULT 0.5f    // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle.
      26             : 
      27             : /// @class      AP_MotorsMulticopter
      28             : class AP_MotorsMulticopter : public AP_Motors {
      29             : public:
      30             : 
      31             :     // Constructor
      32             :     AP_MotorsMulticopter(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
      33             : 
      34             :     // output - sends commands to the motors
      35             :     virtual void        output() override;
      36             : 
      37             :     // output_min - sends minimum values out to the motors
      38             :     void                output_min() override;
      39             : 
      40             :     // set_desired_spool_state - set desired spool state with safety constraints
      41             :     void                set_desired_spool_state(DesiredSpoolState spool) override;
      42             : 
      43             :     // set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
      44           0 :     void                set_yaw_headroom(int16_t pwm) { _yaw_headroom.set(pwm); }
      45             : 
      46             :     // update_throttle_range - update throttle endpoints
      47             :     void                update_throttle_range();
      48             : 
      49             :     // update estimated throttle required to hover
      50             :     void                update_throttle_hover(float dt);
      51    98768553 :     virtual float       get_throttle_hover() const override { return constrain_float(_throttle_hover, AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX); }
      52             : 
      53             :     // passes throttle directly to all motors for ESC calibration.
      54             :     // throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
      55             :     void                set_throttle_passthrough_for_esc_calibration(float throttle_input);
      56             : 
      57             :     // returns maximum thrust in the range 0 to 1
      58    31750600 :     float               get_throttle_thrust_max() const { return _throttle_thrust_max; }
      59             : 
      60             :     // return true if spool up is complete
      61             :     bool spool_up_complete() const { return _spool_state == SpoolState::THROTTLE_UNLIMITED; }
      62             : 
      63             :     // output a thrust to all motors that match a given motor
      64             :     // mask. This is used to control tiltrotor motors in forward
      65             :     // flight. Thrust is in the range 0 to 1
      66             :     virtual void        output_motor_mask(float thrust, uint32_t mask, float rudder_dt);
      67             : 
      68             :     // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
      69             :     // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
      70             :     virtual uint32_t    get_motor_mask() override;
      71             : 
      72             :     // get minimum or maximum pwm value that can be output to motors
      73   233019182 :     int16_t             get_pwm_output_min() const { return _pwm_min; }
      74    91259377 :     int16_t             get_pwm_output_max() const { return _pwm_max; }
      75             :     
      76             :     // parameter check for MOT_PWM_MIN/MAX, returns true if parameters are valid
      77             :     bool check_mot_pwm_params() const;
      78             : 
      79             :     // set thrust compensation callback
      80             :     FUNCTOR_TYPEDEF(thrust_compensation_fn_t, void, float *, uint8_t);
      81          13 :     void                set_thrust_compensation_callback(thrust_compensation_fn_t callback) {
      82          13 :         _thrust_compensation_callback = callback;
      83          13 :     }
      84             :     
      85             :     // disable the use of motor torque to control yaw. Used when an external mechanism such
      86             :     // as vectoring is used for yaw control
      87           2 :     virtual void        disable_yaw_torque(void) {}
      88             : 
      89             :     // return whether a motor is enabled or not
      90         126 :     bool                is_motor_enabled(uint8_t i) override { return motor_enabled[i]; }
      91             : 
      92             :     // convert values to PWM min and max if not configured
      93             :     void                convert_pwm_min_max_param(int16_t radio_min, int16_t radio_max);
      94             : 
      95             :     // return thrust for motor motor_num, returns true if value is valid false otherwise
      96             :     bool                get_thrust(uint8_t motor_num, float& thr_out) const override;
      97             : 
      98             :     bool                get_raw_motor_throttle(uint8_t motor_num, float& thr_out) const override;
      99             : 
     100             : #if HAL_LOGGING_ENABLED
     101             :     // 10hz logging of voltage scaling and max trust
     102             :     void                Log_Write() override;
     103             : #endif
     104             : 
     105             :     // Run arming checks
     106             :     bool arming_checks(size_t buflen, char *buffer) const override;
     107             : 
     108             :     // Getters for AP_Motors example, not used by vehicles
     109             :     float get_throttle_avg_max() const;
     110             :     int16_t get_yaw_headroom() const;
     111             : 
     112             :     // Thrust Linearization handling
     113             :     Thrust_Linearization thr_lin {*this};
     114             : 
     115             :     // var_info for holding Parameter information
     116             :     static const struct AP_Param::GroupInfo        var_info[];
     117             : 
     118             : protected:
     119             : 
     120             :     // run spool logic
     121             :     // advance the motor spool state machine once per cycle
     122             :     // enforce arming/interlock and disarm-pwm safe-time guards
     123             :     // shape two normalised scalars with monotonic ramps during normal operation
     124             :     //  - _spin_up_ratio        controls motor spin/idle shaping
     125             :     //  - _throttle_thrust_max  defines the moving throttle ceiling, limited by current draw
     126             :     // both values are normalised between 0 and 1
     127             :     // zero thrust while !armed() or !get_interlock()
     128             :     // ramps are limited by the configured spool times (see minimum_spool_time)
     129             :     // disarm or interlock drop immediately forces SHUT_DOWN for safety
     130             :     // pre-takeoff checks may hold in GROUND_IDLE until cleared (get_spoolup_block())
     131             :     void                output_logic();
     132             : 
     133             :     // output_to_motors - sends commands to the motors
     134             :     virtual void        output_to_motors() = 0;
     135             : 
     136             :     // update the throttle input filter
     137             :     virtual void        update_throttle_filter() override;
     138             : 
     139             :     // return current-limit as a normalised throttle [0..1] within the min..max range
     140             :     // returns 1.0 when current limiting is disabled, disarmed, or telemetry is unavailable
     141             :     virtual float       get_current_limit_max_throttle();
     142             : 
     143             :     // convert normalised actuator output (0..1) to pwm
     144             :     // in SHUT_DOWN: output 0 pwm when _disarm_disable_pwm && !armed(), else pwm_min
     145             :     // in all other states: map [0..1] linearly between pwm_min and pwm_max
     146             :     int16_t             output_to_pwm(float _actuator_output);
     147             : 
     148             :     // slew limiting on motor outputs:
     149             :     // - MOT_SLEW_UP_TIME = time to go 0 -> 1 (s); 0 disables up-slew limiting
     150             :     // - MOT_SLEW_DN_TIME = time to go 1 -> 0 (s); 0 disables down-slew limiting
     151             :     // - both times are constrained to 0..0.5 s
     152             :     // - no slew limiting while in SHUT_DOWN to allow immediate motor de-energisation
     153             :     void                set_actuator_with_slew(float& actuator_output, float input);
     154             : 
     155             :     // gradually increase actuator output to spin_min
     156             :     // equals _spin_up_ratio * spin_min (constrained to [0..1])
     157             :     float               actuator_spin_up_to_ground_idle() const;
     158             : 
     159             :     // apply any thrust compensation for the frame
     160      163407 :     virtual void        thrust_compensation(void) {}
     161             : 
     162             :     // output booster throttle, if any
     163             :     virtual void        output_boost_throttle(void);
     164             : 
     165             :     // output roll/pitch/yaw/thrust
     166             :     virtual void        output_rpyt(void);
     167             : 
     168             :     // save parameters as part of disarming
     169             :     void                save_params_on_disarm() override;
     170             : 
     171             :     // update external limits from scripting
     172             :     void                update_external_limits();
     173             : 
     174             :     // enum values for HOVER_LEARN parameter
     175             :     enum HoverLearn {
     176             :         HOVER_LEARN_DISABLED = 0,
     177             :         HOVER_LEARN_ONLY = 1,
     178             :         HOVER_LEARN_AND_SAVE = 2
     179             :     };
     180             : 
     181             :     // parameters
     182             :     AP_Int16            _yaw_headroom;          // yaw control is given at least this pwm range
     183             :     AP_Float            _slew_up_time;          // throttle increase slew limiting
     184             :     AP_Float            _slew_dn_time;          // throttle decrease slew limiting
     185             :     AP_Float            _safe_time;             // delay after arming when pwm has been re-enabled (esc/servo startup window)
     186             :     AP_Float            _spin_arm;              // throttle out ratio which produces the armed spin rate.  (i.e. 0 ~ 1 ) of the full throttle range
     187             :     AP_Float            _batt_current_max;      // current over which maximum throttle is limited
     188             :     AP_Float            _batt_current_time_constant;    // Time constant used to limit the maximum current
     189             :     AP_Int16            _pwm_min;               // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
     190             :     AP_Int16            _pwm_max;               // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
     191             :     AP_Float            _throttle_hover;        // estimated throttle required to hover throttle in the range 0 ~ 1
     192             :     AP_Int8             _throttle_hover_learn;  // enable/disabled hover thrust learning
     193             :     AP_Int8             _disarm_disable_pwm;    // disable PWM output while disarmed
     194             : 
     195             :     // Maximum lean angle of yaw servo in degrees. This is specific to tricopter
     196             :     AP_Float            _yaw_servo_angle_max_deg;
     197             : 
     198             :     // time to spool motors to min throttle
     199             :     AP_Float            _spool_up_time;
     200             :     AP_Float            _spool_down_time;
     201             :     AP_Float            _idle_time_delay_s;
     202             : 
     203             :     // scaling for booster motor throttle
     204             :     AP_Float            _boost_scale;
     205             : 
     206             :     // motor output variables
     207             :     bool                motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];    // true if motor is enabled
     208             : 
     209             :     // spool variables
     210             :     float               _spin_up_ratio;         // normalized spin scalar [0..1] between 0 and spin_min (used for ground-idle ramp)
     211             :     float               _idle_time;             // idle delay elapsed time at/above ground-idle spin [s]
     212             :     bool                _spin_up_complete;      // set to true when spin up is complete and spool up blocks have been enabled
     213             : 
     214             :     // battery voltage, current and air pressure compensation variables
     215             :     float               _throttle_limit;        // ratio of throttle limit between hover and maximum
     216             :     float               _throttle_thrust_max;   // moving throttle ceiling [0..1] (bounded by current-limit)
     217             :     float               _disarm_safe_timer;     // counts up to _safe_time after arming when pwm was disabled while disarmed
     218             : 
     219             :     // vehicle supplied callback for thrust compensation. Used for tiltrotors and tiltwings
     220             :     thrust_compensation_fn_t _thrust_compensation_callback;
     221             : 
     222             :     // array of motor output values
     223             :     float _actuator[AP_MOTORS_MAX_NUM_MOTORS];
     224             : 
     225             :     /* motor enabled, checking the override mask
     226             :        _motor_mask_override is only set for tilt quadplanes
     227             :      */
     228   445389376 :     bool motor_enabled_mask(uint8_t i) const {
     229   445389376 :         return motor_enabled[i] && (_motor_mask_override & (1U << i)) == 0;
     230             :     }
     231             : 
     232             :     // mask of overridden motors (used by quadplane tiltrotors)
     233             :     uint32_t _motor_mask_override;
     234             : };