diff --git a/libraries/Filter/SlewCalculator2D.cpp b/libraries/Filter/SlewCalculator2D.cpp new file mode 100644 index 0000000000..06a1555996 --- /dev/null +++ b/libraries/Filter/SlewCalculator2D.cpp @@ -0,0 +1,43 @@ +/* + 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 . + */ +/* + The SlewCalculator2D filter calculates a slew rate useful for detecting + oscillations in a 2-axis PID controller. + */ +#include "SlewCalculator2D.h" + +SlewCalculator2D::SlewCalculator2D() : + xlimiter(slew_rate_max, slew_rate_tau), + ylimiter(slew_rate_max, slew_rate_tau) +{ +} + +// apply filter to sample and calculate slewrate +void SlewCalculator2D::update(const Vector2f& sample, float dt) +{ + if (!is_positive(dt)) { + return; + } + + // call x and y slew rate limiter + xlimiter.modifier(sample.x, dt); + ylimiter.modifier(sample.y, dt); +} + +// get last oscillation slew rate +float SlewCalculator2D::get_slew_rate() const +{ + return safe_sqrt(sq(xlimiter.get_slew_rate()) + sq(xlimiter.get_slew_rate())); +} diff --git a/libraries/Filter/SlewCalculator2D.h b/libraries/Filter/SlewCalculator2D.h new file mode 100644 index 0000000000..7402dd39e5 --- /dev/null +++ b/libraries/Filter/SlewCalculator2D.h @@ -0,0 +1,30 @@ +#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 +#include +#include "SlewLimiter.h" + +class SlewCalculator2D { +public: + SlewCalculator2D(); + + CLASS_NO_COPY(SlewCalculator2D); + + // apply filter to sample and calculate slewrate + void update(const Vector2f& sample, float dt); + + // get last oscillation slew rate + float get_slew_rate() const; + +private: + SlewLimiter xlimiter; // X-axis 1D slew rate limiter + SlewLimiter ylimiter; // Y-axis 1D slew rate limiter + + float slew_rate_max = 0; // slew rate max (always zero) + float slew_rate_tau = 1.0; // slew rate tau (always 1.0) +};