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

Adjust magnometer and flip gravity acceleration

Browse source 
Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>
This commit is contained in:
TheJackiMonster 2023-04-05 02:06:03 +02:00
parent 5aa71fb488
commit d3d63a30a4
No known key found for this signature in database
GPG key ID: D850A5F772E880F9
5 changed files with 58 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.gitmodules vendored
View file

@ -1,3 +1,3 @@
[submodule "modules/Fusion"]
path = modules/Fusion
url = https://github.com/xioTechnologies/Fusion.git
url = https://github.com/TheJackiMonster/Fusion.git

6
interface_lib/include/device3.h
View file

@ -39,9 +39,9 @@ struct __attribute__((__packed__)) device3_packet_t {
uint8_t acceleration_y [3];
uint8_t acceleration_z [3];
uint8_t _padding3 [6];
int16_t magnetic_x;
int16_t magnetic_y;
int16_t magnetic_z;
uint8_t magnetic_x [2];
uint8_t magnetic_y [2];
uint8_t magnetic_z [2];
uint32_t checksum;
uint8_t _padding4 [6];
};

27
interface_lib/src/device3.c
View file

@ -210,6 +210,8 @@ void device3_reset_calibration(device3_type* device) {
device->calibration->accelerometerSensitivity = FUSION_VECTOR_ONES;
device->calibration->accelerometerOffset = FUSION_VECTOR_ZERO;
device->calibration->accelerometerMisalignment.array[2][2] = -1.0f;
device->calibration->softIronMatrix = FUSION_IDENTITY_MATRIX;
device->calibration->hardIronOffset = FUSION_VECTOR_ZERO;
}
@ -262,7 +264,6 @@ int device3_save_calibration(device3_type* device, const char* path) {
}
size_t count = fwrite(device->calibration, 1, sizeof(device3_calibration_type), file);
printf("count: %lu : %lu\n", count, sizeof(device3_calibration_type));
if (sizeof(device3_calibration_type) != count) {
perror("Not fully saved!\n");
@ -293,6 +294,11 @@ static int32_t pack24bit_signed(const uint8_t* data) {
return (int32_t) unsigned_value;
}
static int16_t pack16bit_signed(const uint8_t* data) {
uint16_t unsigned_value = (data[0] << 8) | (data[1]);
return (int16_t) unsigned_value;
}
// based on 24bit signed int w/ FSR = +/-2000 dps, datasheet option
#define GYRO_SCALAR (1.0f / 8388608.0f * 2000.0f)
@ -322,9 +328,9 @@ static void readIMU_from_packet(const device3_packet_type* packet,
accelerometer->axis.y = (float) accel_y * ACCEL_SCALAR;
accelerometer->axis.z = (float) accel_z * ACCEL_SCALAR;
int16_t magnet_x = packet->magnetic_x;
int16_t magnet_y = packet->magnetic_y;
int16_t magnet_z = packet->magnetic_z;
int16_t magnet_x = pack16bit_signed(packet->magnetic_x);
int16_t magnet_y = pack16bit_signed(packet->magnetic_y);
int16_t magnet_z = pack16bit_signed(packet->magnetic_z);
magnetometer->axis.x = (float) magnet_x * MAGNO_SCALAR;
magnetometer->axis.y = (float) magnet_y * MAGNO_SCALAR;
@ -369,6 +375,8 @@ static void apply_calibration(const device3_type* device,
accelerometerSensitivity = FUSION_VECTOR_ONES;
accelerometerOffset = FUSION_VECTOR_ZERO;
accelerometerMisalignment.array[2][2] = -1.0f;
softIronMatrix = FUSION_IDENTITY_MATRIX;
hardIronOffset = FUSION_VECTOR_ZERO;
}
@ -452,11 +460,18 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
FusionVector magnetometer;
readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer);
apply_calibration(device, &gyroscope, &accelerometer, &magnetometer);
if (initialized) {
cal_gyroscope = FusionVectorAdd(cal_gyroscope, gyroscope);
cal_accelerometer = FusionVectorAdd(cal_accelerometer, accelerometer);
} else {
cal_gyroscope = gyroscope;
cal_accelerometer = accelerometer;
}
apply_calibration(device, &gyroscope, &accelerometer, &magnetometer);
if (initialized) {
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.z = min(cal_magnetometer[0].axis.z, magnetometer.axis.z);
@ -464,8 +479,6 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
cal_magnetometer[1].axis.y = max(cal_magnetometer[1].axis.y, magnetometer.axis.y);
cal_magnetometer[1].axis.z = max(cal_magnetometer[1].axis.z, magnetometer.axis.z);
} else {
cal_gyroscope = gyroscope;
cal_accelerometer = accelerometer;
cal_magnetometer[0] = magnetometer;
cal_magnetometer[1] = magnetometer;
initialized = true;

2
modules/Fusion

@ -1 +1 @@
Subproject commit 6c0de47eb6da355985c86030e78e552ab588e049
Subproject commit 55cbd59d56fbf3b83251806efb5e852fb146f4d2

34
src/driver.c
View file

@ -29,17 +29,49 @@
#include <sys/wait.h>
#include <unistd.h>
#include <math.h>
void test3(uint64_t timestamp,
device3_event_type event,
const device3_ahrs_type* ahrs) {
static device3_quat_type old;
static float dmax = -1.0f;
if (event != DEVICE3_EVENT_UPDATE) {
return;
}
device3_quat_type q = device3_get_orientation(ahrs);
const float dx = (old.x - q.x) * (old.x - q.x);
const float dy = (old.y - q.y) * (old.y - q.y);
const float dz = (old.z - q.z) * (old.z - q.z);
const float dw = (old.w - q.w) * (old.w - q.w);
const float d = sqrtf(dx*dx + dy*dy + dz*dz + dw*dw);
if (dmax < 0.0f) {
dmax = 0.0f;
} else {
dmax = (d > dmax? d : dmax);
}
device3_vec3_type e = device3_get_euler(q);
printf("Pitch: %f; Roll: %f; Yaw: %f\n", e.x, e.y, e.z);
if (d >= 0.00005f) {
device3_vec3_type e0 = device3_get_euler(old);
printf("Pitch: %f; Roll: %f; Yaw: %f\n", e0.x, e0.y, e0.z);
printf("Pitch: %f; Roll: %f; Yaw: %f\n", e.x, e.y, e.z);
printf("D = %f; ( %f )\n", d, dmax);
printf("X: %f; Y: %f; Z: %f; W: %f;\n", old.x, old.y, old.z, old.w);
printf("X: %f; Y: %f; Z: %f; W: %f;\n", q.x, q.y, q.z, q.w);
} else {
printf("Pitch: %.2f; Roll: %.2f; Yaw: %.2f\n", e.x, e.y, e.z);
}
old = q;
}
void test4(uint32_t timestamp,