Plane: pass throttle slew limit to motors output_motor_mask function.

ArduPlane/Plane.h

@@ -1180,6 +1180,7 @@ class Plane : public AP_Vehicle {
     void servos_twin_engine_mix();
     void force_flare();
     void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
+    float get_throttle_slewrate() const;
     void throttle_slew_limit();
     bool suppress_throttle(void);
     void update_throttle_hover();

ArduPlane/servos.cpp

@@ -19,10 +19,10 @@
 #include "Plane.h"
 #include <utility>
 
-/*****************************************
-* Throttle slew limit
-*****************************************/
-void Plane::throttle_slew_limit()
+// Get slew rate for throttle as a percentage per second.
+// 100% would allow the throttle to move from 0 to 100 in 1 second.
+// 0 is disabled
+float Plane::get_throttle_slewrate() const
 {
 #if HAL_QUADPLANE_ENABLED
     const bool do_throttle_slew = (control_mode->does_auto_throttle() || quadplane.in_assisted_flight() || quadplane.in_vtol_mode());
@@ -32,10 +32,7 @@ void Plane::throttle_slew_limit()
 
     if (!do_throttle_slew) {
         // only do throttle slew limiting in modes where throttle control is automatic
-        SRV_Channels::set_slew_rate(SRV_Channel::k_throttle,      0.0, 100, G_Dt);
-        SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft,  0.0, 100, G_Dt);
-        SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, 0.0, 100, G_Dt);
-        return;
+        return 0.0;
     }
 
     uint8_t slewrate = aparm.throttle_slewrate;
@@ -57,6 +54,13 @@ void Plane::throttle_slew_limit()
         slewrate = g.takeoff_throttle_slewrate;
     }
 #endif
+    return slewrate;
+}
+
+// Apply throttle slew limit
+void Plane::throttle_slew_limit()
+{
+    const float slewrate = get_throttle_slewrate();
     SRV_Channels::set_slew_rate(SRV_Channel::k_throttle,      slewrate, 100, G_Dt);
     SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft,  slewrate, 100, G_Dt);
     SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, slewrate, 100, G_Dt);

ArduPlane/tailsitter.cpp

@@ -336,7 +336,7 @@ void Tailsitter::output(void)
 
         if (!quadplane.assisted_flight) {
             // set AP_MotorsMatrix throttles for forward flight
-            motors->output_motor_mask(throttle, motor_mask, plane.rudder_dt);
+            motors->output_motor_mask(throttle, motor_mask, plane.rudder_dt, plane.get_throttle_slewrate());
 
             // No tilt output unless forward gain is set
             float tilt_left = 0.0;

ArduPlane/tiltrotor.cpp

@@ -246,7 +246,7 @@ void Tiltrotor::continuous_update(void)
         if (!quadplane.motor_test.running) {
             // the motors are all the way forward, start using them for fwd thrust
             const uint16_t mask = is_zero(current_throttle)?0U:tilt_mask.get();
-            motors->output_motor_mask(current_throttle, mask, plane.rudder_dt);
+            motors->output_motor_mask(current_throttle, mask, plane.rudder_dt, plane.get_throttle_slewrate());
         }
         return;
     }
@@ -367,7 +367,7 @@ void Tiltrotor::binary_update(void)
         if (current_tilt >= 1) {
             const uint16_t mask = is_zero(new_throttle)?0U:tilt_mask.get();
             // the motors are all the way forward, start using them for fwd thrust
-            motors->output_motor_mask(new_throttle, mask, plane.rudder_dt);
+            motors->output_motor_mask(new_throttle, mask, plane.rudder_dt, plane.get_throttle_slewrate());
         }
     } else {
         binary_slew(false);

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