SRV_Channel: add native slew rate limiting

libraries/SRV_Channel/SRV_Channel.h

@@ -367,6 +367,9 @@ public:
     // get scaled output for the given function type.
     static float get_output_scaled(SRV_Channel::Aux_servo_function_t function);
 
+    // get slew limited scaled output for the given function type
+    static float get_slew_limited_output_scaled(SRV_Channel::Aux_servo_function_t function);
+
     // get pwm output for the first channel of the given function type.
     static bool get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value);
 
@@ -381,7 +384,7 @@ public:
     static uint16_t get_output_channel_mask(SRV_Channel::Aux_servo_function_t function);
     
     // limit slew rate to given limit in percent per second
-    static void limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, float dt);
+    static void set_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, uint16_t range, float dt);
 
     // call output_ch() on all channels
     static void output_ch_all(void);
@@ -620,6 +623,16 @@ private:
 
     static bool emergency_stop;
 
+    // linked list for slew rate handling
+    struct slew_list {
+        slew_list(SRV_Channel::Aux_servo_function_t _func) : func(_func) {};
+        const SRV_Channel::Aux_servo_function_t func;
+        float last_scaled_output;
+        float max_change;
+        slew_list * next;
+    };
+    static slew_list *_slew;
+
     // semaphore for multi-thread use of override_counter array
     HAL_Semaphore override_counter_sem;
 };

libraries/SRV_Channel/SRV_Channel_aux.cpp

@@ -542,6 +542,25 @@ float SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function
     return 0;
 }
 
+// get slew limited scaled output for the given function type
+float SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::Aux_servo_function_t function)
+{
+    if (!SRV_Channel::valid_function(function)) {
+        return 0.0;
+    }
+    for (slew_list *slew = _slew; slew; slew = slew->next) {
+        if (slew->func == function) {
+            if (!is_positive(slew->max_change)) {
+                // treat negative or zero slew rate as disabled
+                break;
+            }
+            return constrain_float(functions[function].output_scaled, slew->last_scaled_output - slew->max_change, slew->last_scaled_output + slew->max_change);
+        }
+    }
+    // no slew limiting
+    return functions[function].output_scaled;
+}
+
 /*
   get mask of output channels for a function
  */
@@ -691,33 +710,35 @@ void SRV_Channels::set_output_norm(SRV_Channel::Aux_servo_function_t function, f
   limit slew rate for an output function to given rate in percent per
   second. This assumes output has not yet done to the hal
  */
-void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, float dt)
+void SRV_Channels::set_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, uint16_t range, float dt)
 {
-    if (slew_rate <= 0) {
-        // nothing to do
-        return;
-    }
     if (!SRV_Channel::valid_function(function)) {
         return;
     }
-    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
-        SRV_Channel &c = channels[i];
-        if (c.function == function) {
-            c.calc_pwm(functions[function].output_scaled);
-            uint16_t last_pwm = hal.rcout->read_last_sent(c.ch_num);
-            if (last_pwm == c.get_output_pwm()) {
-                continue;
-            }
-            uint16_t max_change = (c.get_output_max() - c.get_output_min()) * slew_rate * dt * 0.01f;
-            if (max_change == 0 || dt > 1) {
-                // always allow some change. If dt > 1 then assume we
-                // are just starting out, and only allow a small
-                // change for this loop
-                max_change = 1;
-            }
-            c.set_output_pwm(constrain_int16(c.get_output_pwm(), last_pwm-max_change, last_pwm+max_change));
+    const float max_change = range * slew_rate * 0.01 * dt;
+
+    for (slew_list *slew = _slew; slew; slew = slew->next) {
+        if (slew->func == function) {
+            // found existing item, update max change
+            slew->max_change = max_change;
+            return;
         }
     }
+
+    if (!is_positive(max_change)) {
+        // no point in adding a disabled slew limit
+        return;
+    }
+
+    // add new item
+    slew_list *new_slew = new slew_list(function);
+    if (new_slew == nullptr) {
+        return;
+    }
+    new_slew->last_scaled_output = functions[function].output_scaled;
+    new_slew->max_change = max_change;
+    new_slew->next = _slew;
+    _slew = new_slew;
 }
 
 // call set_angle() on matching channels

libraries/SRV_Channel/SRV_Channels.cpp

@@ -68,6 +68,7 @@ bool SRV_Channels::initialised;
 bool SRV_Channels::emergency_stop;
 Bitmask<SRV_Channel::k_nr_aux_servo_functions> SRV_Channels::function_mask;
 SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions];
+SRV_Channels::slew_list *SRV_Channels::_slew;
 
 const AP_Param::GroupInfo SRV_Channels::var_info[] = {
 #if (NUM_SERVO_CHANNELS >= 1)
@@ -306,6 +307,15 @@ void SRV_Channels::setup_failsafe_trim_all_non_motors(void)
  */
 void SRV_Channels::calc_pwm(void)
 {
+    // slew rate limit functions
+    for (slew_list *slew = _slew; slew; slew = slew->next) {
+        if (is_positive(slew->max_change)) {
+            // treat negative or zero slew rate as disabled
+            functions[slew->func].output_scaled = constrain_float(functions[slew->func].output_scaled, slew->last_scaled_output - slew->max_change, slew->last_scaled_output + slew->max_change);
+        }
+        slew->last_scaled_output = functions[slew->func].output_scaled;
+    }
+
     WITH_SEMAPHORE(_singleton->override_counter_sem);
 
     for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {