auto-sync

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

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_ */

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