+118 -16

src/xrt/drivers/wmr/wmr_hmd.c

··· 164 164 } 165 165 } 166 166 167 167 + static void 168 168 + hololens_ensure_controller(struct wmr_hmd *wh, uint8_t controller_id, uint16_t vid, uint16_t pid) 169 169 + { 170 170 + if (controller_id >= WMR_MAX_CONTROLLERS) 171 171 + return; 172 172 + 173 173 + if (wh->controller[controller_id] != NULL) 174 174 + return; 175 175 + 176 176 + WMR_DEBUG(wh, "Adding controller device %d", controller_id); 177 177 + 178 178 + enum xrt_device_type controller_type = 179 179 + controller_id == 0 ? XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER : XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER; 180 180 + uint8_t hmd_cmd_base = controller_id == 0 ? 0x5 : 0xd; 181 181 + 182 182 + struct wmr_hmd_controller_connection *controller = 183 183 + wmr_hmd_controller_create(wh, hmd_cmd_base, controller_type, vid, pid, wh->log_level); 184 184 + 185 185 + os_mutex_lock(&wh->controller_status_lock); 186 186 + wh->controller[controller_id] = controller; 187 187 + os_mutex_unlock(&wh->controller_status_lock); 188 188 + } 167 189 168 190 /* 169 191 * ··· 220 242 break; 221 243 } 222 244 case WMR_CONTROLLER_STATUS_ONLINE: { 223 223 - if (size < 10) { 245 245 + if (size < 7) { 224 246 WMR_TRACE(wh, "Got small controller online status packet (%i)", size); 225 247 return; 226 248 } ··· 230 252 231 253 uint16_t vid = read16(&buffer); 232 254 uint16_t pid = read16(&buffer); 233 233 - uint8_t unknown1 = read8(&buffer); 234 234 - uint16_t unknown2160 = read16(&buffer); 255 255 + 256 256 + if (size >= 10) { 257 257 + uint8_t unknown1 = read8(&buffer); 258 258 + uint16_t unknown2160 = read16(&buffer); 259 259 + WMR_TRACE(wh, "Controller %d online. VID 0x%04x PID 0x%04x val1 %u val2 %u", controller_id, vid, 260 260 + pid, unknown1, unknown2160); 261 261 + } else { 262 262 + WMR_TRACE(wh, "Controller %d online. VID 0x%04x PID 0x%04x", controller_id, vid, pid); 263 263 + } 235 264 236 236 - WMR_TRACE(wh, "Controller %d online. VID 0x%04x PID 0x%04x val1 %u val2 %u", controller_id, vid, pid, 237 237 - unknown1, unknown2160); 265 265 + hololens_ensure_controller(wh, controller_id, vid, pid); 238 266 break; 239 267 } 240 268 default: // 241 269 WMR_DEBUG(wh, "Unknown controller status packet (%i) type 0x%02x", size, pkt_type); 242 270 break; 243 271 } 272 272 + 273 273 + os_mutex_lock(&wh->controller_status_lock); 274 274 + if (controller_id == 0) 275 275 + wh->have_left_controller_status = true; 276 276 + else if (controller_id == 1) 277 277 + wh->have_right_controller_status = true; 278 278 + if (wh->have_left_controller_status && wh->have_right_controller_status) 279 279 + os_cond_signal(&wh->controller_status_cond); 280 280 + os_mutex_unlock(&wh->controller_status_lock); 244 281 } 245 282 246 283 static void ··· 281 318 static void 282 319 hololens_handle_controller_packet(struct wmr_hmd *wh, const unsigned char *buffer, int size) 283 320 { 284 284 - DRV_TRACE_MARKER(); 321 321 + if (size < 45) { 322 322 + WMR_TRACE(wh, "Got unknown short controller packet (%i)\n\t%02x", size, buffer[0]); 323 323 + return; 324 324 + } 285 325 286 286 - if (size >= 45) { 287 287 - WMR_TRACE(wh, 288 288 - "Got controller (%i)\n\t%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | %02x %02x %02x " 289 289 - "%02x %02x %02x %02x %02x %02x %02x | %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", 290 290 - size, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7], 291 291 - buffer[8], buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15], 292 292 - buffer[16], buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22], 293 293 - buffer[23], buffer[24], buffer[25], buffer[26], buffer[27], buffer[28], buffer[29]); 294 294 - } else { 295 295 - WMR_TRACE(wh, "Got controller packet (%i)\n\t%02x", size, buffer[0]); 326 326 + uint8_t packet_id = buffer[0]; 327 327 + struct wmr_controller_connection *controller = NULL; 328 328 + 329 329 + if (packet_id == WMR_MS_HOLOLENS_MSG_LEFT_CONTROLLER) { 330 330 + controller = (struct wmr_controller_connection *)wh->controller[0]; 331 331 + } else if (packet_id == WMR_MS_HOLOLENS_MSG_RIGHT_CONTROLLER) { 332 332 + controller = (struct wmr_controller_connection *)wh->controller[1]; 296 333 } 334 334 + 335 335 + if (controller == NULL) 336 336 + return; /* Controller online message not yet seen */ 337 337 + 338 338 + uint64_t now_ns = os_monotonic_get_ns(); 339 339 + wmr_controller_connection_receive_bytes(controller, now_ns, (uint8_t *)buffer, size); 297 340 } 298 341 299 342 static void ··· 1153 1196 // Destroy the thread object. 1154 1197 os_thread_helper_destroy(&wh->oth); 1155 1198 1199 1199 + // Disconnect tunnelled controllers 1200 1200 + os_mutex_lock(&wh->controller_status_lock); 1201 1201 + if (wh->controller[0] != NULL) { 1202 1202 + struct wmr_controller_connection *wcc = (struct wmr_controller_connection *)wh->controller[0]; 1203 1203 + wmr_controller_connection_disconnect(wcc); 1204 1204 + } 1205 1205 + 1206 1206 + if (wh->controller[1] != NULL) { 1207 1207 + struct wmr_controller_connection *wcc = (struct wmr_controller_connection *)wh->controller[1]; 1208 1208 + wmr_controller_connection_disconnect(wcc); 1209 1209 + } 1210 1210 + os_mutex_unlock(&wh->controller_status_lock); 1211 1211 + 1212 1212 + os_mutex_destroy(&wh->controller_status_lock); 1213 1213 + os_cond_destroy(&wh->controller_status_cond); 1214 1214 + 1156 1215 if (wh->hid_hololens_sensors_dev != NULL) { 1157 1216 os_hid_destroy(wh->hid_hololens_sensors_dev); 1158 1217 wh->hid_hololens_sensors_dev = NULL; ··· 1854 1913 wh->P_imu_me = P_oxr_acc_me; // Assume accel pose is IMU pose 1855 1914 } 1856 1915 1916 1916 + static bool 1917 1917 + wmr_hmd_request_controller_status(struct wmr_hmd *wh) 1918 1918 + { 1919 1919 + DRV_TRACE_MARKER(); 1920 1920 + unsigned char cmd[64] = {WMR_MS_HOLOLENS_MSG_BT_CONTROL, WMR_MS_HOLOLENS_MSG_CONTROLLER_STATUS}; 1921 1921 + return wmr_hmd_send_controller_packet(wh, cmd, sizeof(cmd)); 1922 1922 + } 1923 1923 + 1857 1924 void 1858 1925 wmr_hmd_create(enum wmr_headset_type hmd_type, 1859 1926 struct os_hid_device *hid_holo, ··· 1905 1972 return; 1906 1973 } 1907 1974 1975 1975 + ret = os_mutex_init(&wh->controller_status_lock); 1976 1976 + if (ret != 0) { 1977 1977 + WMR_ERROR(wh, "Failed to init Controller status mutex!"); 1978 1978 + wmr_hmd_destroy(&wh->base); 1979 1979 + wh = NULL; 1980 1980 + return; 1981 1981 + } 1982 1982 + 1983 1983 + ret = os_cond_init(&wh->controller_status_cond); 1984 1984 + if (ret != 0) { 1985 1985 + WMR_ERROR(wh, "Failed to init Controller status cond!"); 1986 1986 + wmr_hmd_destroy(&wh->base); 1987 1987 + wh = NULL; 1988 1988 + return; 1989 1989 + } 1990 1990 + 1908 1991 // Thread and other state. 1909 1992 ret = os_thread_helper_init(&wh->oth); 1910 1993 if (ret != 0) { ··· 2042 2125 wmr_hmd_destroy(&wh->base); 2043 2126 wh = NULL; 2044 2127 return; 2128 2128 + } 2129 2129 + 2130 2130 + /* Send controller status request to check for online controllers 2131 2131 + * and wait 250ms for the reports for Reverb G2 and Odyssey+ */ 2132 2132 + if (wh->hmd_desc->hmd_type == WMR_HEADSET_REVERB_G2 || wh->hmd_desc->hmd_type == WMR_HEADSET_SAMSUNG_800ZAA) { 2133 2133 + bool have_controller_status = false; 2134 2134 + 2135 2135 + os_mutex_lock(&wh->controller_status_lock); 2136 2136 + if (wmr_hmd_request_controller_status(wh)) { 2137 2137 + /* @todo: Add a timed version of os_cond_wait and a timeout? */ 2138 2138 + /* This will be signalled from the reader thread */ 2139 2139 + os_cond_wait(&wh->controller_status_cond, &wh->controller_status_lock); 2140 2140 + have_controller_status = true; 2141 2141 + } 2142 2142 + os_mutex_unlock(&wh->controller_status_lock); 2143 2143 + 2144 2144 + if (!have_controller_status) { 2145 2145 + WMR_WARN(wh, "Failed to request controller status from HMD"); 2146 2146 + } 2045 2147 } 2046 2148 2047 2149 wmr_hmd_setup_ui(wh);

