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

Remove wrong filters and correction

Browse source 
Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>
This commit is contained in:
TheJackiMonster 2023-04-21 21:30:16 +02:00
parent 43e37b17dd
commit b20a24bf66
No known key found for this signature in database
GPG key ID: D850A5F772E880F9
4 changed files with 11 additions and 100 deletions
Download patch file Download diff file Expand all files Collapse all files

5
examples/debug_d3/src/debug.c
View file

@ -28,8 +28,7 @@
void test3(uint64_t timestamp, void test3(uint64_t timestamp,
device3_event_type event, device3_event_type event,
const device3_ahrs_type* ahrs, const device3_ahrs_type* ahrs) {
const device3_correction_type* correction) {
device3_quat_type orientation; device3_quat_type orientation;
device3_vec3_type euler; device3_vec3_type euler;
@ -38,7 +37,7 @@ void test3(uint64_t timestamp,
printf("Initialized\n"); printf("Initialized\n");
break; break;
case DEVICE3_EVENT_UPDATE: case DEVICE3_EVENT_UPDATE:
orientation = device3_get_orientation(ahrs, correction); orientation = device3_get_orientation(ahrs);
euler = device3_get_euler(orientation); euler = device3_get_euler(orientation);
printf("Pitch: %.2f; Roll: %.2f; Yaw: %.2f\n", euler.x, euler.y, euler.z); printf("Pitch: %.2f; Roll: %.2f; Yaw: %.2f\n", euler.x, euler.y, euler.z);
break; break;

18
interface_lib/include/device3.h
View file

@ -69,14 +69,6 @@ enum device3_event_t {
struct device3_ahrs_t; struct device3_ahrs_t;
struct device3_calibration_t; struct device3_calibration_t;
struct device3_filters_t {
float angular_velocity [3 * 4];
float acceleration [3 * 4];
float magnetic [3 * 4];
};
struct device3_correction_t;
struct device3_vec3_t { struct device3_vec3_t {
float x; float x;
float y; float y;
@ -95,8 +87,6 @@ typedef enum device3_event_t device3_event_type;
typedef struct device3_ahrs_t device3_ahrs_type; typedef struct device3_ahrs_t device3_ahrs_type;
typedef struct device3_calibration_t device3_calibration_type; typedef struct device3_calibration_t device3_calibration_type;
typedef struct device3_filters_t device3_filters_type;
typedef struct device3_correction_t device3_correction_type;
typedef struct device3_vec3_t device3_vec3_type; typedef struct device3_vec3_t device3_vec3_type;
typedef struct device3_quat_t device3_quat_type; typedef struct device3_quat_t device3_quat_type;
@ -104,8 +94,7 @@ typedef struct device3_quat_t device3_quat_type;
typedef void (*device3_event_callback)( typedef void (*device3_event_callback)(
uint64_t timestamp, uint64_t timestamp,
device3_event_type event, device3_event_type event,
const device3_ahrs_type* ahrs, const device3_ahrs_type* ahrs
const device3_correction_type* correction
); );
struct device3_t { struct device3_t {
@ -134,8 +123,6 @@ struct device3_t {
device3_event_callback callback; device3_event_callback callback;
device3_calibration_type* calibration; device3_calibration_type* calibration;
device3_filters_type filters;
device3_correction_type* correction;
}; };
typedef struct device3_t device3_type; typedef struct device3_t device3_type;
@ -156,8 +143,7 @@ device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs);
device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs); device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs);
device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs, device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs);
const device3_correction_type* correction);
device3_vec3_type device3_get_euler(device3_quat_type quat); device3_vec3_type device3_get_euler(device3_quat_type quat);

83
interface_lib/src/device3.c
View file

@ -61,6 +61,8 @@ static void reset_filter_v3(float* filter) {
} }
} }
device3_type* device3_open(device3_event_callback callback) { device3_type* device3_open(device3_event_callback callback) {
device3_type* device = (device3_type*) malloc(sizeof(device3_type)); device3_type* device = (device3_type*) malloc(sizeof(device3_type));
@ -196,14 +198,6 @@ device3_type* device3_open(device3_event_callback callback) {
device->calibration = malloc(sizeof(device3_calibration_type)); device->calibration = malloc(sizeof(device3_calibration_type));
device3_reset_calibration(device); device3_reset_calibration(device);
reset_filter_v3(device->filters.angular_velocity);
reset_filter_v3(device->filters.acceleration);
reset_filter_v3(device->filters.magnetic);
device->correction = malloc(sizeof(device3_correction_type));
device->correction->inverseDrift = FUSION_IDENTITY_QUATERNION;
device->correction->stable = FUSION_IDENTITY_QUATERNION;
const FusionAhrsSettings settings = { const FusionAhrsSettings settings = {
.convention = FusionConventionNwu, .convention = FusionConventionNwu,
.gain = 0.5f, .gain = 0.5f,
@ -265,12 +259,6 @@ int device3_load_calibration(device3_type* device, const char* path) {
perror("Not fully loaded!\n"); perror("Not fully loaded!\n");
} }
count = fread(&(device->filters), 1, sizeof(device3_filters_type), file);
if (sizeof(device3_filters_type) != count) {
perror("Not fully loaded!\n");
}
if (0 != fclose(file)) { if (0 != fclose(file)) {
perror("No file closed!\n"); perror("No file closed!\n");
return -4; return -4;
@ -303,12 +291,6 @@ int device3_save_calibration(device3_type* device, const char* path) {
perror("Not fully saved!\n"); perror("Not fully saved!\n");
} }
count = fwrite(&(device->filters), 1, sizeof(device3_filters_type), file);
if (sizeof(device3_filters_type) != count) {
perror("Not fully saved!\n");
}
if (0 != fclose(file)) { if (0 != fclose(file)) {
perror("No file closed!\n"); perror("No file closed!\n");
return -4; return -4;
@ -324,7 +306,7 @@ static void device3_callback(device3_type* device,
return; return;
} }
device->callback(timestamp, event, device->ahrs, device->correction); device->callback(timestamp, event, device->ahrs);
} }
static int32_t pack32bit_signed(const uint8_t* data) { static int32_t pack32bit_signed(const uint8_t* data) {
@ -498,7 +480,7 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
FusionVector cal_accelerometer; FusionVector cal_accelerometer;
FusionVector cal_magnetometer [2]; FusionVector cal_magnetometer [2];
const float factor = iterations > 0? 1.0f / iterations : 0.0f; const float factor = iterations > 0? 1.0f / ((float) iterations) : 0.0f;
while (iterations > 0) { while (iterations > 0) {
memset(&packet, 0, sizeof(device3_packet_type)); memset(&packet, 0, sizeof(device3_packet_type));
@ -542,10 +524,6 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
apply_calibration(device, &gyroscope, &accelerometer, &magnetometer); apply_calibration(device, &gyroscope, &accelerometer, &magnetometer);
update_filter_v3(device->filters.angular_velocity, gyroscope);
update_filter_v3(device->filters.acceleration, accelerometer);
update_filter_v3(device->filters.magnetic, magnetometer);
if (initialized) { if (initialized) {
cal_magnetometer[0].axis.x = min(cal_magnetometer[0].axis.x, magnetometer.axis.x); cal_magnetometer[0].axis.x = min(cal_magnetometer[0].axis.x, magnetometer.axis.x);
cal_magnetometer[0].axis.y = min(cal_magnetometer[0].axis.y, magnetometer.axis.y); cal_magnetometer[0].axis.y = min(cal_magnetometer[0].axis.y, magnetometer.axis.y);
@ -627,34 +605,6 @@ static void apply_filter_drop_v3(const float* filter, FusionVector* v3) {
} }
} }
static void update_correction_with_ahrs(device3_correction_type* correction,
const FusionAhrs* ahrs) {
FusionAhrsFlags flags = FusionAhrsGetFlags(ahrs);
if (flags.magneticRejectionTimeout || flags.accelerationRejectionTimeout) {
FusionQuaternion unstable = FusionAhrsGetQuaternion(ahrs);
const float u2 = (
unstable.element.w * unstable.element.w +
unstable.element.x * unstable.element.x +
unstable.element.y * unstable.element.y +
unstable.element.z * unstable.element.z
);
unstable.element.w *= +1.0f / u2;
unstable.element.x *= -1.0f / u2;
unstable.element.y *= -1.0f / u2;
unstable.element.z *= -1.0f / u2;
correction->inverseDrift = FusionQuaternionMultiply(unstable, correction->stable);
} else {
correction->stable = FusionQuaternionMultiply(
FusionAhrsGetQuaternion(ahrs),
correction->inverseDrift
);
}
}
int device3_read(device3_type* device, int timeout) { int device3_read(device3_type* device, int timeout) {
if (!device) { if (!device) {
perror("No device!\n"); perror("No device!\n");
@ -724,22 +674,8 @@ int device3_read(device3_type* device, int timeout) {
gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope); 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);
apply_filter_drop_v3(device->filters.angular_velocity, &gyroscope);
FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime); FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
if (device->correction) {
update_correction_with_ahrs(device->correction, (const FusionAhrs*) device->ahrs);
}
device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE); device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE);
return 0; return 0;
} }
@ -762,17 +698,8 @@ device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs)
return a; return a;
} }
device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs, device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs) {
const device3_correction_type* correction) {
FusionQuaternion quaternion = FusionAhrsGetQuaternion((const FusionAhrs*) ahrs); FusionQuaternion quaternion = FusionAhrsGetQuaternion((const FusionAhrs*) ahrs);
if (correction) {
quaternion = FusionQuaternionMultiply(
quaternion,
correction->inverseDrift
);
}
device3_quat_type q; device3_quat_type q;
q.x = quaternion.element.x; q.x = quaternion.element.x;
q.y = quaternion.element.y; q.y = quaternion.element.y;

5
src/driver.c
View file

@ -33,8 +33,7 @@
void test3(uint64_t timestamp, void test3(uint64_t timestamp,
device3_event_type event, device3_event_type event,
const device3_ahrs_type* ahrs, const device3_ahrs_type* ahrs) {
const device3_correction_type* correction) {
static device3_quat_type old; static device3_quat_type old;
static float dmax = -1.0f; static float dmax = -1.0f;
@ -42,7 +41,7 @@ void test3(uint64_t timestamp,
return; return;
} }
device3_quat_type q = device3_get_orientation(ahrs, correction); device3_quat_type q = device3_get_orientation(ahrs);
const float dx = (old.x - q.x) * (old.x - q.x); const float dx = (old.x - q.x) * (old.x - q.x);
const float dy = (old.y - q.y) * (old.y - q.y); const float dy = (old.y - q.y) * (old.y - q.y);