Skip to content

GitLab

Commit d4e139f0 authored by Jakob Bornecrantz's avatar Jakob Bornecrantz
Browse files

d/psmv: Use gyro_bias from calibration data on ZCM2

parent 548f4ede
Hide whitespace changes
Inline Side-by-side
Showing with 16 additions and 6 deletions
src/xrt/drivers/psmv/psmv_driver.c
View file @ d4e139f0
......@@ -255,18 +255,16 @@ struct psmv_calibration_zcm2
struct psmv_vec3_i16_wire accel_min_y;
struct psmv_vec3_i16_wire accel_max_z;
struct psmv_vec3_i16_wire accel_min_z;
uint16_t _pad0;
uint16_t _pad1;
uint16_t _pad2;
uint16_t _pad3;
uint16_t _pad4;
//! Pretty sure this is gryo bias.
struct psmv_vec3_i16_wire gyro_bias;
uint8_t _pad0[4];
struct psmv_vec3_i16_wire gyro_pos_x;
struct psmv_vec3_i16_wire gyro_pos_y;
struct psmv_vec3_i16_wire gyro_pos_z;
struct psmv_vec3_i16_wire gyro_neg_x;
struct psmv_vec3_i16_wire gyro_neg_y;
struct psmv_vec3_i16_wire gyro_neg_z;
uint16_t _pad[6];
uint8_t _pad1[12];
};
/*!
......@@ -414,6 +412,9 @@ struct psmv_parsed_calibration_zcm2
struct xrt_vec3_i32 gyro_pos_y;
struct xrt_vec3_i32 gyro_neg_z;
struct xrt_vec3_i32 gyro_pos_z;
//! Pretty sure this is gryo bias.
struct xrt_vec3_i32 gyro_bias;
};
/*!
......@@ -1366,6 +1367,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
psmv_from_vec3_i16_wire(&zcm2->gyro_pos_y, &data.gyro_pos_y);
psmv_from_vec3_i16_wire(&zcm2->gyro_neg_z, &data.gyro_neg_z);
psmv_from_vec3_i16_wire(&zcm2->gyro_pos_z, &data.gyro_pos_z);
psmv_from_vec3_i16_wire(&zcm2->gyro_bias, &data.gyro_bias);
/*
......@@ -1405,6 +1407,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
psmv->calibration.gyro.factor.y = (60.0 * gy) / (2.0 * M_PI * 90.0);
psmv->calibration.gyro.factor.z = (60.0 * gz) / (2.0 * M_PI * 90.0);
#if 0
psmv->calibration.gyro.bias.x =
(zcm2->gyro_neg_y.x + zcm2->gyro_pos_y.x + zcm2->gyro_neg_z.x +
zcm2->gyro_pos_z.x) /
......@@ -1417,6 +1420,11 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
(zcm2->gyro_neg_x.z + zcm2->gyro_pos_x.z + zcm2->gyro_neg_y.z +
zcm2->gyro_pos_y.z) /
4.0;
#else
psmv->calibration.gyro.bias.x = zcm2->gyro_bias.x;
psmv->calibration.gyro.bias.y = zcm2->gyro_bias.y;
psmv->calibration.gyro.bias.z = zcm2->gyro_bias.z;
#endif
/*
......@@ -1439,6 +1447,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
"\t\tgyro_pos_y: %6i %6i %6i\n"
"\t\tgyro_neg_z: %6i %6i %6i\n"
"\t\tgyro_pos_z: %6i %6i %6i\n"
"\t\tgyro_bias: %6i %6i %6i\n"
"\tCalculated:\n"
"\t\taccel.factor: %f %f %f\n"
"\t\taccel.bias: %f %f %f\n"
......@@ -1456,6 +1465,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
zcm2->gyro_pos_y.x, zcm2->gyro_pos_y.y, zcm2->gyro_pos_y.z,
zcm2->gyro_neg_z.x, zcm2->gyro_neg_z.y, zcm2->gyro_neg_z.z,
zcm2->gyro_pos_z.x, zcm2->gyro_pos_z.y, zcm2->gyro_pos_z.z,
zcm2->gyro_bias.x, zcm2->gyro_bias.y, zcm2->gyro_bias.z,
psmv->calibration.accel.factor.x, psmv->calibration.accel.factor.y,
psmv->calibration.accel.factor.z, psmv->calibration.accel.bias.x,
psmv->calibration.accel.bias.y, psmv->calibration.accel.bias.z,
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment