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

d/wmr: Extract JSON configuration from the firmware.

Browse source 
Pull in the WMR config obfuscation key and extract the JSON
calibration data.

Based on a patch from Dan Weatherford <sabretooth@gmail.com>
This commit is contained in:
Jan Schmidt 2021-06-21 01:13:22 +10:00
parent 3ca80e9607
commit bad625965a
8 changed files with 470 additions and 72 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/xrt/drivers/CMakeLists.txt
View file

@ -320,7 +320,7 @@ if(XRT_BUILD_DRIVER_WMR)
)
add_library(drv_wmr STATIC ${WMR_SOURCE_FILES})
target_link_libraries(drv_wmr PRIVATE xrt-interfaces aux_util)
target_link_libraries(drv_wmr PRIVATE xrt-interfaces aux_util aux_math xrt-external-cjson)
list(APPEND ENABLED_HEADSET_DRIVERS wmr)
endif()

5
src/xrt/drivers/meson.build
View file

@ -296,7 +296,10 @@ lib_drv_wmr = static_library(
'wmr/wmr_protocol.c',
'wmr/wmr_protocol.h',
),
include_directories: xrt_include,
include_directories: [
xrt_include,
cjson_include,
],
dependencies: [aux],
build_by_default: 'wmr' in drivers,
)

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

