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

Merge axis swapping and abstract magnetometer calibration step

Browse source 
Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>
This commit is contained in:
TheJackiMonster 2023-11-30 15:47:23 +01:00
parent b01eef4fcd
commit 86169959a3
No known key found for this signature in database
GPG key ID: D850A5F772E880F9
1 changed files with 50 additions and 49 deletions
Download patch file Download diff file Expand all files Collapse all files

99
interface_lib/src/device3.c
View file

@ -529,7 +529,35 @@ static void pre_biased_coordinate_system(FusionVector* v) {
} }
static void post_biased_coordinate_system(const FusionVector* v, FusionVector* res) { static void post_biased_coordinate_system(const FusionVector* v, FusionVector* res) {
*res = FusionAxesSwap(*v, FusionAxesAlignmentPXNYNZ); *res = FusionAxesSwap(*v, FusionAxesAlignmentNZNXPY);
}
static void iterate_iron_offset_estimation(const FusionVector* magnetometer, FusionMatrix* softIronMatrix, FusionVector* hardIronOffset) {
static FusionVector max = { FLT_MIN, FLT_MIN, FLT_MIN };
static FusionVector min = { FLT_MAX, FLT_MAX, FLT_MAX };
for (int i = 0; i < 3; i++) {
max.array[i] = max(max.array[i], magnetometer->array[i]);
min.array[i] = min(min.array[i], magnetometer->array[i]);
}
const float mx = (max.axis.x - min.axis.x) / 2.0f;
const float my = (max.axis.y - min.axis.y) / 2.0f;
const float mz = (max.axis.z - min.axis.z) / 2.0f;
const float cx = (min.axis.x + max.axis.x) / 2.0f;
const float cy = (min.axis.y + max.axis.y) / 2.0f;
const float cz = (min.axis.z + max.axis.z) / 2.0f;
memset(softIronMatrix, 0, sizeof(*softIronMatrix));
softIronMatrix->element.xx = 1.0f / mx;
softIronMatrix->element.yy = 1.0f / my;
softIronMatrix->element.zz = 1.0f / mz;
hardIronOffset->axis.x = cx;
hardIronOffset->axis.y = cy;
hardIronOffset->axis.z = cz;
} }
static void apply_calibration(const device3_type* device, static void apply_calibration(const device3_type* device,
@ -622,27 +650,11 @@ static void apply_calibration(const device3_type* device,
magnetometerOffset magnetometerOffset
); );
static FusionVector max = { FLT_MIN, FLT_MIN, FLT_MIN }, min = { FLT_MAX, FLT_MAX, FLT_MAX }; iterate_iron_offset_estimation(
for (int i = 0; i < 3; i++) { &m,
max.array[i] = max(max.array[i], m.array[i]); &softIronMatrix,
min.array[i] = min(min.array[i], m.array[i]); &hardIronOffset
} );
const float mx = (max.axis.x - min.axis.x) / 2.0f;
const float my = (max.axis.y - min.axis.y) / 2.0f;
const float mz = (max.axis.z - min.axis.z) / 2.0f;
const float cx = (min.axis.x + max.axis.x) / 2.0f;
const float cy = (min.axis.y + max.axis.y) / 2.0f;
const float cz = (min.axis.z + max.axis.z) / 2.0f;
softIronMatrix.element.xx = 1.0f / mx;
softIronMatrix.element.yy = 1.0f / my;
softIronMatrix.element.zz = 1.0f / mz;
hardIronOffset.axis.x = cx;
hardIronOffset.axis.y = cy;
hardIronOffset.axis.z = cz;
if (device->calibration) { if (device->calibration) {
device->calibration->softIronMatrix = softIronMatrix; device->calibration->softIronMatrix = softIronMatrix;
@ -658,12 +670,6 @@ static void apply_calibration(const device3_type* device,
post_biased_coordinate_system(&g, gyroscope); post_biased_coordinate_system(&g, gyroscope);
post_biased_coordinate_system(&a, accelerometer); post_biased_coordinate_system(&a, accelerometer);
post_biased_coordinate_system(&m, magnetometer); post_biased_coordinate_system(&m, magnetometer);
const FusionAxesAlignment alignment = FusionAxesAlignmentPZPXPY;
*gyroscope = FusionAxesSwap(*gyroscope, alignment);
*accelerometer = FusionAxesSwap(*accelerometer, alignment);
*magnetometer = FusionAxesSwap(*magnetometer, alignment);
} }
device3_error_type device3_clear(device3_type* device) { device3_error_type device3_clear(device3_type* device) {
@ -680,6 +686,11 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations,
device3_error("No handle"); device3_error("No handle");
return DEVICE3_ERROR_NO_HANDLE; return DEVICE3_ERROR_NO_HANDLE;
} }
if (!device->calibration) {
device3_error("No calibration allocated");
return DEVICE3_ERROR_NO_ALLOCATION;
}
if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) { if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) {
device3_error("Not proper size"); device3_error("Not proper size");
@ -693,7 +704,9 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations,
FusionVector cal_gyroscope; FusionVector cal_gyroscope;
FusionVector cal_accelerometer; FusionVector cal_accelerometer;
FusionVector cal_magnetometer [2];
FusionMatrix softIronMatrix;
FusionVector hardIronOffset;
const float factor = iterations > 0? 1.0f / ((float) iterations) : 0.0f; const float factor = iterations > 0? 1.0f / ((float) iterations) : 0.0f;
@ -744,19 +757,12 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations,
} }
prev_accel = accelerometer; prev_accel = accelerometer;
if (initialized) { iterate_iron_offset_estimation(
cal_magnetometer[0].axis.x = min(cal_magnetometer[0].axis.x, magnetometer.axis.x); &magnetometer,
cal_magnetometer[0].axis.y = min(cal_magnetometer[0].axis.y, magnetometer.axis.y); &softIronMatrix,
cal_magnetometer[0].axis.z = min(cal_magnetometer[0].axis.z, magnetometer.axis.z); &hardIronOffset
cal_magnetometer[1].axis.x = max(cal_magnetometer[1].axis.x, magnetometer.axis.x); );
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_magnetometer[0] = magnetometer;
cal_magnetometer[1] = magnetometer;
initialized = true;
}
iterations--; iterations--;
} }
@ -783,13 +789,8 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations,
} }
if (magnet) { if (magnet) {
device->calibration->hardIronOffset = FusionVectorAdd( device->calibration->softIronMatrix = softIronMatrix;
device->calibration->hardIronOffset, device->calibration->hardIronOffset = hardIronOffset;
FusionVectorMultiplyScalar(
FusionVectorAdd(cal_magnetometer[0], cal_magnetometer[1]),
0.5f
)
);
} }
} }