UnrealXR/nrealAirLinuxDriver
Archived
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add filters for precision loss and reducing potential drift

Browse source 
Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>
This commit is contained in:
TheJackiMonster 2023-04-05 23:36:33 +02:00
parent 17f944ae5e
commit 3f91741cfa
No known key found for this signature in database
GPG key ID: D850A5F772E880F9
2 changed files with 66 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

58
interface_lib/src/device3.c
View file

@ -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);