Driver code for a WMR HMD. More...

#include "xrt/xrt_config_drivers.h"
#include "xrt/xrt_config_have.h"
#include "xrt/xrt_config_build.h"
#include "xrt/xrt_config_os.h"
#include "xrt/xrt_defines.h"
#include "xrt/xrt_device.h"
#include "xrt/xrt_tracking.h"
#include "os/os_time.h"
#include "os/os_hid.h"
#include "math/m_api.h"
#include "math/m_mathinclude.h"
#include "math/m_predict.h"
#include "math/m_vec2.h"
#include "util/u_var.h"
#include "util/u_misc.h"
#include "util/u_time.h"
#include "util/u_debug.h"
#include "util/u_device.h"
#include "util/u_trace_marker.h"
#include "util/u_distortion_mesh.h"
#include "util/u_sink.h"
#include "util/u_linux.h"
#include "tracking/t_tracking.h"
#include "wmr_hmd.h"
#include "wmr_common.h"
#include "wmr_config_key.h"
#include "wmr_protocol.h"
#include "wmr_source.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <unistd.h>

Include dependency graph for wmr_hmd.c:

Macros
#define	CAMERA_FREQUENCY 30
	Observed value (OV7251) More...

#define	IMU_FREQUENCY 1000
	Observed value (ICM20602) More...

#define	IMU_SAMPLES_PER_PACKET 4
	There are 4 samples for each USB IMU packet. More...

#define	WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)
	Specifies whether the user wants to use a SLAM tracker. More...

#define	WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__)

#define	WMR_DEBUG_HEX(d, data, data_size) U_LOG_XDEV_IFL_D_HEX(&d->base, d->log_level, data, data_size)

#define	WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__)

#define	WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__)

#define	WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__)

#define	HID_SEND(hmd, HID, DATA, STR)

#define	HID_GET(hmd, HID, DATA, STR)

Functions
static int	wmr_hmd_activate_reverb (struct wmr_hmd *wh)

static void	wmr_hmd_deactivate_reverb (struct wmr_hmd *wh)

static void	wmr_hmd_screen_enable_reverb (struct wmr_hmd *wh, bool enable)

static int	wmr_hmd_activate_odyssey_plus (struct wmr_hmd *wh)

static void	wmr_hmd_deactivate_odyssey_plus (struct wmr_hmd *wh)

static void	wmr_hmd_screen_enable_odyssey_plus (struct wmr_hmd *wh, bool enable)

static void	hololens_sensors_decode_packet (struct wmr_hmd wh, struct hololens_sensors_packet pkt, const unsigned char *buffer, int size)

static void	hololens_ensure_controller (struct wmr_hmd *wh, uint8_t controller_id, uint16_t vid, uint16_t pid)

static void	hololens_handle_unknown (struct wmr_hmd wh, const unsigned char buffer, int size)

static void	hololens_handle_control (struct wmr_hmd wh, const unsigned char buffer, int size)

static void	hololens_handle_controller_status_packet (struct wmr_hmd wh, const unsigned char buffer, int size)

static void	hololens_handle_bt_iface_packet (struct wmr_hmd wh, const unsigned char buffer, int size)

static void	hololens_handle_controller_packet (struct wmr_hmd wh, const unsigned char buffer, int size)

static void	hololens_handle_debug (struct wmr_hmd wh, const unsigned char buffer, int size)

static void	hololens_handle_sensors_avg (struct wmr_hmd wh, const unsigned char buffer, int size)

static void	hololens_handle_sensors_all (struct wmr_hmd wh, const unsigned char buffer, int size)

static void	hololens_handle_sensors (struct wmr_hmd wh, const unsigned char buffer, int size)

static bool	hololens_sensors_read_packets (struct wmr_hmd *wh)

static void	control_ipd_value_decode (struct wmr_hmd wh, const unsigned char buffer, int size)

static bool	control_read_packets (struct wmr_hmd *wh)

static void *	wmr_run_thread (void *ptr)

static void	hololens_sensors_enable_imu (struct wmr_hmd *wh)

static void	wmr_hmd_refresh_debug_gui (struct wmr_hmd *wh)

static void	wmr_hmd_screen_enable_toggle (void *wh_ptr)

static int	wmr_config_command_sync (struct wmr_hmd wh, unsigned char type, unsigned char buf, int len)

static int	wmr_read_config_part (struct wmr_hmd wh, unsigned char type, unsigned char data, int len)

static XRT_MAYBE_UNUSED int	wmr_read_config_raw (struct wmr_hmd wh, uint8_t out_data, size_t out_size)

