SRV_Channel: native slew limiting

ArduPlane/Log.cpp

@@ -249,7 +249,7 @@ void Plane::Log_Write_AETR()
         ,elevator : SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)
         ,throttle : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)
         ,rudder   : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder)
-        ,flap     : SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto)
+        ,flap     : SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto)
         ,speed_scaler : get_speed_scaler(),
         };
 

ArduPlane/Plane.h

@@ -1092,7 +1092,7 @@ class Plane : public AP_Vehicle {
     void update_throttle_hover();
     void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in,
                                 SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out) const;
-    void flaperon_update(int8_t flap_percent);
+    void flaperon_update();
 
     // is_flying.cpp
     void update_is_flying_5Hz(void);

ArduPlane/afs_plane.cpp

@@ -24,6 +24,10 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
     if (_terminate_action == TERMINATE_ACTION_LAND) {
         plane.landing.terminate();
     } else {
+        // remove flap slew limiting
+        SRV_Channels::set_slew_rate(SRV_Channel::k_flap_auto, 0.0, 100, plane.G_Dt);
+        SRV_Channels::set_slew_rate(SRV_Channel::k_flap, 0.0, 100, plane.G_Dt);
+
         // aerodynamic termination is the default approach to termination
         SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100.0);
         SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 100.0);

ArduPlane/failsafe.cpp

@@ -99,7 +99,7 @@ void Plane::failsafe_check(void)
         SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 0.0);
 
         // setup flaperons
-        flaperon_update(0);
+        flaperon_update();
 
         servos_output();
 

ArduPlane/radio.cpp

@@ -348,8 +348,8 @@ void Plane::trim_radio()
         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight2);
     }
 
-    if (is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto)) &&
-        is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_flap))) {
+    if (is_zero(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto)) &&
+        is_zero(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap))) {
         // trim flaperons if no flap input
         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_left);
         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_right);

ArduPlane/servos.cpp

@@ -32,6 +32,7 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
 
     if (!do_throttle_slew) {
         // only do throttle slew limiting in modes where throttle control is automatic
+        SRV_Channels::set_slew_rate(func, 0.0, 100, G_Dt);
         return;
     }
 
@@ -54,10 +55,7 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
         slewrate = g.takeoff_throttle_slewrate;
     }
 #endif
-    // if slew limit rate is set to zero then do not slew limit
-    if (slewrate) {                   
-        SRV_Channels::limit_slew_rate(func, slewrate, G_Dt);
-    }
+    SRV_Channels::set_slew_rate(func, slewrate, 100, G_Dt);
 }
 
 /* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
@@ -186,14 +184,15 @@ void Plane::channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, S
 /*
   setup flaperon output channels
  */
-void Plane::flaperon_update(int8_t flap_percent)
+void Plane::flaperon_update()
 {
     /*
       flaperons are implemented as a mixer between aileron and a
       percentage of flaps. Flap input can come from a manual channel
       or from auto flaps.
      */
     float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
+    float flap_percent = SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto);
     float flaperon_left  = constrain_float(aileron + flap_percent * 45, -4500, 4500);
     float flaperon_right = constrain_float(aileron - flap_percent * 45, -4500, 4500);
     SRV_Channels::set_output_scaled(SRV_Channel::k_flaperon_left, flaperon_left);
@@ -275,11 +274,11 @@ void Plane::dspoiler_update(void)
           spoilers to both wings. Get flap percentage from k_flap_auto, which is set
           in set_servos_flaps() as the maximum of manual and auto flap control
          */
-        const int16_t flap_percent = SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto);
+        const float flap_percent = SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto);
 
-        if (flap_percent > 0) {
-            float inner_flap_scaled = (float)flap_percent;
-            float outer_flap_scaled = (float)flap_percent;
+        if (is_positive(flap_percent)) {
+            float inner_flap_scaled = flap_percent;
+            float outer_flap_scaled = flap_percent;
             if (progressive_crow) {
                 // apply 0 - full inner from 0 to 50% flap then add in outer above 50%
                 inner_flap_scaled = constrain_float(inner_flap_scaled * 2, 0,100);
@@ -671,13 +670,11 @@ void Plane::set_servos_flaps(void)
     SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, auto_flap_percent);
     SRV_Channels::set_output_scaled(SRV_Channel::k_flap, manual_flap_percent);
 
-    if (g.flap_slewrate) {
-        SRV_Channels::limit_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate, G_Dt);
-        SRV_Channels::limit_slew_rate(SRV_Channel::k_flap, g.flap_slewrate, G_Dt);
-    }    
+    SRV_Channels::set_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate, 100, G_Dt);
+    SRV_Channels::set_slew_rate(SRV_Channel::k_flap, g.flap_slewrate, 100, G_Dt);
 
     // output to flaperons, if any
-    flaperon_update(auto_flap_percent);
+    flaperon_update();
 }
 
 #if LANDING_GEAR_ENABLED == ENABLED

ArduPlane/tiltrotor.cpp

@@ -202,7 +202,7 @@ float Tiltrotor::get_fully_forward_tilt() const
 // return the target tilt value for forward flight
 float Tiltrotor::get_forward_flight_tilt() const
 {
-    return 1.0 - ((flap_angle_deg / 90.0) * SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto) * 0.01);
+    return 1.0 - ((flap_angle_deg / 90.0) * SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) * 0.01);
 }
 
 /*

libraries/SRV_Channel/SRV_Channel.h

@@ -367,6 +367,9 @@ class SRV_Channels {
     // 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 @@ class SRV_Channels {
     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 @@ class SRV_Channels {
 
     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++) {