mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
Filter: added slew rate filter
This commit is contained in:
parent
d2a4505184
commit
709e1f8f93
libraries/Filter
62
libraries/Filter/SlewLimiter.cpp
Normal file
62
libraries/Filter/SlewLimiter.cpp
Normal file
@ -0,0 +1,62 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
The SlewLimiter filter provides an actuator slew rate limiter for
|
||||
PID controllers. It is used to reduce the P and D gains when the
|
||||
filter detects that the P+D components are pushing the actuator
|
||||
beyond the configured actuator rate limit. This can prevent
|
||||
oscillations that are caused by the output actuation rate going
|
||||
beyond the actuator maximum physical rate, which causes the
|
||||
actuation demand and achieved rate to get out of phase.
|
||||
|
||||
this filter was originally written by Paul Riseborough for fixed
|
||||
wing use. It was adapted for wider use in AC_PID by Andrew Tridgell
|
||||
*/
|
||||
#include "SlewLimiter.h"
|
||||
|
||||
SlewLimiter::SlewLimiter(const float &_slew_rate_max, const float &_slew_rate_tau) :
|
||||
slew_rate_max(_slew_rate_max),
|
||||
slew_rate_tau(_slew_rate_tau)
|
||||
{
|
||||
slew_filter.set_cutoff_frequency(10.0);
|
||||
slew_filter.reset(0.0);
|
||||
}
|
||||
|
||||
/*
|
||||
apply filter to sample, returning multiplier between 0 and 1 to keep
|
||||
output within slew rate
|
||||
*/
|
||||
float SlewLimiter::modifier(float sample, float dt)
|
||||
{
|
||||
if (slew_rate_max <= 0) {
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
// Calculate the slew rate amplitude produced by the unmodified sample
|
||||
// calculate a low pass filtered slew rate
|
||||
float Pterm_slew_rate = slew_filter.apply((fabsf(sample - last_sample)/ dt), dt);
|
||||
|
||||
// rectify and apply a decaying envelope filter. The 10 in the
|
||||
// constrain limits the modifier to be between 0.1 and 1.0, so we
|
||||
// never drop PID gains below 10% of configured value
|
||||
float alpha = 1.0 - constrain_float(dt/slew_rate_tau, 0.0, 1.0);
|
||||
slew_amplitude = constrain_float(Pterm_slew_rate, alpha * slew_amplitude, 10 * slew_rate_max);
|
||||
|
||||
// Calculate the gain adjustment
|
||||
float mod = slew_rate_max / fmaxf(slew_amplitude, slew_rate_max);
|
||||
last_sample = mod * sample;
|
||||
|
||||
return mod;
|
||||
}
|
27
libraries/Filter/SlewLimiter.h
Normal file
27
libraries/Filter/SlewLimiter.h
Normal file
@ -0,0 +1,27 @@
|
||||
#pragma once
|
||||
|
||||
/* slew rate limiting filter. This is used to prevent oscillation of a
|
||||
* controller by modifying the controllers output based on a maximum
|
||||
* slew rate
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include "LowPassFilter.h"
|
||||
|
||||
class SlewLimiter {
|
||||
public:
|
||||
SlewLimiter(const float &slew_rate_max, const float &slew_rate_tau);
|
||||
|
||||
/*
|
||||
apply filter to sample, returning multiplier between 0 and 1 to keep
|
||||
output within slew rate
|
||||
*/
|
||||
float modifier(float sample, float dt);
|
||||
|
||||
private:
|
||||
const float &slew_rate_max;
|
||||
const float &slew_rate_tau;
|
||||
LowPassFilterFloat slew_filter;
|
||||
float slew_amplitude;
|
||||
float last_sample;
|
||||
};
|
Loading…
Reference in New Issue
Block a user