diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c index 1b177c0b9..82b9537d4 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.c +++ b/src/xrt/drivers/wmr/wmr_hmd.c @@ -164,6 +164,28 @@ hololens_sensors_decode_packet(struct wmr_hmd *wh, } } +static void +hololens_ensure_controller(struct wmr_hmd *wh, uint8_t controller_id, uint16_t vid, uint16_t pid) +{ + if (controller_id >= WMR_MAX_CONTROLLERS) + return; + + if (wh->controller[controller_id] != NULL) + return; + + WMR_DEBUG(wh, "Adding controller device %d", controller_id); + + enum xrt_device_type controller_type = + controller_id == 0 ? XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER : XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER; + uint8_t hmd_cmd_base = controller_id == 0 ? 0x5 : 0xd; + + struct wmr_hmd_controller_connection *controller = + wmr_hmd_controller_create(wh, hmd_cmd_base, controller_type, vid, pid, wh->log_level); + + os_mutex_lock(&wh->controller_status_lock); + wh->controller[controller_id] = controller; + os_mutex_unlock(&wh->controller_status_lock); +} /* * @@ -220,7 +242,7 @@ hololens_handle_controller_status_packet(struct wmr_hmd *wh, const unsigned char break; } case WMR_CONTROLLER_STATUS_ONLINE: { - if (size < 10) { + if (size < 7) { WMR_TRACE(wh, "Got small controller online status packet (%i)", size); return; } @@ -230,17 +252,32 @@ hololens_handle_controller_status_packet(struct wmr_hmd *wh, const unsigned char uint16_t vid = read16(&buffer); uint16_t pid = read16(&buffer); - uint8_t unknown1 = read8(&buffer); - uint16_t unknown2160 = read16(&buffer); - WMR_TRACE(wh, "Controller %d online. VID 0x%04x PID 0x%04x val1 %u val2 %u", controller_id, vid, pid, - unknown1, unknown2160); + if (size >= 10) { + uint8_t unknown1 = read8(&buffer); + uint16_t unknown2160 = read16(&buffer); + WMR_TRACE(wh, "Controller %d online. VID 0x%04x PID 0x%04x val1 %u val2 %u", controller_id, vid, + pid, unknown1, unknown2160); + } else { + WMR_TRACE(wh, "Controller %d online. VID 0x%04x PID 0x%04x", controller_id, vid, pid); + } + + hololens_ensure_controller(wh, controller_id, vid, pid); break; } default: // WMR_DEBUG(wh, "Unknown controller status packet (%i) type 0x%02x", size, pkt_type); break; } + + os_mutex_lock(&wh->controller_status_lock); + if (controller_id == 0) + wh->have_left_controller_status = true; + else if (controller_id == 1) + wh->have_right_controller_status = true; + if (wh->have_left_controller_status && wh->have_right_controller_status) + os_cond_signal(&wh->controller_status_cond); + os_mutex_unlock(&wh->controller_status_lock); } static void @@ -281,19 +318,25 @@ hololens_handle_bt_iface_packet(struct wmr_hmd *wh, const unsigned char *buffer, static void hololens_handle_controller_packet(struct wmr_hmd *wh, const unsigned char *buffer, int size) { - DRV_TRACE_MARKER(); - - if (size >= 45) { - WMR_TRACE(wh, - "Got controller (%i)\n\t%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | %02x %02x %02x " - "%02x %02x %02x %02x %02x %02x %02x | %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", - size, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7], - buffer[8], buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15], - buffer[16], buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22], - buffer[23], buffer[24], buffer[25], buffer[26], buffer[27], buffer[28], buffer[29]); - } else { - WMR_TRACE(wh, "Got controller packet (%i)\n\t%02x", size, buffer[0]); + if (size < 45) { + WMR_TRACE(wh, "Got unknown short controller packet (%i)\n\t%02x", size, buffer[0]); + return; } + + uint8_t packet_id = buffer[0]; + struct wmr_controller_connection *controller = NULL; + + if (packet_id == WMR_MS_HOLOLENS_MSG_LEFT_CONTROLLER) { + controller = (struct wmr_controller_connection *)wh->controller[0]; + } else if (packet_id == WMR_MS_HOLOLENS_MSG_RIGHT_CONTROLLER) { + controller = (struct wmr_controller_connection *)wh->controller[1]; + } + + if (controller == NULL) + return; /* Controller online message not yet seen */ + + uint64_t now_ns = os_monotonic_get_ns(); + wmr_controller_connection_receive_bytes(controller, now_ns, (uint8_t *)buffer, size); } static void @@ -1153,6 +1196,22 @@ wmr_hmd_destroy(struct xrt_device *xdev) // Destroy the thread object. os_thread_helper_destroy(&wh->oth); + // Disconnect tunnelled controllers + os_mutex_lock(&wh->controller_status_lock); + if (wh->controller[0] != NULL) { + struct wmr_controller_connection *wcc = (struct wmr_controller_connection *)wh->controller[0]; + wmr_controller_connection_disconnect(wcc); + } + + if (wh->controller[1] != NULL) { + struct wmr_controller_connection *wcc = (struct wmr_controller_connection *)wh->controller[1]; + wmr_controller_connection_disconnect(wcc); + } + os_mutex_unlock(&wh->controller_status_lock); + + os_mutex_destroy(&wh->controller_status_lock); + os_cond_destroy(&wh->controller_status_cond); + if (wh->hid_hololens_sensors_dev != NULL) { os_hid_destroy(wh->hid_hololens_sensors_dev); wh->hid_hololens_sensors_dev = NULL; @@ -1854,6 +1913,14 @@ precompute_sensor_transforms(struct wmr_hmd *wh) wh->P_imu_me = P_oxr_acc_me; // Assume accel pose is IMU pose } +static bool +wmr_hmd_request_controller_status(struct wmr_hmd *wh) +{ + DRV_TRACE_MARKER(); + unsigned char cmd[64] = {WMR_MS_HOLOLENS_MSG_BT_CONTROL, WMR_MS_HOLOLENS_MSG_CONTROLLER_STATUS}; + return wmr_hmd_send_controller_packet(wh, cmd, sizeof(cmd)); +} + void wmr_hmd_create(enum wmr_headset_type hmd_type, struct os_hid_device *hid_holo, @@ -1905,6 +1972,22 @@ wmr_hmd_create(enum wmr_headset_type hmd_type, return; } + ret = os_mutex_init(&wh->controller_status_lock); + if (ret != 0) { + WMR_ERROR(wh, "Failed to init Controller status mutex!"); + wmr_hmd_destroy(&wh->base); + wh = NULL; + return; + } + + ret = os_cond_init(&wh->controller_status_cond); + if (ret != 0) { + WMR_ERROR(wh, "Failed to init Controller status cond!"); + wmr_hmd_destroy(&wh->base); + wh = NULL; + return; + } + // Thread and other state. ret = os_thread_helper_init(&wh->oth); if (ret != 0) { @@ -2044,6 +2127,25 @@ wmr_hmd_create(enum wmr_headset_type hmd_type, return; } + /* Send controller status request to check for online controllers + * and wait 250ms for the reports for Reverb G2 and Odyssey+ */ + if (wh->hmd_desc->hmd_type == WMR_HEADSET_REVERB_G2 || wh->hmd_desc->hmd_type == WMR_HEADSET_SAMSUNG_800ZAA) { + bool have_controller_status = false; + + os_mutex_lock(&wh->controller_status_lock); + if (wmr_hmd_request_controller_status(wh)) { + /* @todo: Add a timed version of os_cond_wait and a timeout? */ + /* This will be signalled from the reader thread */ + os_cond_wait(&wh->controller_status_cond, &wh->controller_status_lock); + have_controller_status = true; + } + os_mutex_unlock(&wh->controller_status_lock); + + if (!have_controller_status) { + WMR_WARN(wh, "Failed to request controller status from HMD"); + } + } + wmr_hmd_setup_ui(wh); *out_hmd = &wh->base; diff --git a/src/xrt/drivers/wmr/wmr_hmd.h b/src/xrt/drivers/wmr/wmr_hmd.h index 54395677a..cbb8dd759 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.h +++ b/src/xrt/drivers/wmr/wmr_hmd.h @@ -28,12 +28,15 @@ #include "wmr_config.h" #include "wmr_camera.h" #include "wmr_common.h" +#include "wmr_hmd_controller.h" #ifdef __cplusplus extern "C" { #endif +/* Support 2 controllers on HP Reverb G2 */ +#define WMR_MAX_CONTROLLERS 2 struct wmr_hmd; @@ -189,6 +192,14 @@ struct wmr_hmd char hand_status[128]; char slam_status[128]; } gui; + + /* Tunnelled controller devices (Reverb G2, Odyssey+) handling */ + struct os_mutex controller_status_lock; + struct os_cond controller_status_cond; + bool have_left_controller_status; + bool have_right_controller_status; + + struct wmr_hmd_controller_connection *controller[WMR_MAX_CONTROLLERS]; }; static inline struct wmr_hmd * diff --git a/src/xrt/drivers/wmr/wmr_hmd_controller.c b/src/xrt/drivers/wmr/wmr_hmd_controller.c index 209d292f1..09d5513ad 100644 --- a/src/xrt/drivers/wmr/wmr_hmd_controller.c +++ b/src/xrt/drivers/wmr/wmr_hmd_controller.c @@ -48,12 +48,12 @@ send_bytes_to_controller(struct wmr_controller_connection *wcc, const uint8_t *b struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc); bool res = false; - assert(buf_size < 64); - os_mutex_lock(&conn->lock); if (!conn->disconnected && buf_size > 0) { uint8_t outbuf[64]; + assert(buf_size <= sizeof(outbuf)); + memcpy(outbuf, buffer, buf_size); outbuf[0] += conn->hmd_cmd_base; res = wmr_hmd_send_controller_packet(conn->hmd, outbuf, buf_size); @@ -72,6 +72,9 @@ read_sync_from_controller(struct wmr_controller_connection *wcc, uint8_t *buffer os_mutex_lock(&conn->lock); if (!conn->disconnected && buf_size > 0) { res = wmr_hmd_read_sync_from_controller(conn->hmd, buffer, buf_size, timeout_ms); + if (res > 0) { + buffer[0] -= conn->hmd_cmd_base; + } } os_mutex_unlock(&conn->lock); @@ -115,6 +118,7 @@ wmr_hmd_controller_connection_disconnect(struct wmr_controller_connection *base) } else { os_mutex_lock(&conn->lock); conn->disconnected = true; + conn->base.wcb = NULL; os_mutex_unlock(&conn->lock); } } @@ -174,7 +178,9 @@ struct xrt_device * wmr_hmd_controller_connection_get_controller(struct wmr_hmd_controller_connection *wcc) { struct wmr_controller_base *wcb = wcc->base.wcb; - struct xrt_device *xdev = &wcb->base; + if (wcb == NULL) + return NULL; + struct xrt_device *xdev = &wcb->base; return xdev; } diff --git a/src/xrt/drivers/wmr/wmr_protocol.h b/src/xrt/drivers/wmr/wmr_protocol.h index 45323d11a..984865727 100644 --- a/src/xrt/drivers/wmr/wmr_protocol.h +++ b/src/xrt/drivers/wmr/wmr_protocol.h @@ -37,6 +37,7 @@ extern "C" { #define WMR_MS_HOLOLENS_MSG_BT_IFACE 0x05 /* Bluetooth interface */ #define WMR_MS_HOLOLENS_MSG_LEFT_CONTROLLER 0x06 /* Left controller */ #define WMR_MS_HOLOLENS_MSG_RIGHT_CONTROLLER 0x0E /* Right controller */ +#define WMR_MS_HOLOLENS_MSG_BT_CONTROL 0x16 /* BT control message on Reverb G2 & Odyssey+ */ #define WMR_MS_HOLOLENS_MSG_CONTROLLER_STATUS 0x17 // Messages types specific to WMR Hololens Sensors' companion devices @@ -52,6 +53,16 @@ extern "C" { #define WMR_CONTROLLER_STATUS_OFFLINE 0x1 #define WMR_CONTROLLER_STATUS_ONLINE 0x2 +/* Messages we can send the G2 via WMR_MS_HOLOLENS_MSG_BT_CONTROL */ +enum wmr_bt_control_msg +{ + WMR_BT_CONTROL_MSG_ONLINE_STATUS = 0x04, + WMR_BT_CONTROL_MSG_PAIR = 0x05, + WMR_BT_CONTROL_MSG_UNPAIR = 0x06, + WMR_BT_CONTROL_MSG_PAIRING_STATUS = 0x08, + WMR_BT_CONTROL_MSG_CMD_STATUS = 0x09, +}; + #define STR_TO_U32(s) ((uint32_t)(((s)[0]) | ((s)[1] << 8) | ((s)[2] << 16) | ((s)[3] << 24))) #define WMR_MAGIC STR_TO_U32("Dlo+")