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)
+};