static int	wmr_read_config (struct wmr_hmd *wh)

static void	wmr_hmd_get_3dof_tracked_pose (struct xrt_device xdev, enum xrt_input_name name, uint64_t at_timestamp_ns, struct xrt_space_relation out_relation)

static XRT_MAYBE_UNUSED struct xrt_pose	wmr_hmd_correct_pose_from_basalt (struct xrt_pose pose)
	Specific pose corrections for Basalt and a WMR headset. More...

static void	wmr_hmd_get_slam_tracked_pose (struct xrt_device xdev, enum xrt_input_name name, uint64_t at_timestamp_ns, struct xrt_space_relation out_relation)

static void	wmr_hmd_get_tracked_pose (struct xrt_device xdev, enum xrt_input_name name, uint64_t at_timestamp_ns, struct xrt_space_relation out_relation)

static void	wmr_hmd_destroy (struct xrt_device *xdev)

static bool	compute_distortion_wmr (struct xrt_device xdev, uint32_t view, float u, float v, struct xrt_uv_triplet result)

static void	compute_distortion_bounds (struct wmr_hmd wh, int view, float out_angle_left, float out_angle_right, float out_angle_down, float *out_angle_up)

static XRT_MAYBE_UNUSED struct t_camera_calibration	wmr_hmd_get_cam_calib (struct wmr_hmd *wh, int cam_index)

static XRT_MAYBE_UNUSED struct xrt_vec2	wmr_hmd_camera_project (struct wmr_hmd *wh, struct xrt_vec3 p3d)

static XRT_MAYBE_UNUSED struct t_stereo_camera_calibration *	wmr_hmd_create_stereo_camera_calib (struct wmr_hmd *wh)
	Creates an OpenCV-compatible t_stereo_camera_calibration pointer from the WMR config. More...

static XRT_MAYBE_UNUSED void	wmr_hmd_fill_slam_cams_calibration (struct wmr_hmd *wh)
	Extended camera calibration info for SLAM. More...

static XRT_MAYBE_UNUSED struct t_imu_calibration	wmr_hmd_get_imu_calib (struct wmr_hmd *wh)

static XRT_MAYBE_UNUSED void	wmr_hmd_fill_slam_imu_calibration (struct wmr_hmd *wh)
	Extended IMU calibration data for SLAM. More...

static XRT_MAYBE_UNUSED void	wmr_hmd_fill_slam_calibration (struct wmr_hmd *wh)

static void	wmr_hmd_switch_hmd_tracker (void *wh_ptr)

static struct xrt_slam_sinks *	wmr_hmd_slam_track (struct wmr_hmd *wh)

static int	wmr_hmd_hand_track (struct wmr_hmd wh, struct t_stereo_camera_calibration stereo_calib, struct xrt_hand_masks_sink masks_sink, struct xrt_slam_sinks out_sinks, struct xrt_device *out_device)

static void	wmr_hmd_setup_ui (struct wmr_hmd *wh)

static bool	wmr_hmd_setup_trackers (struct wmr_hmd wh, struct xrt_slam_sinks out_sinks, struct xrt_device **out_handtracker)
	Procedure to setup trackers: 3dof, SLAM and hand tracking. More...

static bool	wmr_hmd_request_controller_status (struct wmr_hmd *wh)

void	wmr_hmd_create (enum wmr_headset_type hmd_type, struct os_hid_device hid_holo, struct os_hid_device hid_ctrl, struct xrt_prober_device dev_holo, enum u_logging_level log_level, struct xrt_device out_hmd, struct xrt_device out_handtracker, struct xrt_device out_left_controller, struct xrt_device *out_right_controller)

bool	wmr_hmd_send_controller_packet (struct wmr_hmd hmd, const uint8_t buffer, uint32_t buf_size)

int	wmr_hmd_read_sync_from_controller (struct wmr_hmd hmd, uint8_t buffer, uint32_t buf_size, int timeout_ms)

Variables
const struct wmr_headset_descriptor	headset_map []

const int	headset_map_n = sizeof(headset_map) / sizeof(headset_map[0])

Detailed Description

Driver code for a WMR HMD.

Author: Philipp Zabel phili.nosp@m.pp.z.nosp@m.abel@.nosp@m.gmai.nosp@m.l.com; nima01 nima_.nosp@m.zero.nosp@m._one@.nosp@m.prot.nosp@m.onmai.nosp@m.l.co.nosp@m.m; Jakob Bornecrantz jakob.nosp@m.@col.nosp@m.labor.nosp@m.a.co.nosp@m.m; Mateo de Mayo mateo.nosp@m..dem.nosp@m.ayo@c.nosp@m.olla.nosp@m.bora..nosp@m.com; Nova King techn.nosp@m.obab.nosp@m.oo@pr.nosp@m.oton.nosp@m..me

