Feature/highpass

general/src/pca9539.c

@@ -70,4 +70,4 @@ HAL_StatusTypeDef pca9539_write_pin(pca9539_t *pca, uint8_t reg_type,
 	data_new = (data & ~(1u << pin)) | (buf << pin);
 
 	return pca_write_reg(pca, reg_type, &data_new);
-}
+}

middleware/include/high_pass.h

@@ -0,0 +1,33 @@
+#ifndef HIGH_PASS_H
+#define HIGH_PASS_H
+
+#include <stdio.h>
+
+typedef struct {
+	//Function Parameters
+	float alpha;
+	float scale;
+
+	float prev_output;
+
+} high_pass_t;
+
+/**
+ * @brief 
+ * 
+ * @param alpha 
+ * @param scale 
+ * @param filter 
+ */
+void high_pass_init(float alpha, float scale, high_pass_t *filter);
+
+/**
+ * @brief 
+ * 
+ * @param filter 
+ * @param input 
+ * @return float 
+ */
+float high_pass(high_pass_t *filter, float input);
+
+#endif

middleware/src/high_pass.c

@@ -0,0 +1,21 @@
+#include "high_pass.h"
+
+void high_pass_init(float alpha, float scale, high_pass_t *filter)
+{
+	filter->alpha = alpha;
+	filter->scale = scale;
+
+	filter->prev_output = 0.0;
+}
+
+float high_pass(high_pass_t *filter, float input)
+{
+	float output =
+		filter->scale(input - (filter->alpha * input +
+				       (1 - filter->alpha) * prev_output));
+
+	filter->prev_output =
+		filter->alpha * input + (1 - filter->alpha) * prev_output;
+
+	return output;
+}