@ -7,64 +7,269 @@
* @author Jan Schmidt <jan@centricular.com>
* @ingroup drv_wmr
*/
#include <string.h>
#include "math/m_api.h"
#include "util/u_misc.h"
#include "util/u_json.h"
#include "wmr_config.h"
bool
wmr_config_parse(struct wmr_hmd_config *c)
#define WMR_TRACE(ll, ...) U_LOG_IFL_T(ll, __VA_ARGS__)
#define WMR_DEBUG(ll, ...) U_LOG_IFL_D(ll, __VA_ARGS__)
#define WMR_INFO(ll, ...) U_LOG_IFL_I(ll, __VA_ARGS__)
#define WMR_WARN(ll, ...) U_LOG_IFL_W(ll, __VA_ARGS__)
#define WMR_ERROR(ll, ...) U_LOG_IFL_E(ll, __VA_ARGS__)
#define JSON_INT(a, b, c) u_json_get_int(u_json_get(a, b), c)
#define JSON_FLOAT(a, b, c) u_json_get_float(u_json_get(a, b), c)
#define JSON_DOUBLE(a, b, c) u_json_get_double(u_json_get(a, b), c)
#define JSON_VEC3(a, b, c) u_json_get_vec3_array(u_json_get(a, b), c)
#define JSON_MATRIX_3X3(a, b, c) u_json_get_matrix_3x3(u_json_get(a, b), c)
#define JSON_STRING(a, b, c) u_json_get_string_into_array(u_json_get(a, b), c, sizeof(c))
static void
wmr_config_init_defaults(struct wmr_hmd_config *c)
{
int i, j, k;
memset(c, 0, sizeof(struct wmr_hmd_config));
const struct xrt_vec2 display_size[2] = {{4320, 2160}, {4320, 2160}};
// initialize default sensor transforms
math_pose_identity(&c->eye_params[0].pose);
math_pose_identity(&c->eye_params[1].pose);
math_pose_identity(&c->accel_pose);
math_pose_identity(&c->gyro_pose);
math_pose_identity(&c->mag_pose);
}
// eye_centers from VisibleAreaCenter X/Y
struct xrt_vec2 eye_centers[2] = {
{1171.2011028243148, 1078.7720082720277},
{3154.10490135909, 1085.7209119898746},
};
static void
wmr_config_compute_pose(struct xrt_pose *out_pose, const struct xrt_vec3 *tx, const struct xrt_matrix_3x3 *rx)
{
// Adjust the coordinate system / conventions of the raw Tx and Rx config to yield a usable xrt_pose
// The config stores a 3x3 rotation matrix and a vec3 translation.
// Translation is applied after rotation, and the coordinate system is flipped in YZ.
const double eye_radius[2] = {1500, 1500};
const double eye_affine[2][9] = {
{1467.741455078125, -0, 1171.2010498046875, -0, 1467.642333984375, 1078.77197265625, 0, 0, 1},
{1469.2613525390625, -0, 3154.10498046875, -0, 1468.5185546875, 1085.720947265625, 0, 0, 1}};
struct xrt_matrix_3x3 coordsys = {.v = {1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, -1.0}};
// These need to be acquired from the WMR config:
/* From DistortionRed/Green/Blue ModelParameters[0] and [1] */
const struct xrt_vec2 eye_centers_rgb[2][3][2] = {{{{1173.7816892048809, 1079.9493112867228}},
{{1171.8855282399534, 1078.1630786236972}},
{{1169.6008128643944, 1074.7670304358412}}},
{{{3150.2395019256492, 1086.1749083261475}},
{{3149.7810962925541, 1084.9113795043713}},
{{3150.3983095783842, 1082.3335369357619}}}};
/* From DistortionRed/Green/Blue ModelParameters [2,3,4] */
const double distortion_params[2][3][3] = {
{
{1.6392082863852426E-7, 4.0096839564631026E-14, 6.6538855737065E-20}, /* Red */
{2.1590946493033866E-7, -4.78028658789357E-14, 1.1929574027716904E-19}, /* Green */
{3.1991456111909366E-7, -2.300137347653273E-13, 2.3405778580046485E-19} /* Blue */
},
{
{1.5982850735663643E-7, 4.990973924637425E-14, 6.0056239395619067E-20}, /* Red */
{2.1206804797012724E-7, -3.5561864117498794E-14, 1.0992145779675043E-19}, /* Green */
{3.1395508877599257E-7, -2.0999418299177255E-13, 2.1828476911150306E-19} /* Blue */
}};
struct xrt_matrix_3x3 rx_adj;
math_matrix_3x3_multiply(&coordsys, rx, &rx_adj);
math_quat_from_matrix_3x3(&rx_adj, &out_pose->orientation);
for (i = 0; i < 2; i++) {
struct wmr_distortion_eye_config *eye = c->eye_params + i;
struct xrt_vec3 v;
math_matrix_3x3_transform_vec3(&coordsys, tx, &v);
math_matrix_3x3_transform_vec3(&rx_adj, &v, &out_pose->position);
}
eye->display_size = display_size[i];
eye->visible_center = eye_centers[i];
eye->visible_radius = eye_radius[i];
for (j = 0; j < 9; j++) {
eye->affine_xform.v[j] = eye_affine[i][j];
}
static bool
wmr_config_parse_display(struct wmr_hmd_config *c, cJSON *display, enum u_logging_level ll)
{
cJSON *json_eye = cJSON_GetObjectItem(display, "AssignedEye");
char *json_eye_name = cJSON_GetStringValue(json_eye);
for (j = 0; j < 3; j++) {
/* RGB distortion params */
eye->distortion3K[j].eye_center = *eye_centers_rgb[i][j];
for (k = 0; k < 3; k++) {
eye->distortion3K[j].k[k] = distortion_params[i][j][k];
}
}
if (json_eye_name == NULL) {
WMR_ERROR(ll, "Invalid/missing eye assignment block");
return false;
}
struct wmr_distortion_eye_config *eye = NULL;
if (!strcmp(json_eye_name, "CALIBRATION_DisplayEyeLeft")) {
eye = &c->eye_params[0];
} else if (!strcmp(json_eye_name, "CALIBRATION_DisplayEyeRight")) {
eye = &c->eye_params[1];
} else {
WMR_ERROR(ll, "Unknown AssignedEye \"%s\"", json_eye_name);
return false;
}
/* Extract display panel parameters */
cJSON *affine = cJSON_GetObjectItem(display, "Affine");
if (affine == NULL || u_json_get_float_array(affine, eye->affine_xform.v, 9) != 9) {
WMR_ERROR(ll, "Missing affine transform for AssignedEye \"%s\"", json_eye_name);
return false;
}
if (!JSON_FLOAT(display, "DisplayWidth", &eye->display_size.x) ||
!JSON_FLOAT(display, "DisplayHeight", &eye->display_size.y))
return false;
cJSON *visible_area_center = cJSON_GetObjectItem(display, "VisibleAreaCenter");
if (visible_area_center == NULL || !JSON_FLOAT(visible_area_center, "X", &eye->visible_center.x) ||
!JSON_FLOAT(visible_area_center, "Y", &eye->visible_center.y)) {
return false;
}
if (!JSON_DOUBLE(display, "VisibleAreaRadius", &eye->visible_radius))
return false;
/* Compute eye pose */
cJSON *rt = cJSON_GetObjectItem(display, "Rt");
cJSON *rx = cJSON_GetObjectItem(rt, "Rotation");
if (rt == NULL || rx == NULL)
return false;
struct xrt_vec3 translation;
struct xrt_matrix_3x3 rotation;
if (!JSON_VEC3(rt, "Translation", &translation))
return false;
if (u_json_get_float_array(rx, rotation.v, 9) != 9)
return false;
wmr_config_compute_pose(&eye->pose, &translation, &rotation);
/* Parse color distortion channels */
const char *channel_names[] = {"DistortionRed", "DistortionGreen", "DistortionBlue"};
for (int channel = 0; channel < 3; ++channel) {
struct wmr_distortion_3K *distortion3K = &eye->distortion3K[channel];
cJSON *dist = cJSON_GetObjectItemCaseSensitive(display, channel_names[channel]);
if (!dist) {
WMR_ERROR(ll, "Missing distortion channel info %s", channel_names[channel]);
return false;
}
const char *model_type = cJSON_GetStringValue(cJSON_GetObjectItemCaseSensitive(dist, "ModelType"));
if (model_type == NULL) {
WMR_ERROR(ll, "Missing distortion type");
return false;
}
if (!strcmp(model_type, "CALIBRATION_DisplayDistortionModelPolynomial3K")) {
distortion3K->model = WMR_DISTORTION_MODEL_POLYNOMIAL_3K;
} else {
distortion3K->model = WMR_DISTORTION_MODEL_UNKNOWN;
WMR_ERROR(ll, "Unknown distortion model %s", model_type);
return false;
}
int param_count;
double parameters[5];
if (!JSON_INT(dist, "ModelParameterCount", &param_count)) {
WMR_ERROR(ll, "Missing distortion parameters");
return false;
}
cJSON *params_json = cJSON_GetObjectItemCaseSensitive(dist, "ModelParameters");
if (params_json == NULL ||
u_json_get_double_array(params_json, parameters, param_count) != (size_t)param_count) {
WMR_ERROR(ll, "Missing distortion parameters");
return false;
}
distortion3K->eye_center.x = parameters[0];
distortion3K->eye_center.y = parameters[1];
distortion3K->k[0] = parameters[2];
distortion3K->k[1] = parameters[3];
distortion3K->k[2] = parameters[4];
}
return true;
}
static bool
wmr_config_parse_inertial_sensor(struct wmr_hmd_config *c, cJSON *sensor, enum u_logging_level ll)
{
struct xrt_pose *out_pose;
const char *sensor_type = cJSON_GetStringValue(cJSON_GetObjectItem(sensor, "SensorType"));
if (sensor_type == NULL) {
WMR_WARN(ll, "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(ll, "Unhandled sensor type \"%s\"", sensor_type);
return false;
}
struct xrt_vec3 translation;
struct xrt_matrix_3x3 rotation;
cJSON *rt = cJSON_GetObjectItem(sensor, "Rt");
cJSON *rx = cJSON_GetObjectItem(rt, "Rotation");
if (rt == NULL || rx == NULL) {
WMR_WARN(ll, "Missing Inertial Sensor calibration");
return false;
}
if (!JSON_VEC3(rt, "Translation", &translation) || u_json_get_float_array(rx, rotation.v, 9) != 9) {
WMR_WARN(ll, "Invalid Inertial Sensor calibration");
return false;
}
wmr_config_compute_pose(out_pose, &translation, &rotation);
return true;
}
static bool
wmr_config_parse_calibration(struct wmr_hmd_config *c, cJSON *calib_info, enum u_logging_level ll)
{
cJSON *item = NULL;
// calib_info is object with keys "Cameras", "Displays", and "InertialSensors"
cJSON *displays = cJSON_GetObjectItemCaseSensitive(calib_info, "Displays");
if (!cJSON_IsArray(displays)) {
WMR_ERROR(ll, "Displays: not found or not an Array");
return false;
}
cJSON_ArrayForEach(item, displays)
{
if (!wmr_config_parse_display(c, item, ll)) {
WMR_ERROR(ll, "Error parsing Display entry");
return false;
}
}
cJSON *sensors = cJSON_GetObjectItemCaseSensitive(calib_info, "InertialSensors");
if (!cJSON_IsArray(sensors)) {
WMR_ERROR(ll, "InertialSensors: not found or not an Array");
return false;
}
cJSON_ArrayForEach(item, sensors)
{
if (!wmr_config_parse_inertial_sensor(c, item, ll)) {
WMR_WARN(ll, "Error parsing InertialSensor entry");
}
}
return true;
}
bool
wmr_config_parse(struct wmr_hmd_config *c, char *json_string, enum u_logging_level ll)
{
wmr_config_init_defaults(c);
cJSON *json_root = cJSON_Parse(json_string);
if (!cJSON_IsObject(json_root)) {
WMR_ERROR(ll, "Could not parse JSON data.");
cJSON_Delete(json_root);
return false;
}
cJSON *calib_info = cJSON_GetObjectItemCaseSensitive(json_root, "CalibrationInformation");
if (!cJSON_IsObject(calib_info)) {
WMR_ERROR(ll, "CalibrationInformation object not found");
cJSON_Delete(json_root);
return false;
}
bool res = wmr_config_parse_calibration(c, calib_info, ll);
cJSON_Delete(json_root);
return res;
}

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

@ -11,7 +11,13 @@
#pragma once
#include "math/m_vec2.h"
#include "util/u_logging.h"
enum wmr_distortion_model
{
WMR_DISTORTION_MODEL_UNKNOWN = 0,
WMR_DISTORTION_MODEL_POLYNOMIAL_3K
};
#ifdef __cplusplus
extern "C" {
@ -19,6 +25,8 @@ extern "C" {
struct wmr_distortion_3K
{
enum wmr_distortion_model model;
/* X/Y center of the distortion (pixels) */
struct xrt_vec2 eye_center;
/* k1,k2,k3 params for radial distortion as
@ -31,6 +39,8 @@ struct wmr_distortion_eye_config
{
/* 3x3 camera matrix that moves from normalised camera coords (X/Z & Y/Z) to undistorted pixels */
struct xrt_matrix_3x3 affine_xform;
/* Eye pose in world space */
struct xrt_pose pose;
/* Radius of the (undistorted) visible area from the center (pixels) (I think) */
double visible_radius;
@ -47,11 +57,14 @@ struct wmr_hmd_config
{
/* Left and Right eye mapping and distortion params */
struct wmr_distortion_eye_config eye_params[2];
struct xrt_pose accel_pose;
struct xrt_pose gyro_pose;
struct xrt_pose mag_pose;
};
/* FIXME: Pass JSON config when we have that: */
bool
wmr_config_parse(struct wmr_hmd_config *c);
wmr_config_parse(struct wmr_hmd_config *c, char *json_string, enum u_logging_level ll);
#ifdef __cplusplus
}

78
src/xrt/drivers/wmr/wmr_config_key.h Normal file
View file

@ -0,0 +1,78 @@
/* Copyright 2021 Jan Schmidt
* SPDX-License-Identifier: BSL-1.0
*/
/*
* This block is XOR-ed with the configuration stored in all WMR headsets seen so far.
* It was derived from a handful of raw config blocks read from headsets using
* https://github.com/pH5/wmr-config
*/
// clang-format off
const uint8_t wmr_config_key[] =
{
0x2F, 0xC8, 0x0F, 0x38, 0xDD, 0x00, 0xF6, 0x5C, 0xA1, 0x31, 0xEF, 0xF1, 0xEA, 0x6F, 0xA0, 0xF8,
0x26, 0xB5, 0x9B, 0x39, 0xCF, 0x3A, 0x88, 0xC8, 0x2E, 0x17, 0xC0, 0x63, 0x5B, 0x46, 0x27, 0xBB,
0x98, 0x2F, 0x0E, 0x2A, 0x90, 0x4B, 0x28, 0x2D, 0x82, 0x76, 0xE5, 0x28, 0x72, 0x50, 0x8A, 0xF0,
0xBF, 0x84, 0x54, 0x3B, 0xA8, 0x77, 0x91, 0xCE, 0x87, 0x80, 0x53, 0x2F, 0x07, 0xAD, 0x1B, 0x3F,
0x8C, 0x67, 0x33, 0x2E, 0xEB, 0x6A, 0x2A, 0x52, 0x77, 0x7C, 0x1F, 0x02, 0x11, 0x9E, 0x2A, 0x59,
0x5C, 0x94, 0x0E, 0x4F, 0xF5, 0x44, 0x54, 0x01, 0xE7, 0x8F, 0x66, 0xF0, 0xAD, 0x68, 0x71, 0x3C,
0x6D, 0x2E, 0x1C, 0xE3, 0x11, 0x46, 0xF7, 0x7F, 0x02, 0x6C, 0x15, 0xA0, 0x10, 0xEE, 0x3B, 0x14,
0xAE, 0x6C, 0xA7, 0x3F, 0xAF, 0x83, 0x6A, 0xD7, 0x12, 0x88, 0x53, 0xFE, 0xEB, 0x5C, 0x78, 0x85,
0xAF, 0x1F, 0x80, 0x7F, 0xB6, 0xDA, 0x7C, 0x0E, 0x84, 0xB5, 0x02, 0x8E, 0x92, 0xA3, 0x5B, 0x83,
0x56, 0x11, 0x7B, 0xDF, 0x80, 0xB3, 0x4C, 0x13, 0x8E, 0x61, 0x61, 0xE6, 0x82, 0x8C, 0xDA, 0x08,
0x76, 0x88, 0xBF, 0x85, 0x7F, 0xE4, 0x28, 0x26, 0x1F, 0xB5, 0x67, 0x80, 0x63, 0xD9, 0x26, 0xD4,
0x91, 0xD1, 0xC1, 0x51, 0xCE, 0x61, 0x64, 0x2B, 0x56, 0xAE, 0x3D, 0x06, 0x5D, 0xCD, 0xF7, 0x05,
0x9A, 0x6F, 0xEB, 0x2E, 0xC2, 0x69, 0x42, 0x86, 0xBE, 0x78, 0x32, 0xC3, 0xA8, 0x94, 0xA3, 0x97,
0x84, 0x07, 0xF1, 0x6E, 0x3F, 0x10, 0xDE, 0x2B, 0xB1, 0x41, 0x1A, 0x59, 0xE3, 0x7F, 0x23, 0xDE,
0xF3, 0x13, 0x23, 0xD1, 0x60, 0x1C, 0xBB, 0xC5, 0x4A, 0xB1, 0xC6, 0x02, 0x38, 0x7B, 0xFE, 0xEF,
0xB6, 0x50, 0x16, 0x23, 0x4B, 0xD4, 0xEF, 0xEA, 0x67, 0xEC, 0x44, 0x2C, 0xC0, 0xA6, 0x2F, 0x0D,
0x6E, 0x17, 0x52, 0xC8, 0x26, 0xCB, 0x63, 0x85, 0x72, 0x8D, 0xBA, 0xD8, 0x09, 0xA3, 0x89, 0x64,
0x70, 0x12, 0xC6, 0xDF, 0x4C, 0x28, 0xD4, 0xB8, 0x49, 0x18, 0x69, 0x22, 0x36, 0xF1, 0x00, 0xC3,
0x91, 0xB3, 0x7C, 0xA0, 0xAA, 0x7D, 0x9E, 0x27, 0x65, 0xCD, 0x16, 0x3C, 0x71, 0x3C, 0xCC, 0xF5,
0x02, 0xD1, 0xA5, 0x06, 0x95, 0xB2, 0x1E, 0x71, 0x92, 0x6F, 0xC2, 0xD2, 0xEF, 0x58, 0x7B, 0xD0,
0x53, 0x5E, 0xE9, 0xB6, 0xCA, 0x1C, 0x13, 0x96, 0xAC, 0xF1, 0xF5, 0x19, 0xD9, 0x8A, 0x1D, 0xA9,
0x0D, 0xAE, 0xD0, 0xF4, 0xB3, 0xDD, 0x2D, 0x43, 0x6E, 0x41, 0x22, 0xDC, 0x09, 0xA4, 0x92, 0x42,
0xC5, 0x32, 0x7D, 0xD7, 0xA2, 0x57, 0xA5, 0xA5, 0x11, 0xD2, 0x22, 0xD0, 0xD7, 0x75, 0xFF, 0xFC,
0x2F, 0x66, 0xB5, 0xA9, 0xA3, 0x2B, 0xB4, 0x2B, 0x14, 0xEF, 0xC4, 0xB4, 0x18, 0xF0, 0x56, 0x55,
0x93, 0x6A, 0x30, 0xF6, 0xDF, 0x14, 0x23, 0xF7, 0x2A, 0xDC, 0x4C, 0xE7, 0x78, 0x2B, 0x66, 0x22,
0xB4, 0xAC, 0x2E, 0x03, 0xF2, 0xEF, 0xEC, 0x35, 0xAF, 0x22, 0x6D, 0x05, 0x12, 0x6D, 0xC5, 0x2C,
0x12, 0x9B, 0x7D, 0x66, 0x47, 0x8F, 0xC0, 0x81, 0xCD, 0xFE, 0x97, 0x4C, 0x01, 0xC1, 0xD7, 0x24,
0xD8, 0x46, 0xFD, 0x29, 0xF7, 0xE1, 0x61, 0xF3, 0xA0, 0x69, 0xBD, 0x23, 0x35, 0x7E, 0x66, 0xB1,
0xF6, 0x3A, 0xD9, 0xF8, 0x29, 0xA2, 0x99, 0x8E, 0xF7, 0xBC, 0xCC, 0xD6, 0x37, 0x11, 0x09, 0xC8,
0x07, 0x68, 0x5D, 0xF6, 0xC2, 0x73, 0xE8, 0xE5, 0x2D, 0xA4, 0x62, 0x0E, 0x9D, 0xC2, 0x76, 0xA9,
0x94, 0x06, 0x19, 0x39, 0x9F, 0x8F, 0x83, 0x62, 0xE9, 0xDE, 0xED, 0x76, 0xFD, 0xBC, 0x42, 0x77,
0x1E, 0x49, 0x18, 0xD8, 0x15, 0x22, 0x55, 0x5C, 0xD3, 0xF2, 0x7F, 0xD0, 0x8A, 0x27, 0x82, 0x05,
0xD3, 0xBD, 0x27, 0x0C, 0x2F, 0xB9, 0x72, 0xE9, 0x9D, 0x6B, 0xD3, 0xD6, 0xD6, 0x84, 0xA4, 0x1F,
0x6C, 0x26, 0x7C, 0x61, 0xE0, 0x7E, 0x58, 0x05, 0xAD, 0xC5, 0xE1, 0x14, 0x3A, 0xD7, 0x40, 0x6A,
0x52, 0x57, 0x82, 0xAA, 0x9B, 0xF0, 0xCA, 0x60, 0x5D, 0x6C, 0xC0, 0xA4, 0x6B, 0xF3, 0x87, 0x6D,
0x04, 0x80, 0x2C, 0x7B, 0xEB, 0x9F, 0xD4, 0x03, 0x81, 0x91, 0xD0, 0xB9, 0x74, 0xAE, 0x19, 0xBF,
0x48, 0x63, 0x8F, 0x8C, 0xEE, 0xBC, 0xB4, 0xC0, 0x16, 0x4A, 0xF5, 0x5E, 0x1C, 0x7A, 0xDB, 0xD5,
0xA4, 0x16, 0x92, 0xCB, 0x52, 0x86, 0xCB, 0xD1, 0x1E, 0x1D, 0xEE, 0x90, 0x01, 0x90, 0x52, 0x52,
0x52, 0x8C, 0x25, 0x0A, 0xB7, 0xDE, 0x10, 0x51, 0xB8, 0x23, 0x5C, 0xCB, 0x32, 0x6A, 0xB0, 0xB9,
0xA4, 0x58, 0xB6, 0x14, 0x28, 0xF0, 0xFB, 0xC2, 0xCD, 0x6F, 0x5E, 0x10, 0x48, 0xAD, 0x1F, 0xC8,
0xCE, 0x4F, 0x09, 0xDA, 0xF8, 0xD0, 0x84, 0x44, 0x8C, 0x57, 0x4B, 0xE1, 0x87, 0x5B, 0x79, 0xD0,
0x93, 0x38, 0x57, 0x65, 0x31, 0x55, 0xF2, 0xD6, 0x1F, 0x6C, 0xC9, 0xD1, 0x3A, 0x17, 0x3C, 0x4F,
0x97, 0x23, 0x07, 0xB9, 0xB6, 0xB5, 0x32, 0x28, 0x24, 0x0E, 0xCC, 0x1A, 0xA1, 0x74, 0x39, 0x06,
0xD9, 0x52, 0xD6, 0x38, 0xFC, 0x95, 0xBF, 0x84, 0x3A, 0x76, 0xA3, 0xC3, 0x54, 0xF2, 0x71, 0x4D,
0x2D, 0xE8, 0x9F, 0x58, 0x19, 0xE9, 0xD3, 0x5A, 0xCE, 0x30, 0x1E, 0xB5, 0xEE, 0xB5, 0x83, 0xF4,
0xB9, 0x23, 0xF3, 0xA1, 0xFC, 0xEA, 0x68, 0x2F, 0xAF, 0x22, 0x73, 0xF2, 0x21, 0x66, 0x8C, 0x29,
0xF2, 0x34, 0x7A, 0x39, 0xB9, 0x3C, 0x2C, 0x96, 0x54, 0x7A, 0x7E, 0xA5, 0x24, 0x98, 0xF7, 0x06,
0x78, 0x28, 0x70, 0x7A, 0x3C, 0x73, 0x8D, 0x82, 0xB1, 0x9C, 0x1E, 0xD9, 0xDB, 0xBB, 0xEF, 0x3F,
0xC2, 0x0F, 0xAF, 0x73, 0x0E, 0xC0, 0x01, 0x2E, 0x5B, 0x8A, 0xC4, 0x39, 0x5A, 0x71, 0xA9, 0x2B,
0xD9, 0xD3, 0x9A, 0x0D, 0x28, 0x95, 0xFE, 0x7E, 0xD8, 0xC7, 0x73, 0xDD, 0x77, 0x52, 0x56, 0x94,
0x80, 0x93, 0xD1, 0xFF, 0x02, 0x28, 0xE0, 0x18, 0xA1, 0xF2, 0x7E, 0x9A, 0x1C, 0xF2, 0x7B, 0x76,
0x2C, 0xF0, 0xB7, 0x39, 0xF3, 0x10, 0x08, 0x90, 0x8F, 0xA6, 0xEB, 0x5F, 0xF5, 0x1A, 0xB1, 0x72,
0xF0, 0x1B, 0x7A, 0xF4, 0xF7, 0x4D, 0x5C, 0xC0, 0x82, 0x1F, 0x27, 0xCE, 0xA4, 0x52, 0xB2, 0xE8,
0x24, 0xC7, 0xCA, 0x8C, 0xB9, 0xCB, 0x6C, 0xC5, 0xA0, 0x42, 0x18, 0x7F, 0xE5, 0xFA, 0xA9, 0x8E,
0xA0, 0xF4, 0x58, 0x78, 0xB9, 0x30, 0x86, 0x49, 0x01, 0x15, 0x8E, 0xB0, 0x22, 0x8C, 0xF5, 0x12,
0x64, 0xE6, 0x69, 0x90, 0xD6, 0x86, 0x92, 0x9B, 0x83, 0xD4, 0xF7, 0x01, 0x15, 0x9A, 0x7C, 0xF8,
0xB3, 0xCD, 0x0A, 0xA1, 0x3D, 0x49, 0x90, 0x21, 0x69, 0xD7, 0x25, 0xFC, 0x1A, 0x64, 0x22, 0x77,
0x7A, 0xBF, 0x3C, 0x1C, 0x4B, 0x06, 0x6E, 0x83, 0x03, 0x5D, 0x5C, 0x76, 0xEA, 0x84, 0x29, 0xB5,
0x7C, 0xC0, 0x74, 0xBC, 0x4A, 0x21, 0x7B, 0xDC, 0xFE, 0x1B, 0x1F, 0x77, 0x64, 0x20, 0x59, 0x6A,
0x0B, 0x48, 0xC2, 0x0E, 0x2D, 0xFF, 0xCE, 0x4C, 0x06, 0xED, 0x0E, 0x1C, 0xB6, 0x1A, 0x62, 0x79,
0xEC, 0x25, 0xD6, 0x89, 0xBF, 0x4F, 0x16, 0x75, 0x82, 0xD7, 0x98, 0x5C, 0xBA, 0x75, 0xBA, 0xD3,
0x2D, 0xC7, 0x47, 0xF3, 0xB6, 0x31, 0x54, 0xE0, 0x86, 0xFE, 0x29, 0x8E, 0xE2, 0x92, 0x79, 0x89,
0xE4, 0x43, 0xB4, 0x9C, 0xF7, 0xED, 0x1B, 0xA6, 0x0B, 0x0C, 0x69, 0x23, 0xF4, 0x7D, 0x0A, 0xA2,
0x8C, 0xEC, 0xD5, 0x2C, 0x8E, 0xB6, 0x20, 0x8F, 0xA6, 0xB9, 0x86, 0xB8, 0xBA, 0x59, 0xA3, 0xA7
};

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

@ -31,6 +31,7 @@
#include "wmr_hmd.h"
#include "wmr_common.h"
#include "wmr_config_key.h"
#include "wmr_protocol.h"
#include <stdio.h>
@ -151,8 +152,12 @@ hololens_sensors_read_packets(struct wmr_hmd *wh)
struct xrt_vec3 raw_accel[4];
for (int i = 0; i < 4; i++) {
vec3_from_hololens_gyro(wh->packet.gyro, i, &raw_gyro[i]);
vec3_from_hololens_accel(wh->packet.accel, i, &raw_accel[i]);
struct xrt_vec3 sample;
vec3_from_hololens_gyro(wh->packet.gyro, i, &sample);
math_quat_rotate_vec3(&wh->gyro_to_centerline.orientation, &sample, &raw_gyro[i]);
vec3_from_hololens_accel(wh->packet.accel, i, &sample);
math_quat_rotate_vec3(&wh->accel_to_centerline.orientation, &sample, &raw_accel[i]);
}
os_mutex_lock(&wh->fusion.mutex);
@ -455,10 +460,11 @@ wmr_read_config_raw(struct wmr_hmd *wh, uint8_t **out_data, size_t *out_size)
* seem to be little endian size of the data store.
*/
data_size = meta[0] | (meta[1] << 8);
data = calloc(1, data_size);
data = calloc(1, data_size + 1);
if (!data) {
return -1;
}
data[data_size] = '\0';
size = wmr_read_config_part(wh, 0x04, data, data_size);
WMR_DEBUG(wh, "(0x04, data) => %d", size);
@ -475,6 +481,51 @@ wmr_read_config_raw(struct wmr_hmd *wh, uint8_t **out_data, size_t *out_size)
return 0;
}
static int
wmr_read_config(struct wmr_hmd *wh)
{
unsigned char *data = NULL, *config_json_block;
size_t data_size;
int ret;
// Read config
ret = wmr_read_config_raw(wh, &data, &data_size);
if (ret < 0)
return ret;
/* De-obfuscate the JSON config */
/* FIXME: The header contains little-endian values that need swapping for big-endian */
struct wmr_config_header *hdr = (struct wmr_config_header *)data;
WMR_INFO(wh, "Manufacturer: %.*s", (int)sizeof(hdr->manufacturer), hdr->manufacturer);
WMR_INFO(wh, "Device: %.*s", (int)sizeof(hdr->device), hdr->device);
WMR_INFO(wh, "Serial: %.*s", (int)sizeof(hdr->serial), hdr->serial);
WMR_INFO(wh, "UID: %.*s", (int)sizeof(hdr->uid), hdr->uid);
WMR_INFO(wh, "Name: %.*s", (int)sizeof(hdr->name), hdr->name);
WMR_INFO(wh, "Revision: %.*s", (int)sizeof(hdr->revision), hdr->revision);
WMR_INFO(wh, "Revision Date: %.*s", (int)sizeof(hdr->revision_date), hdr->revision_date);
snprintf(wh->base.str, XRT_DEVICE_NAME_LEN, "%.*s", (int)sizeof(hdr->name), hdr->name);
if (hdr->json_start >= data_size || (data_size - hdr->json_start) < hdr->json_size) {
WMR_ERROR(wh, "Invalid WMR config block - incorrect sizes");
free(data);
return -1;
}
config_json_block = data + hdr->json_start + sizeof(uint16_t);
for (unsigned int i = 0; i < hdr->json_size - sizeof(uint16_t); i++) {
config_json_block[i] ^= wmr_config_key[i % sizeof(wmr_config_key)];
}
if (!wmr_config_parse(&wh->config, (char *)config_json_block, wh->log_level)) {
free(data);
return -1;
}
free(data);
return 0;
}
/*
*
@ -645,8 +696,6 @@ wmr_hmd_create(struct os_hid_device *hid_holo, struct os_hid_device *hid_ctrl, e
wh->hid_hololens_sensors_dev = hid_holo;
wh->hid_control_dev = hid_ctrl;
snprintf(wh->base.str, XRT_DEVICE_NAME_LEN, "HP Reverb VR Headset");
// Mutex before thread.
ret = os_mutex_init(&wh->fusion.mutex);
if (ret != 0) {
@ -665,33 +714,50 @@ wmr_hmd_create(struct os_hid_device *hid_holo, struct os_hid_device *hid_ctrl, e
return NULL;
}
if (wmr_hmd_activate(wh) != 0) {
WMR_ERROR(wh, "Activation of HMD failed");
wmr_hmd_destroy(&wh->base);
wh = NULL;
return NULL;
}
// Switch on IMU on the HMD.
hololens_sensors_enable_imu(wh);
// Setup input.
wh->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
// TODO: Read config file from HMD, provide guestimate values for now.
if (!wmr_config_parse(&wh->config)) {
// Read config file from HMD
if (wmr_read_config(wh) < 0) {
WMR_ERROR(wh, "Failed to load headset configuration!");
wmr_hmd_destroy(&wh->base);
wh = NULL;
return NULL;
}
// Compute centerline in the HMD's calibration coordinate space as the average of the two display poses
math_quat_slerp(&wh->config.eye_params[0].pose.orientation, &wh->config.eye_params[1].pose.orientation, 0.5f,
&wh->centerline.orientation);
wh->centerline.position.x =
(wh->config.eye_params[0].pose.position.x + wh->config.eye_params[1].pose.position.x) * 0.5f;
wh->centerline.position.y =
(wh->config.eye_params[0].pose.position.y + wh->config.eye_params[1].pose.position.y) * 0.5f;
wh->centerline.position.z =
(wh->config.eye_params[0].pose.position.z + wh->config.eye_params[1].pose.position.z) * 0.5f;
// Compute display and sensor offsets relative to the centerline
for (int dIdx = 0; dIdx < 2; ++dIdx) {
math_pose_invert(&wh->config.eye_params[dIdx].pose, &wh->display_to_centerline[dIdx]);
math_pose_transform(&wh->centerline, &wh->display_to_centerline[dIdx],
&wh->display_to_centerline[dIdx]);
}
math_pose_invert(&wh->config.accel_pose, &wh->accel_to_centerline);
math_pose_transform(&wh->centerline, &wh->accel_to_centerline, &wh->accel_to_centerline);
math_pose_invert(&wh->config.gyro_pose, &wh->gyro_to_centerline);
math_pose_transform(&wh->centerline, &wh->gyro_to_centerline, &wh->gyro_to_centerline);
math_pose_invert(&wh->config.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;
info.display.w_pixels = 4320;
info.display.h_pixels = 2160;
info.display.w_pixels = wh->config.eye_params[0].display_size.x;
info.display.h_pixels = wh->config.eye_params[0].display_size.y;
info.lens_horizontal_separation_meters =
fabs(wh->display_to_centerline[1].position.x - wh->display_to_centerline[0].position.x);
// TODO placeholder values below here
info.display.w_meters = 0.13f;
info.display.h_meters = 0.07f;
info.lens_horizontal_separation_meters = 0.13f / 2.0f;
info.lens_vertical_position_meters = 0.07f / 2.0f;
info.views[0].fov = 85.0f * (M_PI / 180.0f);
info.views[1].fov = 85.0f * (M_PI / 180.0f);
@ -727,6 +793,18 @@ wmr_hmd_create(struct os_hid_device *hid_holo, struct os_hid_device *hid_ctrl, e
wh->base.compute_distortion = compute_distortion_wmr;
u_distortion_mesh_fill_in_compute(&wh->base);
/* We're set up. Activate the HMD and turn on the IMU */
if (wmr_hmd_activate(wh) != 0) {
WMR_ERROR(wh, "Activation of HMD failed");
wmr_hmd_destroy(&wh->base);
wh = NULL;
return NULL;
}
// Switch on IMU on the HMD.
hololens_sensors_enable_imu(wh);
// Hand over hololens sensor device to reading thread.
ret = os_thread_helper_start(&wh->oth, wmr_run_thread, wh);
if (ret != 0) {

7
src/xrt/drivers/wmr/wmr_hmd.h
View file

@ -86,6 +86,13 @@ struct wmr_hmd
/* Distortion related parameters */
struct wmr_hmd_distortion_params distortion_params[2];
// Config-derived poses
struct xrt_pose centerline;
struct xrt_pose display_to_centerline[2];
struct xrt_pose accel_to_centerline;
struct xrt_pose gyro_to_centerline;
struct xrt_pose mag_to_centerline;
struct hololens_sensors_packet packet;
struct

14
src/xrt/drivers/wmr/wmr_protocol.h
View file

@ -56,6 +56,20 @@ struct hololens_sensors_packet
uint64_t video_timestamp[4];
};
struct wmr_config_header
{
uint32_t json_start;
uint32_t json_size;
char manufacturer[0x40];
char device[0x40];
char serial[0x40];
char uid[0x26];
char unk[0xd5];
char name[0x40];
char revision[0x20];
char revision_date[0x20];
};
/*!
* @}
*/