mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-01 12:46:12 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/wmr: Extend config reading to include bias/mix_matrix

Browse source 
Add extraction of bias offsets and mix_matrix (rotation)
correction for IMU accel, gyro and mag configuration entries.
This commit is contained in:
Jan Schmidt 2021-12-03 15:54:38 +11:00
parent 385a3bd33d
commit ee030ff0c8
3 changed files with 83 additions and 30 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

87
src/xrt/drivers/wmr/wmr_config.c
View file

@ -36,9 +36,14 @@ wmr_hmd_config_init_defaults(struct wmr_hmd_config *c)
// initialize default sensor transforms
math_pose_identity(&c->eye_params[0].pose);
math_pose_identity(&c->eye_params[1].pose);
math_pose_identity(&c->sensors.accel_pose);
math_pose_identity(&c->sensors.gyro_pose);
math_pose_identity(&c->sensors.mag_pose);
math_pose_identity(&c->sensors.accel.pose);
math_pose_identity(&c->sensors.gyro.pose);
math_pose_identity(&c->sensors.mag.pose);
math_matrix_3x3_identity(&c->sensors.accel.mix_matrix);
math_matrix_3x3_identity(&c->sensors.gyro.mix_matrix);
math_matrix_3x3_identity(&c->sensors.mag.mix_matrix);
}
static void
@ -170,27 +175,8 @@ wmr_config_parse_display(struct wmr_hmd_config *c, cJSON *display, enum u_loggin
}
static bool
wmr_inertial_sensors_config_parse(struct wmr_inertial_sensors_config *c, cJSON *sensor, enum u_logging_level log_level)
wmr_inertial_sensor_config_parse(struct wmr_inertial_sensor_config *c, cJSON *sensor, enum u_logging_level log_level)
{
struct xrt_pose *out_pose;
const char *sensor_type = cJSON_GetStringValue(cJSON_GetObjectItem(sensor, "SensorType"));
if (sensor_type == NULL) {
WMR_WARN(log_level, "Missing sensor type");
return false;
}
if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Gyro")) {
out_pose = &c->gyro_pose;
} else if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Accelerometer")) {
out_pose = &c->accel_pose;
} else if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Magnetometer")) {
out_pose = &c->mag_pose;
} else {
WMR_WARN(log_level, "Unhandled sensor type \"%s\"", sensor_type);
return false;
}
struct xrt_vec3 translation;
struct xrt_matrix_3x3 rotation;
@ -206,11 +192,64 @@ wmr_inertial_sensors_config_parse(struct wmr_inertial_sensors_config *c, cJSON *
return false;
}
wmr_config_compute_pose(out_pose, &translation, &rotation);
wmr_config_compute_pose(&c->pose, &translation, &rotation);
/* compute the bias offsets and mix matrix by taking the constant
* coefficients from the configuration */
cJSON *mix_model = cJSON_GetObjectItem(sensor, "MixingMatrixTemperatureModel");
cJSON *bias_model = cJSON_GetObjectItem(sensor, "BiasTemperatureModel");
float mix_model_values[3 * 3 * 4];
float bias_model_values[12];
if (mix_model == NULL || bias_model == NULL) {
WMR_WARN(log_level, "Missing Inertial Sensor calibration");
return false;
}
if (u_json_get_float_array(mix_model, mix_model_values, 3 * 3 * 4) != 3 * 3 * 4) {
WMR_WARN(log_level, "Invalid Inertial Sensor calibration (invalid MixingMatrixTemperatureModel)");
return false;
}
for (int i = 0; i < 9; i++) {
c->mix_matrix.v[i] = mix_model_values[i * 4];
}
if (u_json_get_float_array(bias_model, bias_model_values, 12) != 12) {
WMR_WARN(log_level, "Invalid Inertial Sensor calibration (invalid BiasTemperatureModel)");
return false;
}
c->bias_offsets.x = bias_model_values[0];
c->bias_offsets.y = bias_model_values[4];
c->bias_offsets.z = bias_model_values[8];
return true;
}
static bool
wmr_inertial_sensors_config_parse(struct wmr_inertial_sensors_config *c, cJSON *sensor, enum u_logging_level log_level)
{
struct wmr_inertial_sensor_config *target = NULL;
const char *sensor_type = cJSON_GetStringValue(cJSON_GetObjectItem(sensor, "SensorType"));
if (sensor_type == NULL) {
WMR_WARN(log_level, "Missing sensor type");
return false;
}
if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Gyro")) {
target = &c->gyro;
} else if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Accelerometer")) {
target = &c->accel;
} else if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Magnetometer")) {
target = &c->mag;
} else {
WMR_WARN(log_level, "Unhandled sensor type \"%s\"", sensor_type);
return false;
}
return wmr_inertial_sensor_config_parse(target, sensor, log_level);
}
static bool
wmr_config_parse_camera_config(struct wmr_hmd_config *c, cJSON *camera, enum u_logging_level log_level)
{

20
src/xrt/drivers/wmr/wmr_config.h
View file

@ -105,11 +105,25 @@ struct wmr_camera_config
struct wmr_distortion_6KT distortion6KT;
};
/* Configuration for a single inertial sensor */
struct wmr_inertial_sensor_config
{
struct xrt_pose pose;
/* Current bias and mix matrix extracted from
* the configuration, which provides coefficients
* for temperature adjustments - but they're always 0,
* so we just take the constant coefficient */
struct xrt_vec3 bias_offsets;
struct xrt_matrix_3x3 mix_matrix;
};
/* Configuration for the set of inertial sensors */
struct wmr_inertial_sensors_config
{
struct xrt_pose accel_pose;
struct xrt_pose gyro_pose;
struct xrt_pose mag_pose;
struct wmr_inertial_sensor_config accel;
struct wmr_inertial_sensor_config gyro;
struct wmr_inertial_sensor_config mag;
};
struct wmr_hmd_config

6
src/xrt/drivers/wmr/wmr_hmd.c
View file

@ -996,11 +996,11 @@ wmr_hmd_create(enum wmr_headset_type hmd_type,
math_pose_transform(&wh->centerline, &wh->display_to_centerline[dIdx],
&wh->display_to_centerline[dIdx]);
}
math_pose_invert(&wh->config.sensors.accel_pose, &wh->accel_to_centerline);
math_pose_invert(&wh->config.sensors.accel.pose, &wh->accel_to_centerline);
math_pose_transform(&wh->centerline, &wh->accel_to_centerline, &wh->accel_to_centerline);
math_pose_invert(&wh->config.sensors.gyro_pose, &wh->gyro_to_centerline);
math_pose_invert(&wh->config.sensors.gyro.pose, &wh->gyro_to_centerline);
math_pose_transform(&wh->centerline, &wh->gyro_to_centerline, &wh->gyro_to_centerline);
math_pose_invert(&wh->config.sensors.mag_pose, &wh->mag_to_centerline);
math_pose_invert(&wh->config.sensors.mag.pose, &wh->mag_to_centerline);
math_pose_transform(&wh->centerline, &wh->mag_to_centerline, &wh->mag_to_centerline);
struct u_device_simple_info info;