+11

src/xrt/drivers/wmr/wmr_hmd.h

··· 28 28 #include "wmr_config.h" 29 29 #include "wmr_camera.h" 30 30 #include "wmr_common.h" 31 31 + #include "wmr_hmd_controller.h" 31 32 32 33 33 34 #ifdef __cplusplus 34 35 extern "C" { 35 36 #endif 36 37 38 38 + /* Support 2 controllers on HP Reverb G2 */ 39 39 + #define WMR_MAX_CONTROLLERS 2 37 40 38 41 struct wmr_hmd; 39 42 ··· 189 192 char hand_status[128]; 190 193 char slam_status[128]; 191 194 } gui; 195 195 + 196 196 + /* Tunnelled controller devices (Reverb G2, Odyssey+) handling */ 197 197 + struct os_mutex controller_status_lock; 198 198 + struct os_cond controller_status_cond; 199 199 + bool have_left_controller_status; 200 200 + bool have_right_controller_status; 201 201 + 202 202 + struct wmr_hmd_controller_connection *controller[WMR_MAX_CONTROLLERS]; 192 203 }; 193 204 194 205 static inline struct wmr_hmd *

+9 -3

src/xrt/drivers/wmr/wmr_hmd_controller.c

··· 48 48 struct wmr_hmd_controller_connection *conn = (struct wmr_hmd_controller_connection *)(wcc); 49 49 bool res = false; 50 50 51 51 - assert(buf_size < 64); 52 52 - 53 51 os_mutex_lock(&conn->lock); 54 52 if (!conn->disconnected && buf_size > 0) { 55 53 uint8_t outbuf[64]; 54 54 + 55 55 + assert(buf_size <= sizeof(outbuf)); 56 56 57 57 memcpy(outbuf, buffer, buf_size); 58 58 outbuf[0] += conn->hmd_cmd_base; ··· 72 72 os_mutex_lock(&conn->lock); 73 73 if (!conn->disconnected && buf_size > 0) { 74 74 res = wmr_hmd_read_sync_from_controller(conn->hmd, buffer, buf_size, timeout_ms); 75 75 + if (res > 0) { 76 76 + buffer[0] -= conn->hmd_cmd_base; 77 77 + } 75 78 } 76 79 os_mutex_unlock(&conn->lock); 77 80 ··· 115 118 } else { 116 119 os_mutex_lock(&conn->lock); 117 120 conn->disconnected = true; 121 121 + conn->base.wcb = NULL; 118 122 os_mutex_unlock(&conn->lock); 119 123 } 120 124 } ··· 174 178 wmr_hmd_controller_connection_get_controller(struct wmr_hmd_controller_connection *wcc) 175 179 { 176 180 struct wmr_controller_base *wcb = wcc->base.wcb; 177 177 - struct xrt_device *xdev = &wcb->base; 181 181 + if (wcb == NULL) 182 182 + return NULL; 178 183 184 184 + struct xrt_device *xdev = &wcb->base; 179 185 return xdev; 180 186 }

