From 682e7edb203fdb5fbd85f8c15dd22e949fe6ef1f Mon Sep 17 00:00:00 2001 From: TheJackiMonster Date: Sun, 23 Apr 2023 13:55:28 +0200 Subject: [PATCH] Adjust code using calibration Signed-off-by: TheJackiMonster --- interface_lib/src/device3.c | 37 +++++++++++++++++++++++++++++++++---- src/driver.c | 22 ---------------------- 2 files changed, 33 insertions(+), 26 deletions(-) diff --git a/interface_lib/src/device3.c b/interface_lib/src/device3.c index eb06a06..7ed4868 100644 --- a/interface_lib/src/device3.c +++ b/interface_lib/src/device3.c @@ -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, diff --git a/src/driver.c b/src/driver.c index 8fc8524..79366ba 100644 --- a/src/driver.c +++ b/src/driver.c @@ -107,28 +107,6 @@ int main(int argc, const char** argv) { if (pid == 0) { device3_type* dev3 = device3_open(test3); - printf("Load calibration:\n"); - device3_load_calibration(dev3, "nreal_air_calibration.dat"); - - /* - printf("Calibrating...\n"); - printf("Now hold the device steady!\n"); - sleep(1); - - device3_calibrate(dev3, 200, true, false, false); - - printf("Calibrating...\n"); - printf("Now rotate the device around!\n"); - sleep(3); - - device3_calibrate(dev3, 20000, false, false, true); - - printf("Calibrating finished!\n"); - - printf("Save calibration:\n"); - device3_save_calibration(dev3, "nreal_air_calibration.dat"); - */ - while (dev3) { if (device3_read(dev3, 0) < 0) { break;