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 Initiailzing the high pass filter with filter coefficient & desired scale
+ * 
+ * @param alpha filter coefficient controlling freq response
+ * @param scale desired scaling for filter
+ * @param filter pointer to a new high pass struct
+ */
+void high_pass_init(float alpha, float scale, high_pass_t *filter);
+
+/**
+ * @brief Function applying filter to a new sample, returning the filtered output
+ * 
+ * @param filter passing pointer to initialized high pass struct
+ * @param input new sample to be filtered
+ * @return float Filtered & Scaled output value based on prev values
+ */
+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;
+}