AP_Motors: allow external slew limit in output_motor_mask

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -740,13 +740,18 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th
 // output a thrust to all motors that match a given motor mask. This
 // is used to control tiltrotor motors in forward flight. Thrust is in
 // the range 0 to 1
-void AP_MotorsMulticopter::output_motor_mask(float thrust, uint16_t mask, float rudder_dt)
+void AP_MotorsMulticopter::output_motor_mask(const float thrust, const uint16_t mask, const float rudder_dt, const float slew_limit)
 {
     const int16_t pwm_min = get_pwm_output_min();
     const int16_t pwm_range = get_pwm_output_max() - pwm_min;
 
     _motor_mask_override = mask;
 
+    // Convert from percentage per second to change per loop.
+    // Negative or zero is disabled
+    const float max_change = slew_limit * 0.01 * _dt;
+    const bool apply_slew_limit = is_positive(max_change);
+
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i] && (mask & (1U << i)) != 0) {
             if (armed() && get_interlock()) {
@@ -755,8 +760,22 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint16_t mask, float
                  copter frame roll is plane frame yaw as this only
                  apples to either tilted motors or tailsitters
                  */
-                float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
+                const float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5;
+
+                // Apply slew limiting, this is the lower of the motors slew limiting and the slew limit passed in.
+                const float last = _actuator[i];
+
+                // Apply motor slew limits
                 set_actuator_with_slew(_actuator[i], thrust + diff_thrust);
+
+                // Apply passed in slew limit
+                if (apply_slew_limit) {
+                    const float lower = MAX(0.0, last - max_change);
+                    const float upper = MIN(1.0, last + max_change);
+
+                    _actuator[i] = constrain_float(_actuator[i], lower, upper);
+                }
+
             } else {
                 // zero throttle
                 _actuator[i] = 0.0;

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -60,7 +60,7 @@ class AP_MotorsMulticopter : public AP_Motors {
     // output a thrust to all motors that match a given motor
     // mask. This is used to control tiltrotor motors in forward
     // flight. Thrust is in the range 0 to 1
-    virtual void        output_motor_mask(float thrust, uint16_t mask, float rudder_dt);
+    virtual void        output_motor_mask(const float thrust, const uint16_t mask, const float rudder_dt, const float slew_limit);
 
     // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
     //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -351,10 +351,10 @@ void AP_MotorsTri::thrust_compensation(void)
 /*
   override tricopter tail servo output in output_motor_mask
  */
-void AP_MotorsTri::output_motor_mask(float thrust, uint16_t mask, float rudder_dt)
+void AP_MotorsTri::output_motor_mask(const float thrust, const uint16_t mask, const float rudder_dt, const float slew_limit)
 {
     // normal multicopter output
-    AP_MotorsMulticopter::output_motor_mask(thrust, mask, rudder_dt);
+    AP_MotorsMulticopter::output_motor_mask(thrust, mask, rudder_dt, slew_limit);
 
     // and override yaw servo
     rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);

libraries/AP_Motors/AP_MotorsTri.h

@@ -42,7 +42,7 @@ class AP_MotorsTri : public AP_MotorsMulticopter {
     // mask. This is used to control tiltrotor motors in forward
     // flight. Thrust is in the range 0 to 1
     // rudder_dt applys diffential thrust for yaw in the range 0 to 1
-    void                output_motor_mask(float thrust, uint16_t mask, float rudder_dt) override;
+    void                output_motor_mask(const float thrust, const uint16_t mask, const float rudder_dt, const float slew_limit) override;
 
     // return the roll factor of any motor, this is used for tilt rotors and tail sitters
     // using copter motors for forward flight