From c78df4e9845f9d91c99288c8e52c8db80f63487d Mon Sep 17 00:00:00 2001
From: rusEfi <russianefi@gmail.com>
Date: Sat, 10 Sep 2016 16:03:27 -0400
Subject: [PATCH] auto-sync

---
 firmware/controllers/math/biquad.cpp | 26 ++++++++++++++++++++++++++
 firmware/controllers/math/biquad.h   | 20 ++++++++++++++++++++
 firmware/controllers/math/math.mk    |  1 +
 3 files changed, 47 insertions(+)
 create mode 100644 firmware/controllers/math/biquad.cpp
 create mode 100644 firmware/controllers/math/biquad.h

diff --git a/firmware/controllers/math/biquad.cpp b/firmware/controllers/math/biquad.cpp
new file mode 100644
index 0000000000..7d24f32ce7
--- /dev/null
+++ b/firmware/controllers/math/biquad.cpp
@@ -0,0 +1,26 @@
+/*
+ * @file biquad.cpp
+ *
+ * @date Sep 10, 2016
+ * @author Andrey Belomutskiy, (c) 2012-2016
+ */
+
+#include "biquad.h"
+
+Biquad::Biquad() {
+	a0 = 0.0000024635293743901;
+	a1 = 0.00000492705874878021;
+	a2 = 0.0000024635293743901;
+	b1 = -1.9968534854;
+	b2 = 0.9968633396;
+
+	z1 = z2 = 0;
+}
+
+float Biquad::getValue(float input) {
+    float result = input * a0 + z1;
+    z1 = input * a1 + z2 - b1 * result;
+    z2 = input * a2 - b2 * result;
+    return result;
+}
+
diff --git a/firmware/controllers/math/biquad.h b/firmware/controllers/math/biquad.h
new file mode 100644
index 0000000000..2a2edd462b
--- /dev/null
+++ b/firmware/controllers/math/biquad.h
@@ -0,0 +1,20 @@
+/*
+ * @file biquad.h
+ *
+ * @date Sep 10, 2016
+ * @author Andrey Belomutskiy, (c) 2012-2016
+ */
+
+#ifndef CONTROLLERS_MATH_BIQUAD_H_
+#define CONTROLLERS_MATH_BIQUAD_H_
+
+class Biquad {
+public:
+    Biquad();
+    float getValue(float input);
+
+    float a0, a1, a2, b1, b2;
+    float z1, z2;
+};
+
+#endif /* CONTROLLERS_MATH_BIQUAD_H_ */
diff --git a/firmware/controllers/math/math.mk b/firmware/controllers/math/math.mk
index a82c256898..fe41b81d02 100644
--- a/firmware/controllers/math/math.mk
+++ b/firmware/controllers/math/math.mk
@@ -3,4 +3,5 @@ CONTROLLERS_MATH_SRC =
 
 CONTROLLERS_MATH_SRC_CPP = $(PROJECT_DIR)/controllers/math/engine_math.cpp \
 	$(PROJECT_DIR)/controllers/math/pid.cpp \
+	$(PROJECT_DIR)/controllers/math/biquad.cpp \
 	$(PROJECT_DIR)/controllers/math/speed_density.cpp