+11

src/xrt/drivers/wmr/wmr_protocol.h

··· 37 37 #define WMR_MS_HOLOLENS_MSG_BT_IFACE 0x05 /* Bluetooth interface */ 38 38 #define WMR_MS_HOLOLENS_MSG_LEFT_CONTROLLER 0x06 /* Left controller */ 39 39 #define WMR_MS_HOLOLENS_MSG_RIGHT_CONTROLLER 0x0E /* Right controller */ 40 40 + #define WMR_MS_HOLOLENS_MSG_BT_CONTROL 0x16 /* BT control message on Reverb G2 & Odyssey+ */ 40 41 #define WMR_MS_HOLOLENS_MSG_CONTROLLER_STATUS 0x17 41 42 42 43 // Messages types specific to WMR Hololens Sensors' companion devices ··· 51 52 #define WMR_CONTROLLER_STATUS_UNPAIRED 0x0 52 53 #define WMR_CONTROLLER_STATUS_OFFLINE 0x1 53 54 #define WMR_CONTROLLER_STATUS_ONLINE 0x2 55 55 + 56 56 + /* Messages we can send the G2 via WMR_MS_HOLOLENS_MSG_BT_CONTROL */ 57 57 + enum wmr_bt_control_msg 58 58 + { 59 59 + WMR_BT_CONTROL_MSG_ONLINE_STATUS = 0x04, 60 60 + WMR_BT_CONTROL_MSG_PAIR = 0x05, 61 61 + WMR_BT_CONTROL_MSG_UNPAIR = 0x06, 62 62 + WMR_BT_CONTROL_MSG_PAIRING_STATUS = 0x08, 63 63 + WMR_BT_CONTROL_MSG_CMD_STATUS = 0x09, 64 64 + }; 54 65 55 66 #define STR_TO_U32(s) ((uint32_t)(((s)[0]) | ((s)[1] << 8) | ((s)[2] << 16) | ((s)[3] << 24))) 56 67 #define WMR_MAGIC STR_TO_U32("Dlo+")