diff --git a/src/xrt/drivers/wmr/wmr_config.c b/src/xrt/drivers/wmr/wmr_config.c index 9fbfff0de..c023d27cd 100644 --- a/src/xrt/drivers/wmr/wmr_config.c +++ b/src/xrt/drivers/wmr/wmr_config.c @@ -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) { diff --git a/src/xrt/drivers/wmr/wmr_config.h b/src/xrt/drivers/wmr/wmr_config.h index 66e237530..bf0f93b55 100644 --- a/src/xrt/drivers/wmr/wmr_config.h +++ b/src/xrt/drivers/wmr/wmr_config.h @@ -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 diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c index 97efac96e..cc6fd4089 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.c +++ b/src/xrt/drivers/wmr/wmr_hmd.c @@ -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;