Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   The SlewLimiter filter provides an actuator slew rate limiter for
      17             :   PID controllers. It is used to reduce the P and D gains when the
      18             :   filter detects that the P+D components are pushing the actuator
      19             :   beyond the configured actuator rate limit. This can prevent
      20             :   oscillations that are caused by the output actuation rate going
      21             :   beyond the actuator maximum physical rate, which causes the
      22             :   actuation demand and achieved rate to get out of phase.
      23             : 
      24             :   this filter was originally written by Paul Riseborough for fixed
      25             :   wing use. It was adapted for wider use in AC_PID by Andrew Tridgell
      26             :  */
      27             : #include "SlewLimiter.h"
      28             : 
      29             : #define WINDOW_MS 300                   // time in msec required for a half cycle of the slowest oscillation frequency expected
      30             : #define MODIFIER_GAIN 1.5f              // ratio of modifier reduction to slew rate exceedance ratio
      31             : #define DERIVATIVE_CUTOFF_FREQ 25.0f
      32             : 
      33        9702 : SlewLimiter::SlewLimiter(const float &_slew_rate_max, const float &_slew_rate_tau) :
      34        9702 :     slew_rate_max(_slew_rate_max),
      35        9702 :     slew_rate_tau(_slew_rate_tau)
      36             : {
      37        9702 :     slew_filter.set_cutoff_frequency(DERIVATIVE_CUTOFF_FREQ);
      38        9702 :     slew_filter.reset(0.0);
      39        9702 : }
      40             : 
      41             : /*
      42             :   apply filter to sample, returning multiplier between 0 and 1 to keep
      43             :   output within slew rate
      44             :  */
      45   144423235 : float SlewLimiter::modifier(float sample, float dt)
      46             : {
      47   144423235 :     if (!is_positive(dt)) {
      48           0 :         return 1.0;
      49             :     }
      50             : 
      51             :     // Calculate a low pass filtered slew rate
      52   144423235 :     const float slew_rate = slew_filter.apply((sample - last_sample) / dt, dt);
      53   144423235 :     last_sample = sample;
      54             : 
      55   144423235 :     uint32_t now_ms = AP_HAL::millis();
      56             : 
      57             :     // Apply a filter to decay maximum seen slew rate once the value had left the window period
      58   144423234 :     const float decay_alpha = fminf(dt, slew_rate_tau) / slew_rate_tau;
      59             : 
      60             :     // Apply a filter to increases in slew rate only to reduce the effect of gusts and large controller setpoint changes
      61   144423234 :     const float attack_alpha = fminf(2.0f * decay_alpha, 1.0f);
      62             : 
      63             :     // Decay the peak positive and negative slew rate if they are outside the window
      64             :     // Never drop PID gains below 10% of configured value
      65   144423234 :     if (slew_rate > _max_pos_slew_rate) {
      66     2486854 :         _max_pos_slew_rate = slew_rate;
      67     2486854 :         _max_pos_slew_event_ms = now_ms;
      68   141936380 :     } else if (now_ms - _max_pos_slew_event_ms > WINDOW_MS) {
      69    79787545 :         _max_pos_slew_rate *= (1.0f - decay_alpha);
      70             :     }
      71             : 
      72   144423234 :     if (-slew_rate > _max_neg_slew_rate) {
      73     2429887 :         _max_neg_slew_rate = -slew_rate;
      74     2429887 :         _max_neg_slew_event_ms = now_ms;
      75   141993347 :     } else if (now_ms - _max_neg_slew_event_ms > WINDOW_MS) {
      76    81397840 :         _max_neg_slew_rate *= (1.0f - decay_alpha);
      77             :     }
      78             : 
      79   144423234 :     const float raw_slew_rate = 0.5f*(_max_pos_slew_rate + _max_neg_slew_rate);
      80   144423234 :     _output_slew_rate = (1.0f - attack_alpha) * _output_slew_rate + attack_alpha * raw_slew_rate;
      81   144423234 :     _output_slew_rate = fminf(_output_slew_rate, raw_slew_rate);
      82             : 
      83   144423234 :     if (slew_rate_max <= 0) {
      84   121127001 :         return 1.0;
      85             :     }
      86             : 
      87             :     // Constrain slew rate used for calculation
      88    23296233 :     const float limited_raw_slew_rate = 0.5f*(fminf(_max_pos_slew_rate, 10.0f * slew_rate_max) + fminf(_max_neg_slew_rate, 10.0f * slew_rate_max));
      89             : 
      90             :     // Store a series of positive slew rate exceedance events
      91    23296233 :     if (!_pos_event_stored && slew_rate > slew_rate_max) {
      92        5969 :         if (_pos_event_index >= SLEWLIMITER_N_EVENTS) {
      93        2809 :             _pos_event_index = 0;
      94             :         }
      95        5969 :         _pos_event_ms[_pos_event_index] = now_ms;
      96        5969 :         _pos_event_index++;
      97        5969 :         _pos_event_stored = true;
      98        5969 :         _neg_event_stored = false;
      99             :     }
     100             : 
     101             :     // Store a series of negative slew rate exceedance events
     102    23296233 :     if (!_neg_event_stored && -slew_rate > slew_rate_max) {
     103        6028 :         if (_neg_event_index >= SLEWLIMITER_N_EVENTS) {
     104        2829 :             _neg_event_index = 0;
     105             :         }
     106        6028 :         _neg_event_ms[_neg_event_index] = now_ms;
     107        6028 :         _neg_event_index++;
     108        6028 :         _neg_event_stored = true;
     109        6028 :         _pos_event_stored = false;
     110             :     }
     111             : 
     112             :     // Find the oldest event time
     113    23296233 :     uint32_t oldest_ms = now_ms;
     114    69888702 :     for (uint8_t index = 0; index < SLEWLIMITER_N_EVENTS; index++) {
     115    46592468 :         oldest_ms = MIN(oldest_ms, _pos_event_ms[index]);
     116    46592468 :         oldest_ms = MIN(oldest_ms, _neg_event_ms[index]);
     117             :     }
     118             : 
     119             :     // Apply a further reduction when the oldest exceedance event falls outside the window required for the
     120             :     // specified number of exceedance events. This prevents spikes due to control mode changed, etc causing
     121             :     // unwanted gain reduction and is only applied to the slew rate used for gain reduction
     122    23296234 :     float modifier_input = limited_raw_slew_rate;
     123    23296234 :     if (now_ms - oldest_ms > (SLEWLIMITER_N_EVENTS + 1) * WINDOW_MS) {
     124    23134673 :         const float oldest_time_from_window = 0.001f*(float)(now_ms - oldest_ms - (SLEWLIMITER_N_EVENTS + 1) * WINDOW_MS);
     125    23134673 :         modifier_input *= expf(-oldest_time_from_window / slew_rate_tau);
     126             :     }
     127             : 
     128    23296234 :     _modifier_slew_rate = (1.0f - attack_alpha) * _modifier_slew_rate + attack_alpha * modifier_input;
     129    23296234 :     _modifier_slew_rate = fminf(_modifier_slew_rate, modifier_input);
     130             : 
     131             :     // Calculate the gain adjustment
     132    23296234 :     float mod = 1.0f;
     133    23296234 :     if (_modifier_slew_rate > slew_rate_max) {
     134      183125 :         mod = slew_rate_max / (slew_rate_max + MODIFIER_GAIN * (_modifier_slew_rate - slew_rate_max));
     135             :     }
     136             : 
     137    23296234 :     return mod;
     138             : }