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

Align axes with fusion algorithm expectations

Browse source 
Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>
This commit is contained in:
TheJackiMonster 2023-04-24 19:59:07 +02:00
parent d41415b485
commit 7998de05b7
No known key found for this signature in database
GPG key ID: D850A5F772E880F9
1 changed files with 28 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

43
interface_lib/src/device3.c
View file

@ -346,34 +346,22 @@ device3_type* device3_open(device3_event_callback callback) {
struct json_object* dev1 = json_object_object_get(imu, "device_1"); struct json_object* dev1 = json_object_object_get(imu, "device_1");
FusionVector accel_bias = json_object_get_vector(json_object_object_get(dev1, "accel_bias")); 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_bias = json_object_get_vector(json_object_object_get(dev1, "gyro_bias"));
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")); 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 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_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_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);
device->calibration->gyroscopeMisalignment = gyro_misalignment;
device->calibration->gyroscopeSensitivity = scale_gyro; device->calibration->gyroscopeSensitivity = scale_gyro;
device->calibration->gyroscopeOffset = gyro_bias; device->calibration->gyroscopeOffset = gyro_bias;
device->calibration->accelerometerSensitivity = scale_accel; device->calibration->accelerometerSensitivity = scale_accel;
device->calibration->accelerometerOffset = accel_bias; device->calibration->accelerometerOffset = accel_bias;
FusionMatrix mag_misalignment = FusionQuaternionToMatrix(accel_q_mag);
device->calibration->magnetometerMisalignment = mag_misalignment;
device->calibration->magnetometerSensitivity = scale_mag; device->calibration->magnetometerSensitivity = scale_mag;
device->calibration->magnetometerOffset = mag_bias; device->calibration->magnetometerOffset = mag_bias;
device->calibration->softIronMatrix = mag_misalignment;
device->calibration->hardIronOffset = mag_bias;
device->calibration->noises = imu_noises; device->calibration->noises = imu_noises;
json_tokener_free(tokener); json_tokener_free(tokener);
@ -652,15 +640,40 @@ static void apply_calibration(const device3_type* device,
magnetometerOffset magnetometerOffset
); );
static FusionVector max = { -1000, -1000, -1000 }, min = { 1000, 1000, 1000 };
for (int i = 0; i < 3; i++) {
if (magnetometer->array[i] > max.array[i]) max.array[i] = magnetometer->array[i];
if (magnetometer->array[i] < min.array[i]) min.array[i] = magnetometer->array[i];
}
const float mx = 2.0f / (max.axis.x - min.axis.x);
const float my = 2.0f / (max.axis.y - min.axis.y);
const float mz = 2.0f / (max.axis.z - min.axis.z);
if ((mx*mx > 0.0f) && (my*my > 0.0f) && (mz*mz > 0.0f)) {
softIronMatrix.array[0][0] = mx;
softIronMatrix.array[1][1] = my;
softIronMatrix.array[2][2] = mz;
hardIronOffset.axis.x = (min.axis.x + max.axis.x) / (2.0f * mz);
hardIronOffset.axis.y = (min.axis.y + max.axis.y) / (2.0f * my);
hardIronOffset.axis.z = (min.axis.z + max.axis.z) / (2.0f * mz);
device->calibration->softIronMatrix = softIronMatrix;
device->calibration->hardIronOffset = hardIronOffset;
}
*magnetometer = FusionCalibrationMagnetic( *magnetometer = FusionCalibrationMagnetic(
*magnetometer, *magnetometer,
softIronMatrix, softIronMatrix,
hardIronOffset hardIronOffset
); );
gyroscope->axis.z *= -1.0f; const FusionAxesAlignment alignment = FusionAxesAlignmentNXNYPZ;
accelerometer->axis.z *= -1.0f;
magnetometer->axis.z *= -1.0f; *gyroscope = FusionAxesSwap(*gyroscope, alignment);
*accelerometer = FusionAxesSwap(*accelerometer, alignment);
*magnetometer = FusionAxesSwap(*magnetometer, alignment);
} }
void device3_clear(device3_type* device) { void device3_clear(device3_type* device) {