Macro Definition Documentation

◆ CAMERA_FREQUENCY

#define CAMERA_FREQUENCY 30

Observed value (OV7251)

◆ HID_GET

#define HID_GET	(	hmd,
		HID,
		DATA,
		STR
	)

Value:

        do {                                                                                                           \
                os_mutex_lock(&hmd->hid_lock);                                                                         \
                int _ret = os_hid_get_feature(HID, DATA[0], DATA, sizeof(DATA));                                       \
                os_mutex_unlock(&hmd->hid_lock);                                                                       \
                if (_ret < 0) {                                                                                        \
                        WMR_ERROR(wh, "Get (%s): %i", STR, _ret);                                                      \
                } else {                                                                                               \
                        WMR_DEBUG(wh, "0x%02x HID feature returned", DATA[0]);                                         \
                        WMR_DEBUG_HEX(wh, DATA, _ret);                                                                 \
                }                                                                                                      \
        } while (false);

◆ HID_SEND

#define HID_SEND	(	hmd,
		HID,
		DATA,
		STR
	)

Value:

        do {                                                                                                           \
                os_mutex_lock(&hmd->hid_lock);                                                                         \
                int _ret = os_hid_set_feature(HID, DATA, sizeof(DATA));                                                \
                os_mutex_unlock(&hmd->hid_lock);                                                                       \
                if (_ret < 0) {                                                                                        \
                        WMR_ERROR(wh, "Send (%s): %i", STR, _ret);                                                     \
                }                                                                                                      \
        } while (false);

◆ IMU_FREQUENCY

#define IMU_FREQUENCY 1000

Observed value (ICM20602)

◆ IMU_SAMPLES_PER_PACKET

#define IMU_SAMPLES_PER_PACKET 4

There are 4 samples for each USB IMU packet.

◆ WMR_TRACE

#define WMR_TRACE	(	d,
		...
	)	U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)

Specifies whether the user wants to use a SLAM tracker.

Specifies whether the user wants to use a SLAM tracker. Specifies whether the user wants to use the hand tracker. Specifies the y offset of the views.

Function Documentation

◆ wmr_hmd_correct_pose_from_basalt()

static XRT_MAYBE_UNUSED struct xrt_pose wmr_hmd_correct_pose_from_basalt ( struct xrt_pose pose )

inlinestatic

Specific pose corrections for Basalt and a WMR headset.

◆ wmr_hmd_create()

void wmr_hmd_create	(	enum wmr_headset_type	hmd_type,
		struct os_hid_device *	hid_holo,
		struct os_hid_device *	hid_ctrl,
		struct xrt_prober_device *	dev_holo,
		enum u_logging_level	log_level,
		struct xrt_device **	out_hmd,
		struct xrt_device **	out_handtracker,
		struct xrt_device **	out_left_controller,
		struct xrt_device **	out_right_controller
	)

Todo:: Could reach this due to !XRT_HAVE_LIBUSB but the HMD should keep working

◆ wmr_hmd_create_stereo_camera_calib()

static XRT_MAYBE_UNUSED struct t_stereo_camera_calibration* wmr_hmd_create_stereo_camera_calib ( struct wmr_hmd * wh )

static

Creates an OpenCV-compatible t_stereo_camera_calibration pointer from the WMR config.

Note that the camera model used on WMR headsets seems to be the same as the one in Azure-Kinect-Sensor-SDK. That model is slightly different than OpenCV's in the following ways:

There are "center of distortion", codx and cody, parameters
The terms that use the tangential parameters, p1 and p2, aren't multiplied by 2
There is a "metric radius" that delimits a valid area of distortion/undistortion

Thankfully, parameters of points 1 and 2 tend to be almost zero in practice. For 3, we place metric_radius into the calibration struct so that downstream tracking algorithms can use it as needed.

References T_DISTORTION_WMR, t_stereo_camera_calibration::t_stereo_camera_calibration_alloc(), and t_stereo_camera_calibration::view.

◆ wmr_hmd_deactivate_odyssey_plus()

static void wmr_hmd_deactivate_odyssey_plus ( struct wmr_hmd * wh )

static

Todo:: Power down IMU, and maybe more.

◆ wmr_hmd_deactivate_reverb()

static void wmr_hmd_deactivate_reverb ( struct wmr_hmd * wh )

static

Todo:: Power down IMU, and maybe more.

