Add filters for precision loss and reducing potential drift

Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>

interface_lib/include/device3.h

@@ -66,6 +66,12 @@ enum device3_event_t {
 struct device3_ahrs_t;
 struct device3_calibration_t;
 
+struct device3_filters_t {
+	float angular_velocity [3 * 4];
+	float acceleration [3 * 4];
+	float magnetic [3 * 4];
+};
+
 struct device3_vec3_t {
 	float x;
 	float y;
@@ -84,6 +90,7 @@ typedef enum device3_event_t device3_event_type;
 
 typedef struct device3_ahrs_t device3_ahrs_type;
 typedef struct device3_calibration_t device3_calibration_type;
+typedef struct device3_filters_t device3_filters_type;
 
 typedef struct device3_vec3_t device3_vec3_type;
 typedef struct device3_quat_t device3_quat_type;
@@ -119,6 +126,7 @@ struct device3_t {
 	
 	device3_event_callback callback;
 	device3_calibration_type* calibration;
+	device3_filters_type filters;
 };
 
 typedef struct device3_t device3_type;

interface_lib/src/device3.c

@@ -24,6 +24,7 @@
 
 #include "device3.h"
 
+#include <float.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -44,6 +45,17 @@ struct device3_calibration_t {
 	FusionVector hardIronOffset;
 };
 
+static void reset_filter_v3(float* filter) {
+	for (uint8_t i = 0; i < 3; i++) {
+		const uint8_t i4 = i * 4;
+		
+		filter[i4 + 0] = -FLT_MAX;
+		filter[i4 + 1] = -FLT_MIN;
+		filter[i4 + 2] = +FLT_MAX;
+		filter[i4 + 3] = +FLT_MIN;
+	}
+}
+
 device3_type* device3_open(device3_event_callback callback) {
 	device3_type* device = (device3_type*) malloc(sizeof(device3_type));
 	
@@ -179,6 +191,10 @@ device3_type* device3_open(device3_event_callback callback) {
 	device->calibration = malloc(sizeof(device3_calibration_type));
 	device3_reset_calibration(device);
 	
+	reset_filter_v3(device->filters.angular_velocity);
+	reset_filter_v3(device->filters.acceleration);
+	reset_filter_v3(device->filters.magnetic);
+	
 	const FusionAhrsSettings settings = {
 			.convention = FusionConventionNwu,
 			.gain = 0.5f,
@@ -522,6 +538,40 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
 	return 0;
 }
 
+static void update_filter_v3(float* filter, FusionVector v3) {
+	for (uint8_t i = 0; i < 3; i++) {
+		const float v = v3.array[i];
+		const uint8_t i4 = i * 4;
+		
+		if (v <= -0.0f) {
+			filter[i4 + 0] = max(v, filter[i4 + 0]);
+			filter[i4 + 1] = min(v, filter[i4 + 1]);
+		}
+		
+		if (v >= +0.0f) {
+			filter[i4 + 2] = min(v, filter[i4 + 2]);
+			filter[i4 + 3] = max(v, filter[i4 + 3]);
+		}
+	}
+}
+
+static void apply_filter_v3(const float* filter, FusionVector* v3) {
+	for (uint8_t i = 0; i < 3; i++) {
+		float v = v3->array[i];
+		const uint8_t i4 = i * 4;
+		
+		if ((v <= -0.0f) && (filter[i4 + 1] < filter[i4 + 0])) {
+			v = (v - filter[i4 + 0]) * filter[i4 + 1] / (filter[i4 + 1] - filter[i4 + 0]);
+		}
+		
+		if ((v >= +0.0f) && (filter[i4 + 3] > filter[i4 + 2])) {
+			v = (v - filter[i4 + 2]) * filter[i4 + 3] / (filter[i4 + 3] - filter[i4 + 2]);
+		}
+		
+		v3->array[i] = v;
+	}
+}
+
 int device3_read(device3_type* device, int timeout) {
 	if (!device) {
 		perror("No device!\n");
@@ -586,6 +636,14 @@ int device3_read(device3_type* device, int timeout) {
 	
 	gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope);
 	
+	update_filter_v3(device->filters.angular_velocity, gyroscope);
+	update_filter_v3(device->filters.acceleration, accelerometer);
+	update_filter_v3(device->filters.magnetic, magnetometer);
+	
+	apply_filter_v3(device->filters.angular_velocity, &gyroscope);
+	apply_filter_v3(device->filters.acceleration, &accelerometer);
+	apply_filter_v3(device->filters.magnetic, &magnetometer);
+	
 	FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
 	
 	device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE, device->ahrs);