The open source OpenXR runtime
d/wmr: Extend config reading to include bias/mix_matrix
Add extraction of bias offsets and mix_matrix (rotation)
correction for IMU accel, gyro and mag configuration entries.
+83
-30
+63
-24
src/xrt/drivers/wmr/wmr_config.c
+63
-24
src/xrt/drivers/wmr/wmr_config.c
···
36
36
// initialize default sensor transforms
37
37
math_pose_identity(&c->eye_params[0].pose);
38
38
math_pose_identity(&c->eye_params[1].pose);
39
-
math_pose_identity(&c->sensors.accel_pose);
40
-
math_pose_identity(&c->sensors.gyro_pose);
41
-
math_pose_identity(&c->sensors.mag_pose);
39
+
40
+
math_pose_identity(&c->sensors.accel.pose);
41
+
math_pose_identity(&c->sensors.gyro.pose);
42
+
math_pose_identity(&c->sensors.mag.pose);
43
+
44
+
math_matrix_3x3_identity(&c->sensors.accel.mix_matrix);
45
+
math_matrix_3x3_identity(&c->sensors.gyro.mix_matrix);
46
+
math_matrix_3x3_identity(&c->sensors.mag.mix_matrix);
42
47
}
43
48
44
49
static void
···
170
175
}
171
176
172
177
static bool
173
-
wmr_inertial_sensors_config_parse(struct wmr_inertial_sensors_config *c, cJSON *sensor, enum u_logging_level log_level)
178
+
wmr_inertial_sensor_config_parse(struct wmr_inertial_sensor_config *c, cJSON *sensor, enum u_logging_level log_level)
174
179
{
175
-
struct xrt_pose *out_pose;
176
-
177
-
const char *sensor_type = cJSON_GetStringValue(cJSON_GetObjectItem(sensor, "SensorType"));
178
-
if (sensor_type == NULL) {
179
-
WMR_WARN(log_level, "Missing sensor type");
180
-
return false;
181
-
}
182
-
183
-
if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Gyro")) {
184
-
out_pose = &c->gyro_pose;
185
-
} else if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Accelerometer")) {
186
-
out_pose = &c->accel_pose;
187
-
} else if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Magnetometer")) {
188
-
out_pose = &c->mag_pose;
189
-
} else {
190
-
WMR_WARN(log_level, "Unhandled sensor type \"%s\"", sensor_type);
191
-
return false;
192
-
}
193
-
194
180
struct xrt_vec3 translation;
195
181
struct xrt_matrix_3x3 rotation;
196
182
···
206
192
return false;
207
193
}
208
194
209
-
wmr_config_compute_pose(out_pose, &translation, &rotation);
195
+
wmr_config_compute_pose(&c->pose, &translation, &rotation);
196
+
197
+
/* compute the bias offsets and mix matrix by taking the constant
198
+
* coefficients from the configuration */
199
+
cJSON *mix_model = cJSON_GetObjectItem(sensor, "MixingMatrixTemperatureModel");
200
+
cJSON *bias_model = cJSON_GetObjectItem(sensor, "BiasTemperatureModel");
201
+
float mix_model_values[3 * 3 * 4];
202
+
float bias_model_values[12];
203
+
204
+
if (mix_model == NULL || bias_model == NULL) {
205
+
WMR_WARN(log_level, "Missing Inertial Sensor calibration");
206
+
return false;
207
+
}
208
+
209
+
if (u_json_get_float_array(mix_model, mix_model_values, 3 * 3 * 4) != 3 * 3 * 4) {
210
+
WMR_WARN(log_level, "Invalid Inertial Sensor calibration (invalid MixingMatrixTemperatureModel)");
211
+
return false;
212
+
}
213
+
for (int i = 0; i < 9; i++) {
214
+
c->mix_matrix.v[i] = mix_model_values[i * 4];
215
+
}
216
+
217
+
if (u_json_get_float_array(bias_model, bias_model_values, 12) != 12) {
218
+
WMR_WARN(log_level, "Invalid Inertial Sensor calibration (invalid BiasTemperatureModel)");
219
+
return false;
220
+
}
221
+
c->bias_offsets.x = bias_model_values[0];
222
+
c->bias_offsets.y = bias_model_values[4];
223
+
c->bias_offsets.z = bias_model_values[8];
210
224
211
225
return true;
226
+
}
227
+
228
+
static bool
229
+
wmr_inertial_sensors_config_parse(struct wmr_inertial_sensors_config *c, cJSON *sensor, enum u_logging_level log_level)
230
+
{
231
+
struct wmr_inertial_sensor_config *target = NULL;
232
+
233
+
const char *sensor_type = cJSON_GetStringValue(cJSON_GetObjectItem(sensor, "SensorType"));
234
+
if (sensor_type == NULL) {
235
+
WMR_WARN(log_level, "Missing sensor type");
236
+
return false;
237
+
}
238
+
239
+
if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Gyro")) {
240
+
target = &c->gyro;
241
+
} else if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Accelerometer")) {
242
+
target = &c->accel;
243
+
} else if (!strcmp(sensor_type, "CALIBRATION_InertialSensorType_Magnetometer")) {
244
+
target = &c->mag;
245
+
} else {
246
+
WMR_WARN(log_level, "Unhandled sensor type \"%s\"", sensor_type);
247
+
return false;
248
+
}
249
+
250
+
return wmr_inertial_sensor_config_parse(target, sensor, log_level);
212
251
}
213
252
214
253
static bool
+17
-3
src/xrt/drivers/wmr/wmr_config.h
+17
-3
src/xrt/drivers/wmr/wmr_config.h
···
105
105
struct wmr_distortion_6KT distortion6KT;
106
106
};
107
107
108
+
/* Configuration for a single inertial sensor */
109
+
struct wmr_inertial_sensor_config
110
+
{
111
+
struct xrt_pose pose;
112
+
113
+
/* Current bias and mix matrix extracted from
114
+
* the configuration, which provides coefficients
115
+
* for temperature adjustments - but they're always 0,
116
+
* so we just take the constant coefficient */
117
+
struct xrt_vec3 bias_offsets;
118
+
struct xrt_matrix_3x3 mix_matrix;
119
+
};
120
+
121
+
/* Configuration for the set of inertial sensors */
108
122
struct wmr_inertial_sensors_config
109
123
{
110
-
struct xrt_pose accel_pose;
111
-
struct xrt_pose gyro_pose;
112
-
struct xrt_pose mag_pose;
124
+
struct wmr_inertial_sensor_config accel;
125
+
struct wmr_inertial_sensor_config gyro;
126
+
struct wmr_inertial_sensor_config mag;
113
127
};
114
128
115
129
struct wmr_hmd_config
+3
-3
src/xrt/drivers/wmr/wmr_hmd.c
+3
-3
src/xrt/drivers/wmr/wmr_hmd.c
···
996
996
math_pose_transform(&wh->centerline, &wh->display_to_centerline[dIdx],
997
997
&wh->display_to_centerline[dIdx]);
998
998
}
999
-
math_pose_invert(&wh->config.sensors.accel_pose, &wh->accel_to_centerline);
999
+
math_pose_invert(&wh->config.sensors.accel.pose, &wh->accel_to_centerline);
1000
1000
math_pose_transform(&wh->centerline, &wh->accel_to_centerline, &wh->accel_to_centerline);
1001
-
math_pose_invert(&wh->config.sensors.gyro_pose, &wh->gyro_to_centerline);
1001
+
math_pose_invert(&wh->config.sensors.gyro.pose, &wh->gyro_to_centerline);
1002
1002
math_pose_transform(&wh->centerline, &wh->gyro_to_centerline, &wh->gyro_to_centerline);
1003
-
math_pose_invert(&wh->config.sensors.mag_pose, &wh->mag_to_centerline);
1003
+
math_pose_invert(&wh->config.sensors.mag.pose, &wh->mag_to_centerline);
1004
1004
math_pose_transform(&wh->centerline, &wh->mag_to_centerline, &wh->mag_to_centerline);
1005
1005
1006
1006
struct u_device_simple_info info;