◆ wmr_hmd_fill_slam_cams_calibration()

static XRT_MAYBE_UNUSED void wmr_hmd_fill_slam_cams_calibration ( struct wmr_hmd * wh )

static

Extended camera calibration info for SLAM.

Note: The calibration json for the reverb G2v2 (the only 4-camera wmr headset we know about) has the HT2 and HT3 extrinsics flipped compared to the order the third and fourth camera images come from usb.

References t_slam_calibration::cam_count, t_slam_calibration::cams, wmr_hmd::config, xrt_matrix_4x4::math_matrix_4x4_isometry_from_pose(), wmr_inertial_sensor_config::pose, wmr_hmd::slam_calib, wmr_hmd_config::tcam_count, and wmr_hmd::tracking.

◆ wmr_hmd_fill_slam_imu_calibration()

static XRT_MAYBE_UNUSED void wmr_hmd_fill_slam_imu_calibration ( struct wmr_hmd * wh )

static

Extended IMU calibration data for SLAM.

Note: average_imus might change during runtime but the calibration data will be already submitted

References wmr_hmd::average_imus, IMU_FREQUENCY, and IMU_SAMPLES_PER_PACKET.

◆ wmr_hmd_setup_trackers()

static bool wmr_hmd_setup_trackers	(	struct wmr_hmd *	wh,
		struct xrt_slam_sinks *	out_sinks,
		struct xrt_device **	out_handtracker
	)

static

Procedure to setup trackers: 3dof, SLAM and hand tracking.

Determines which trackers to initialize and starts them. Fills out_sinks to stream raw data to for tracking. In the case of hand tracking being enabled, it returns a hand tracker device in out_handtracker.

Parameters

wh	the wmr headset device
out_sinks	sinks to stream video/IMU data to for tracking
out_handtracker	a newly created hand tracker device

Returns: true on success, false when an unexpected state is reached.

Variable Documentation

◆ headset_map

const struct wmr_headset_descriptor headset_map[]

Initial value:

= {
    {WMR_HEADSET_GENERIC, NULL, "Unknown WMR HMD", NULL, NULL, NULL}, 
    {WMR_HEADSET_HP_VR1000, "HP Reverb VR Headset VR1000-1xxx", "HP VR1000", NULL, NULL, NULL}, 
    {WMR_HEADSET_REVERB_G1, "HP Reverb VR Headset VR1000-2xxx", "HP Reverb", wmr_hmd_activate_reverb,
     wmr_hmd_deactivate_reverb, wmr_hmd_screen_enable_reverb},
    {WMR_HEADSET_REVERB_G2, "HP Reverb Virtual Reality Headset G2", "HP Reverb G2", wmr_hmd_activate_reverb,
     wmr_hmd_deactivate_reverb, wmr_hmd_screen_enable_reverb},
    {WMR_HEADSET_SAMSUNG_XE700X3AI, "Samsung Windows Mixed Reality XE700X3AI", "Samsung Odyssey",
     wmr_hmd_activate_odyssey_plus, wmr_hmd_deactivate_odyssey_plus, wmr_hmd_screen_enable_odyssey_plus},
    {WMR_HEADSET_SAMSUNG_800ZAA, "Samsung Windows Mixed Reality 800ZAA", "Samsung Odyssey+",
     wmr_hmd_activate_odyssey_plus, wmr_hmd_deactivate_odyssey_plus, wmr_hmd_screen_enable_odyssey_plus},
    {WMR_HEADSET_LENOVO_EXPLORER, "Lenovo VR-2511N", "Lenovo Explorer", NULL, NULL, NULL},
    {WMR_HEADSET_MEDION_ERAZER_X1000, "Medion Erazer X1000", "Medion Erazer", NULL, NULL, NULL},
    {WMR_HEADSET_DELL_VISOR, "DELL VR118", "Dell Visor", NULL, NULL, NULL},
}

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ CAMERA_FREQUENCY

◆ HID_GET

◆ HID_SEND

◆ IMU_FREQUENCY

◆ IMU_SAMPLES_PER_PACKET

◆ WMR_TRACE

Function Documentation

◆ wmr_hmd_correct_pose_from_basalt()

◆ wmr_hmd_create()

◆ wmr_hmd_create_stereo_camera_calib()

◆ wmr_hmd_deactivate_odyssey_plus()

◆ wmr_hmd_deactivate_reverb()

◆ wmr_hmd_fill_slam_cams_calibration()

◆ wmr_hmd_fill_slam_imu_calibration()

◆ wmr_hmd_setup_trackers()

Variable Documentation

◆ headset_map