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

Adjust code using calibration

Browse source 
Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>
This commit is contained in:
TheJackiMonster 2023-04-23 13:55:28 +02:00
parent 31937982f3
commit 682e7edb20
No known key found for this signature in database
GPG key ID: D850A5F772E880F9
2 changed files with 33 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

37
interface_lib/src/device3.c
View file

@ -43,6 +43,10 @@ struct device3_calibration_t {
FusionVector accelerometerSensitivity;
FusionVector accelerometerOffset;
FusionMatrix magnetometerMisalignment;
FusionVector magnetometerSensitivity;
FusionVector magnetometerOffset;
FusionMatrix softIronMatrix;
FusionVector hardIronOffset;
@ -344,13 +348,12 @@ device3_type* device3_open(device3_event_callback callback) {
FusionVector accel_bias = json_object_get_vector(json_object_object_get(dev1, "accel_bias"));
FusionQuaternion accel_q_gyro = json_object_get_quaternion(json_object_object_get(dev1, "accel_q_gyro"));
FusionVector gyro_bias = json_object_get_vector(json_object_object_get(dev1, "gyro_bias"));
FusionVector gyro_p_mag = json_object_get_vector(json_object_object_get(dev1, "gyro_p_mag"));
FusionQuaternion gyro_q_mag = json_object_get_quaternion(json_object_object_get(dev1, "gyro_q_mag"));
FusionQuaternion imu_noises = json_object_get_quaternion(json_object_object_get(dev1, "imu_noises"));
FusionVector mag_bias = json_object_get_vector(json_object_object_get(dev1, "mag_bias"));
FusionVector scale_accel = json_object_get_vector(json_object_object_get(dev1, "scale_accel"));
FusionVector scale_gyro = json_object_get_vector(json_object_object_get(dev1, "scale_gyro"));
//FusionVector scale_mag = json_object_get_vector(json_object_object_get(dev1, "scale_mag"));
FusionVector scale_mag = json_object_get_vector(json_object_object_get(dev1, "scale_mag"));
FusionMatrix gyro_misalignment = FusionQuaternionToMatrix(accel_q_gyro);
FusionQuaternion accel_q_mag = FusionQuaternionMultiply(accel_q_gyro, gyro_q_mag);
@ -363,10 +366,13 @@ device3_type* device3_open(device3_event_callback callback) {
device->calibration->accelerometerOffset = accel_bias;
FusionMatrix mag_misalignment = FusionQuaternionToMatrix(accel_q_mag);
mag_bias = FusionMatrixMultiplyVector(mag_misalignment, mag_bias);
device->calibration->magnetometerMisalignment = mag_misalignment;
device->calibration->magnetometerSensitivity = scale_mag;
device->calibration->magnetometerOffset = mag_bias;
device->calibration->softIronMatrix = mag_misalignment;
device->calibration->hardIronOffset = FusionVectorAdd(gyro_p_mag, mag_bias);
device->calibration->hardIronOffset = mag_bias;
device->calibration->noises = imu_noises;
@ -419,6 +425,10 @@ void device3_reset_calibration(device3_type* device) {
device->calibration->accelerometerMisalignment.array[2][2] = -1.0f;
device->calibration->magnetometerMisalignment = FUSION_IDENTITY_MATRIX;
device->calibration->magnetometerSensitivity = FUSION_VECTOR_ONES;
device->calibration->magnetometerOffset = FUSION_VECTOR_ZERO;
device->calibration->softIronMatrix = FUSION_IDENTITY_MATRIX;
device->calibration->hardIronOffset = FUSION_VECTOR_ZERO;
@ -579,6 +589,10 @@ static void apply_calibration(const device3_type* device,
FusionVector accelerometerSensitivity;
FusionVector accelerometerOffset;
FusionMatrix magnetometerMisalignment;
FusionVector magnetometerSensitivity;
FusionVector magnetometerOffset;
FusionMatrix softIronMatrix;
FusionVector hardIronOffset;
@ -591,6 +605,10 @@ static void apply_calibration(const device3_type* device,
accelerometerSensitivity = device->calibration->accelerometerSensitivity;
accelerometerOffset = device->calibration->accelerometerOffset;
magnetometerMisalignment = device->calibration->magnetometerMisalignment;
magnetometerSensitivity = device->calibration->magnetometerSensitivity;
magnetometerOffset = device->calibration->magnetometerOffset;
softIronMatrix = device->calibration->softIronMatrix;
hardIronOffset = device->calibration->hardIronOffset;
} else {
@ -604,6 +622,10 @@ static void apply_calibration(const device3_type* device,
accelerometerMisalignment.array[2][2] = -1.0f;
magnetometerMisalignment = FUSION_IDENTITY_MATRIX;
magnetometerSensitivity = FUSION_VECTOR_ONES;
magnetometerOffset = FUSION_VECTOR_ZERO;
softIronMatrix = FUSION_IDENTITY_MATRIX;
hardIronOffset = FUSION_VECTOR_ZERO;
}
@ -622,6 +644,13 @@ static void apply_calibration(const device3_type* device,
accelerometerOffset
);
*magnetometer = FusionCalibrationInertial(
*magnetometer,
magnetometerMisalignment,
magnetometerSensitivity,
magnetometerOffset
);
*magnetometer = FusionCalibrationMagnetic(
*magnetometer,
softIronMatrix,