AP_Motors: calculate throttle slew rate

use derivative filter and apply filtering to slew detector

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -291,6 +291,8 @@ void AP_MotorsMulticopter::output_min()
 // update the throttle input filter
 void AP_MotorsMulticopter::update_throttle_filter()
 {
+    const float last_thr = _throttle_filter.get();
+
     if (armed()) {
         _throttle_filter.apply(_throttle_in, _dt);
         // constrain filtered throttle
@@ -303,6 +305,16 @@ void AP_MotorsMulticopter::update_throttle_filter()
     } else {
         _throttle_filter.reset(0.0f);
     }
+
+    float new_thr = _throttle_filter.get();
+
+    if (!is_equal(last_thr, new_thr)) {
+        _throttle_slew.update(new_thr, AP_HAL::micros());
+    }
+
+    // calculate slope normalized from per-micro
+    const float rate = fabsf(_throttle_slew.slope() * 1e6);
+    _throttle_slew_rate = _throttle_slew_filter.apply(rate, 1.0f / _loop_rate);
 }
 
 // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -19,6 +19,8 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Notify/AP_Notify.h>
 
+#define AP_MOTORS_SLEW_FILTER_CUTOFF 50.0f
+
 extern const AP_HAL::HAL& hal;
 
 // singleton instance
@@ -28,6 +30,8 @@ AP_Motors *AP_Motors::_singleton;
 AP_Motors::AP_Motors(uint16_t speed_hz) :
     _speed_hz(speed_hz),
     _throttle_filter(),
+    _throttle_slew(),
+    _throttle_slew_filter(),
     _spool_desired(DesiredSpoolState::SHUT_DOWN),
     _spool_state(SpoolState::SHUT_DOWN),
     _air_density_ratio(1.0f)
@@ -38,6 +42,12 @@ AP_Motors::AP_Motors(uint16_t speed_hz) :
     _throttle_filter.set_cutoff_frequency(0.0f);
     _throttle_filter.reset(0.0f);
 
+    _throttle_slew_filter.set_cutoff_frequency(AP_MOTORS_SLEW_FILTER_CUTOFF);
+    _throttle_slew_filter.reset(0.0f);
+
+    // setup throttle slew detector
+    _throttle_slew.reset();
+
     // init limit flags
     limit.roll = true;
     limit.pitch = true;

libraries/AP_Motors/AP_Motors_Class.h

@@ -3,6 +3,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #include <Filter/Filter.h>         // filter library
+#include <Filter/DerivativeFilter.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 
 // offsets for motors in motor_out and _motor_filtered arrays
@@ -137,6 +138,7 @@ public:
     void                set_throttle(float throttle_in) { _throttle_in = throttle_in; };   // range 0 ~ 1
     void                set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max, 0.0f, 1.0f); };   // range 0 ~ 1
     void                set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
+    void                set_slew_filter_cutoff(float filt_hz) { _throttle_slew_filter.set_cutoff_frequency(filt_hz); }
     void                set_forward(float forward_in) { _forward_in = forward_in; }; // range -1 ~ +1
     void                set_lateral(float lateral_in) { _lateral_in = lateral_in; };     // range -1 ~ +1
 
@@ -153,6 +155,7 @@ public:
     float               get_throttle_out() const { return _throttle_out; }
     float               get_throttle() const { return constrain_float(_throttle_filter.get(), 0.0f, 1.0f); }
     float               get_throttle_bidirectional() const { return constrain_float(2 * (_throttle_filter.get() - 0.5f), -1.0f, 1.0f); }
+    float               get_throttle_slew_rate() const { return _throttle_slew_rate; }
     float               get_forward() const { return _forward_in; }
     float               get_lateral() const { return _lateral_in; }
     virtual float       get_throttle_hover() const = 0;
@@ -306,10 +309,13 @@ protected:
     float               _yaw_in_ff;                 // desired yaw feed forward control from attitude controller, -1 ~ +1
     float               _throttle_in;               // last throttle input from set_throttle caller
     float               _throttle_out;              // throttle after mixing is complete
+    float               _throttle_slew_rate;        // throttle slew rate from input
     float               _forward_in;                // last forward input from set_forward caller
     float               _lateral_in;                // last lateral input from set_lateral caller
     float               _throttle_avg_max;          // last throttle input from set_throttle_avg_max
-    LowPassFilterFloat  _throttle_filter;           // throttle input filter
+    LowPassFilterFloat  _throttle_filter;           // pilot throttle input filter
+    DerivativeFilterFloat_Size7  _throttle_slew;    // throttle output slew detector
+    LowPassFilterFloat  _throttle_slew_filter;      // filter for the output of the throttle slew
     DesiredSpoolState   _spool_desired;             // desired spool state
     SpoolState          _spool_state;               // current spool mode