xrt: adapt driver to new get_tracked_pose api change

+2 -1

src/xrt/drivers/android/android_sensors.c

··· 170 free(android); 171 } 172 173 - static void 174 android_device_get_tracked_pose(struct xrt_device *xdev, 175 enum xrt_input_name name, 176 int64_t at_timestamp_ns, ··· 189 XRT_SPACE_RELATION_POSITION_VALID_BIT); 190 191 *out_relation = new_relation; 192 } 193 194

··· 170 free(android); 171 } 172 173 + static xrt_result_t 174 android_device_get_tracked_pose(struct xrt_device *xdev, 175 enum xrt_input_name name, 176 int64_t at_timestamp_ns, ··· 189 XRT_SPACE_RELATION_POSITION_VALID_BIT); 190 191 *out_relation = new_relation; 192 + return XRT_SUCCESS; 193 } 194 195

+5 -3

src/xrt/drivers/arduino/arduino_device.c

··· 273 * 274 */ 275 276 - static void 277 arduino_get_fusion_pose(struct arduino_device *ad, enum xrt_input_name name, struct xrt_space_relation *out_relation) 278 { 279 out_relation->pose.orientation = ad->fusion.rot; ··· 281 //! @todo assuming that orientation is actually currently tracked. 282 out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | 283 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 284 } 285 286 static void ··· 331 return XRT_SUCCESS; 332 } 333 334 - static void 335 arduino_device_get_tracked_pose(struct xrt_device *xdev, 336 enum xrt_input_name name, 337 int64_t at_timestamp_ns, ··· 340 struct arduino_device *ad = arduino_device(xdev); 341 342 (void)at_timestamp_ns; 343 - arduino_get_fusion_pose(ad, name, out_relation); 344 } 345 346

··· 273 * 274 */ 275 276 + static xrt_result_t 277 arduino_get_fusion_pose(struct arduino_device *ad, enum xrt_input_name name, struct xrt_space_relation *out_relation) 278 { 279 out_relation->pose.orientation = ad->fusion.rot; ··· 281 //! @todo assuming that orientation is actually currently tracked. 282 out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | 283 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 284 + 285 + return XRT_SUCCESS; 286 } 287 288 static void ··· 333 return XRT_SUCCESS; 334 } 335 336 + static xrt_result_t 337 arduino_device_get_tracked_pose(struct xrt_device *xdev, 338 enum xrt_input_name name, 339 int64_t at_timestamp_ns, ··· 342 struct arduino_device *ad = arduino_device(xdev); 343 344 (void)at_timestamp_ns; 345 + return arduino_get_fusion_pose(ad, name, out_relation); 346 } 347 348

+3 -1

src/xrt/drivers/daydream/daydream_device.c

··· 310 return XRT_SUCCESS; 311 } 312 313 - static void 314 daydream_device_get_tracked_pose(struct xrt_device *xdev, 315 enum xrt_input_name name, 316 int64_t at_timestamp_ns, ··· 320 321 (void)at_timestamp_ns; 322 daydream_get_fusion_pose(daydream, name, out_relation); 323 } 324 325

··· 310 return XRT_SUCCESS; 311 } 312 313 + static xrt_result_t 314 daydream_device_get_tracked_pose(struct xrt_device *xdev, 315 enum xrt_input_name name, 316 int64_t at_timestamp_ns, ··· 320 321 (void)at_timestamp_ns; 322 daydream_get_fusion_pose(daydream, name, out_relation); 323 + 324 + return XRT_SUCCESS; 325 } 326 327

+3 -1

src/xrt/drivers/euroc/euroc_device.c

··· 102 return (struct euroc_device *)xdev; 103 } 104 105 - static void 106 euroc_device_get_tracked_pose(struct xrt_device *xdev, 107 enum xrt_input_name name, 108 int64_t at_timestamp_ns, ··· 128 out_relation->relation_flags = (enum xrt_space_relation_flags)( 129 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 130 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 131 } 132 133 static void

··· 102 return (struct euroc_device *)xdev; 103 } 104 105 + static xrt_result_t 106 euroc_device_get_tracked_pose(struct xrt_device *xdev, 107 enum xrt_input_name name, 108 int64_t at_timestamp_ns, ··· 128 out_relation->relation_flags = (enum xrt_space_relation_flags)( 129 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 130 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 131 + 132 + return XRT_SUCCESS; 133 } 134 135 static void

+5 -4

src/xrt/drivers/hdk/hdk_device.cpp

··· 213 return 1; 214 } 215 216 - static void 217 hdk_device_get_tracked_pose(struct xrt_device *xdev, 218 enum xrt_input_name name, 219 int64_t requested_timestamp_ns, ··· 222 struct hdk_device *hd = hdk_device(xdev); 223 224 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 225 - HDK_ERROR(hd, "unknown input name"); 226 - return; 227 } 228 229 os_mutex_lock(&hd->lock); ··· 231 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; 232 HDK_TRACE(hd, "GET_TRACKED_POSE: No pose"); 233 os_mutex_unlock(&hd->lock); 234 - return; 235 } 236 237 out_relation->pose.orientation = hd->quat; ··· 248 249 HDK_TRACE(hd, "GET_TRACKED_POSE (%f, %f, %f, %f) ANG_VEL (%f, %f, %f)", hd->quat.x, hd->quat.y, hd->quat.z, 250 hd->quat.w, hd->ang_vel_quat.x, hd->ang_vel_quat.y, hd->ang_vel_quat.z); 251 } 252 253 static void *

··· 213 return 1; 214 } 215 216 + static xrt_result_t 217 hdk_device_get_tracked_pose(struct xrt_device *xdev, 218 enum xrt_input_name name, 219 int64_t requested_timestamp_ns, ··· 222 struct hdk_device *hd = hdk_device(xdev); 223 224 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 225 + U_LOG_XDEV_UNSUPPORTED_INPUT(&hd->base, hd->log_level, name); 226 + return XRT_ERROR_INPUT_UNSUPPORTED; 227 } 228 229 os_mutex_lock(&hd->lock); ··· 231 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; 232 HDK_TRACE(hd, "GET_TRACKED_POSE: No pose"); 233 os_mutex_unlock(&hd->lock); 234 + return XRT_SUCCESS; 235 } 236 237 out_relation->pose.orientation = hd->quat; ··· 248 249 HDK_TRACE(hd, "GET_TRACKED_POSE (%f, %f, %f, %f) ANG_VEL (%f, %f, %f)", hd->quat.x, hd->quat.y, hd->quat.z, 250 hd->quat.w, hd->ang_vel_quat.x, hd->ang_vel_quat.y, hd->ang_vel_quat.z); 251 + return XRT_SUCCESS; 252 } 253 254 static void *

+29 -15

src/xrt/drivers/ht_ctrl_emu/ht_ctrl_emu.cpp

··· 175 return out_relation.pose; 176 } 177 178 - static void 179 do_grip_pose(struct xrt_hand_joint_set *joint_set, 180 struct xrt_space_relation *out_relation, 181 float grip_offset_from_palm, ··· 211 math_quat_from_plus_x_z(&plus_x, &plus_z, &out_relation->pose.orientation); 212 213 out_relation->relation_flags = valid_flags; 214 } 215 216 217 218 - static void 219 get_other_two(struct cemu_device *dev, 220 int64_t head_timestamp_ns, 221 int64_t hand_timestamp_ns, ··· 223 xrt_hand_joint_set *out_secondary) 224 { 225 struct xrt_space_relation head_rel; 226 - xrt_device_get_tracked_pose(dev->sys->in_head, XRT_INPUT_GENERIC_HEAD_POSE, head_timestamp_ns, &head_rel); 227 *out_head = head_rel.pose; 228 int other; 229 if (dev->hand_index == 0) { ··· 235 int64_t noop; 236 xrt_device_get_hand_tracking(dev->sys->in_hand, dev->sys->out_hand[other]->ht_input_name, hand_timestamp_ns, 237 out_secondary, &noop); 238 } 239 240 // Mostly stolen from 241 // https://github.com/maluoi/StereoKit/blob/048b689f71d080a67fde29838c0362a49b88b3d6/StereoKitC/systems/hand/hand_oxr_articulated.cpp#L149 242 - static void 243 do_aim_pose(struct cemu_device *dev, 244 struct xrt_hand_joint_set *joint_set_primary, 245 int64_t head_timestamp_ns, ··· 251 struct xrt_hand_joint_set joint_set_secondary; 252 #if 0 253 // "Jakob way" 254 - get_other_two(dev, hand_timestamp_ns, hand_timestamp_ns, &head, &joint_set_secondary); 255 #else 256 // "Moses way" 257 - get_other_two(dev, head_timestamp_ns, hand_timestamp_ns, &head, &joint_set_secondary); 258 #endif 259 - 260 261 // Average shoulder width for women:37cm, men:41cm, center of shoulder 262 // joint is around 4cm inwards ··· 313 math_quat_from_plus_x_z(&out_x_vector, &ray_direction, &out_relation->pose.orientation); 314 315 out_relation->relation_flags = valid_flags; 316 } 317 318 // Pose for controller emulation 319 - static void 320 cemu_device_get_tracked_pose(struct xrt_device *xdev, 321 enum xrt_input_name name, 322 int64_t at_timestamp_ns, ··· 326 struct cemu_system *sys = dev->sys; 327 328 if (name != XRT_INPUT_SIMPLE_GRIP_POSE && name != XRT_INPUT_SIMPLE_AIM_POSE) { 329 - CEMU_ERROR(dev, "unknown input name %d for controller pose", name); 330 - return; 331 } 332 static int64_t hand_timestamp_ns; 333 ··· 337 338 if (joint_set.is_active == false) { 339 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; 340 - return; 341 } 342 343 - 344 - 345 switch (name) { 346 case XRT_INPUT_SIMPLE_GRIP_POSE: { 347 - do_grip_pose(&joint_set, out_relation, sys->grip_offset_from_palm, dev->hand_index); 348 break; 349 } 350 case XRT_INPUT_SIMPLE_AIM_POSE: { 351 // Assume that now we're doing everything in the timestamp from the hand-tracker, so use 352 // hand_timestamp_ns. This will cause the controller to lag behind but otherwise be correct 353 - do_aim_pose(dev, &joint_set, at_timestamp_ns, hand_timestamp_ns, out_relation); 354 break; 355 } 356 default: assert(false); 357 } 358 } 359 360 static void

··· 175 return out_relation.pose; 176 } 177 178 + static xrt_result_t 179 do_grip_pose(struct xrt_hand_joint_set *joint_set, 180 struct xrt_space_relation *out_relation, 181 float grip_offset_from_palm, ··· 211 math_quat_from_plus_x_z(&plus_x, &plus_z, &out_relation->pose.orientation); 212 213 out_relation->relation_flags = valid_flags; 214 + 215 + return XRT_SUCCESS; 216 } 217 218 219 220 + static xrt_result_t 221 get_other_two(struct cemu_device *dev, 222 int64_t head_timestamp_ns, 223 int64_t hand_timestamp_ns, ··· 225 xrt_hand_joint_set *out_secondary) 226 { 227 struct xrt_space_relation head_rel; 228 + xrt_result_t xret = 229 + xrt_device_get_tracked_pose(dev->sys->in_head, XRT_INPUT_GENERIC_HEAD_POSE, head_timestamp_ns, &head_rel); 230 + if (xret != XRT_SUCCESS) { 231 + return xret; 232 + } 233 + 234 *out_head = head_rel.pose; 235 int other; 236 if (dev->hand_index == 0) { ··· 242 int64_t noop; 243 xrt_device_get_hand_tracking(dev->sys->in_hand, dev->sys->out_hand[other]->ht_input_name, hand_timestamp_ns, 244 out_secondary, &noop); 245 + 246 + return xret; 247 } 248 249 // Mostly stolen from 250 // https://github.com/maluoi/StereoKit/blob/048b689f71d080a67fde29838c0362a49b88b3d6/StereoKitC/systems/hand/hand_oxr_articulated.cpp#L149 251 + static xrt_result_t 252 do_aim_pose(struct cemu_device *dev, 253 struct xrt_hand_joint_set *joint_set_primary, 254 int64_t head_timestamp_ns, ··· 260 struct xrt_hand_joint_set joint_set_secondary; 261 #if 0 262 // "Jakob way" 263 + xrt_result_t xret = get_other_two(dev, hand_timestamp_ns, hand_timestamp_ns, &head, &joint_set_secondary); 264 #else 265 // "Moses way" 266 + xrt_result_t xret = get_other_two(dev, head_timestamp_ns, hand_timestamp_ns, &head, &joint_set_secondary); 267 #endif 268 + if (xret != XRT_SUCCESS) { 269 + return xret; 270 + } 271 272 // Average shoulder width for women:37cm, men:41cm, center of shoulder 273 // joint is around 4cm inwards ··· 324 math_quat_from_plus_x_z(&out_x_vector, &ray_direction, &out_relation->pose.orientation); 325 326 out_relation->relation_flags = valid_flags; 327 + 328 + return xret; 329 } 330 331 // Pose for controller emulation 332 + static xrt_result_t 333 cemu_device_get_tracked_pose(struct xrt_device *xdev, 334 enum xrt_input_name name, 335 int64_t at_timestamp_ns, ··· 339 struct cemu_system *sys = dev->sys; 340 341 if (name != XRT_INPUT_SIMPLE_GRIP_POSE && name != XRT_INPUT_SIMPLE_AIM_POSE) { 342 + U_LOG_XDEV_UNSUPPORTED_INPUT(&dev->base, dev->sys->log_level, name); 343 + return XRT_ERROR_INPUT_UNSUPPORTED; 344 } 345 static int64_t hand_timestamp_ns; 346 ··· 350 351 if (joint_set.is_active == false) { 352 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; 353 + return XRT_SUCCESS; 354 } 355 356 + xrt_result_t xret = XRT_SUCCESS; 357 switch (name) { 358 case XRT_INPUT_SIMPLE_GRIP_POSE: { 359 + xret = do_grip_pose(&joint_set, out_relation, sys->grip_offset_from_palm, dev->hand_index); 360 break; 361 } 362 case XRT_INPUT_SIMPLE_AIM_POSE: { 363 // Assume that now we're doing everything in the timestamp from the hand-tracker, so use 364 // hand_timestamp_ns. This will cause the controller to lag behind but otherwise be correct 365 + xret = do_aim_pose(dev, &joint_set, at_timestamp_ns, hand_timestamp_ns, out_relation); 366 break; 367 } 368 default: assert(false); 369 } 370 + 371 + return xret; 372 } 373 374 static void

+3 -1

src/xrt/drivers/hydra/hydra_driver.c

··· 482 return XRT_SUCCESS; 483 } 484 485 - static void 486 hydra_device_get_tracked_pose(struct xrt_device *xdev, 487 enum xrt_input_name name, 488 int64_t at_timestamp_ns, ··· 505 // HYDRA_SPEW(hd, "GET_TRACKED_POSE (%f, %f, %f) (%f, %f, %f, %f) ", 506 // pos.x, 507 // pos.y, pos.z, quat.x, quat.y, quat.z, quat.w); 508 } 509 510 static void

··· 482 return XRT_SUCCESS; 483 } 484 485 + static xrt_result_t 486 hydra_device_get_tracked_pose(struct xrt_device *xdev, 487 enum xrt_input_name name, 488 int64_t at_timestamp_ns, ··· 505 // HYDRA_SPEW(hd, "GET_TRACKED_POSE (%f, %f, %f) (%f, %f, %f, %f) ", 506 // pos.x, 507 // pos.y, pos.z, quat.x, quat.y, quat.z, quat.w); 508 + 509 + return XRT_SUCCESS; 510 } 511 512 static void

+12 -4

src/xrt/drivers/multi_wrapper/multi.c

··· 79 m_relation_chain_resolve(&xrc, out_relation); 80 } 81 82 - static void 83 get_tracked_pose(struct xrt_device *xdev, 84 enum xrt_input_name name, 85 int64_t at_timestamp_ns, ··· 91 92 struct xrt_space_relation tracker_relation; 93 94 - xrt_device_get_tracked_pose(tracker, tracker_input_name, at_timestamp_ns, &tracker_relation); 95 96 switch (d->override_type) { 97 case XRT_TRACKING_OVERRIDE_DIRECT: { ··· 101 struct xrt_device *target = d->tracking_override.target; 102 103 struct xrt_space_relation target_relation; 104 - xrt_device_get_tracked_pose(target, name, at_timestamp_ns, &target_relation); 105 - 106 107 // just use the origin of the tracker space as reference frame 108 struct xrt_space_relation in_target_space; ··· 116 &in_target_space, out_relation); 117 } break; 118 } 119 } 120 121 static void

··· 79 m_relation_chain_resolve(&xrc, out_relation); 80 } 81 82 + static xrt_result_t 83 get_tracked_pose(struct xrt_device *xdev, 84 enum xrt_input_name name, 85 int64_t at_timestamp_ns, ··· 91 92 struct xrt_space_relation tracker_relation; 93 94 + xrt_result_t xret = 95 + xrt_device_get_tracked_pose(tracker, tracker_input_name, at_timestamp_ns, &tracker_relation); 96 + if (xret != XRT_SUCCESS) { 97 + return xret; 98 + } 99 100 switch (d->override_type) { 101 case XRT_TRACKING_OVERRIDE_DIRECT: { ··· 105 struct xrt_device *target = d->tracking_override.target; 106 107 struct xrt_space_relation target_relation; 108 + xret = xrt_device_get_tracked_pose(target, name, at_timestamp_ns, &target_relation); 109 + if (xret != XRT_SUCCESS) { 110 + break; 111 + } 112 113 // just use the origin of the tracker space as reference frame 114 struct xrt_space_relation in_target_space; ··· 122 &in_target_space, out_relation); 123 } break; 124 } 125 + 126 + return xret; 127 } 128 129 static void

+4 -3

src/xrt/drivers/north_star/ns_hmd.c

··· 367 u_device_free(&ns->base); 368 } 369 370 - static void 371 ns_hmd_get_tracked_pose(struct xrt_device *xdev, 372 enum xrt_input_name name, 373 int64_t at_timestamp_ns, ··· 377 NS_DEBUG(ns, "Called!"); 378 379 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 380 - NS_ERROR(ns, "unknown input name"); 381 - return; 382 } 383 384 *out_relation = ns->no_tracker_relation; // you can change this using the debug gui 385 } 386 387 static void

··· 367 u_device_free(&ns->base); 368 } 369 370 + static xrt_result_t 371 ns_hmd_get_tracked_pose(struct xrt_device *xdev, 372 enum xrt_input_name name, 373 int64_t at_timestamp_ns, ··· 377 NS_DEBUG(ns, "Called!"); 378 379 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 380 + U_LOG_XDEV_UNSUPPORTED_INPUT(&ns->base, ns->log_level, name); 381 + return XRT_ERROR_INPUT_UNSUPPORTED; 382 } 383 384 *out_relation = ns->no_tracker_relation; // you can change this using the debug gui 385 + return XRT_SUCCESS; 386 } 387 388 static void

+6 -4

src/xrt/drivers/ohmd/oh_device.c

··· 373 return (ohd->ohmd_device_type == OPENHMD_GENERIC_TRACKER) && name == XRT_INPUT_GENERIC_TRACKER_POSE; 374 } 375 376 - static void 377 oh_device_get_tracked_pose(struct xrt_device *xdev, 378 enum xrt_input_name name, 379 int64_t at_timestamp_ns, ··· 384 struct xrt_vec3 pos = XRT_VEC3_ZERO; 385 386 if (!check_head_pose(ohd, name) && !check_controller_pose(ohd, name) && !check_tracker_pose(ohd, name)) { 387 - OHMD_ERROR(ohd, "unknown input name: %d", name); 388 - return; 389 } 390 391 ohmd_ctx_update(ohd->ctx); ··· 429 */ 430 *out_relation = ohd->last_relation; 431 OHMD_TRACE(ohd, "GET_TRACKED_POSE (%s) - no new data", ohd->base.str); 432 - return; 433 } 434 435 /*! ··· 470 // Update state within driver 471 ohd->last_update = (int64_t)now; 472 ohd->last_relation = *out_relation; 473 } 474 475

··· 373 return (ohd->ohmd_device_type == OPENHMD_GENERIC_TRACKER) && name == XRT_INPUT_GENERIC_TRACKER_POSE; 374 } 375 376 + static xrt_result_t 377 oh_device_get_tracked_pose(struct xrt_device *xdev, 378 enum xrt_input_name name, 379 int64_t at_timestamp_ns, ··· 384 struct xrt_vec3 pos = XRT_VEC3_ZERO; 385 386 if (!check_head_pose(ohd, name) && !check_controller_pose(ohd, name) && !check_tracker_pose(ohd, name)) { 387 + U_LOG_XDEV_UNSUPPORTED_INPUT(&ohd->base, ohd->log_level, name); 388 + return XRT_ERROR_INPUT_UNSUPPORTED; 389 } 390 391 ohmd_ctx_update(ohd->ctx); ··· 429 */ 430 *out_relation = ohd->last_relation; 431 OHMD_TRACE(ohd, "GET_TRACKED_POSE (%s) - no new data", ohd->base.str); 432 + return XRT_SUCCESS; 433 } 434 435 /*! ··· 470 // Update state within driver 471 ohd->last_update = (int64_t)now; 472 ohd->last_relation = *out_relation; 473 + 474 + return XRT_SUCCESS; 475 } 476 477

+31 -45

src/xrt/drivers/psmv/psmv_driver.c

··· 794 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 795 } 796 797 - static void 798 - psmv_push_pose_offset(enum xrt_input_name name, struct xrt_relation_chain *xrc) 799 - { 800 - /* 801 - * Both the grip and aim pose needs adjustments, the grip is a rotated 802 - * body center pose, while the aim pose needs to rotated and translated 803 - * to the tip of the ball. 804 - */ 805 - if (name != XRT_INPUT_PSMV_AIM_POSE && name != XRT_INPUT_PSMV_GRIP_POSE) { 806 - return; 807 - } 808 - 809 - float y = 0.0; 810 - if (name == XRT_INPUT_PSMV_AIM_POSE) { 811 - y += PSMV_BALL_FROM_IMU_Y_M; 812 - y += PSMV_BALL_DIAMETER_M / 2.0; 813 - } 814 - 815 - struct xrt_pose pose = { 816 - {0.7071068, 0, 0, 0.7071068}, 817 - {0, y, 0}, 818 - }; 819 - 820 - m_relation_chain_push_pose(xrc, &pose); 821 - } 822 - 823 824 /* 825 * ··· 888 } 889 890 static xrt_result_t 891 - psmv_device_get_relation_chain(struct xrt_device *xdev, 892 - enum xrt_input_name name, 893 - uint64_t at_timestamp_ns, 894 - struct xrt_relation_chain *xrc) 895 { 896 struct psmv_device *psmv = psmv_device(xdev); 897 898 - psmv_push_pose_offset(name, xrc); 899 900 - struct xrt_space_relation *rel = m_relation_chain_reserve(xrc); 901 902 - if (psmv->ball != NULL) { 903 - xrt_tracked_psmv_get_tracked_pose(psmv->ball, name, at_timestamp_ns, rel); 904 - } else { 905 - psmv_get_fusion_pose(psmv, name, at_timestamp_ns, rel); 906 } 907 908 - return XRT_SUCCESS; 909 - } 910 911 - static void 912 - psmv_device_get_tracked_pose(struct xrt_device *xdev, 913 - enum xrt_input_name name, 914 - int64_t at_timestamp_ns, 915 - struct xrt_space_relation *out_relation) 916 - { 917 struct xrt_relation_chain xrc = {0}; 918 919 - psmv_device_get_relation_chain(xdev, name, at_timestamp_ns, &xrc); 920 921 m_relation_chain_resolve(&xrc, out_relation); 922 } 923 924 static float

··· 794 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 795 } 796 797 798 /* 799 * ··· 862 } 863 864 static xrt_result_t 865 + psmv_device_get_tracked_pose(struct xrt_device *xdev, 866 + enum xrt_input_name name, 867 + int64_t at_timestamp_ns, 868 + struct xrt_space_relation *out_relation) 869 { 870 struct psmv_device *psmv = psmv_device(xdev); 871 872 + if (name != XRT_INPUT_PSMV_AIM_POSE && name != XRT_INPUT_PSMV_GRIP_POSE) { 873 + U_LOG_XDEV_UNSUPPORTED_INPUT(&psmv->base, psmv->log_level, name); 874 + return XRT_ERROR_INPUT_UNSUPPORTED; 875 + } 876 877 + /* 878 + * Both the grip and aim pose needs adjustments, the grip is a rotated 879 + * body center pose, while the aim pose needs to rotated and translated 880 + * to the tip of the ball. 881 + */ 882 883 + float y = 0.0; 884 + if (name == XRT_INPUT_PSMV_AIM_POSE) { 885 + y += PSMV_BALL_FROM_IMU_Y_M; 886 + y += PSMV_BALL_DIAMETER_M / 2.0; 887 } 888 889 + struct xrt_pose pose = { 890 + {0.7071068, 0, 0, 0.7071068}, 891 + {0, y, 0}, 892 + }; 893 894 struct xrt_relation_chain xrc = {0}; 895 + m_relation_chain_push_pose(&xrc, &pose); 896 897 + struct xrt_space_relation *rel = m_relation_chain_reserve(&xrc); 898 + 899 + if (psmv->ball != NULL) { 900 + xrt_tracked_psmv_get_tracked_pose(psmv->ball, name, at_timestamp_ns, rel); 901 + } else { 902 + psmv_get_fusion_pose(psmv, name, at_timestamp_ns, rel); 903 + } 904 905 m_relation_chain_resolve(&xrc, out_relation); 906 + 907 + return XRT_SUCCESS; 908 } 909 910 static float

+5 -3

src/xrt/drivers/pssense/pssense_driver.c

··· 757 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 758 } 759 760 - static void 761 pssense_get_tracked_pose(struct xrt_device *xdev, 762 enum xrt_input_name name, 763 int64_t at_timestamp_ns, ··· 766 struct pssense_device *pssense = (struct pssense_device *)xdev; 767 768 if (name != XRT_INPUT_PSSENSE_AIM_POSE && name != XRT_INPUT_PSSENSE_GRIP_POSE) { 769 - PSSENSE_ERROR(pssense, "Unknown pose name requested %u", name); 770 - return; 771 } 772 773 struct xrt_relation_chain xrc = {0}; ··· 785 os_mutex_unlock(&pssense->lock); 786 787 m_relation_chain_resolve(&xrc, out_relation); 788 } 789 790 static xrt_result_t

··· 757 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 758 } 759 760 + static xrt_result_t 761 pssense_get_tracked_pose(struct xrt_device *xdev, 762 enum xrt_input_name name, 763 int64_t at_timestamp_ns, ··· 766 struct pssense_device *pssense = (struct pssense_device *)xdev; 767 768 if (name != XRT_INPUT_PSSENSE_AIM_POSE && name != XRT_INPUT_PSSENSE_GRIP_POSE) { 769 + U_LOG_XDEV_UNSUPPORTED_INPUT(&pssense->base, pssense->log_level, name); 770 + return XRT_ERROR_INPUT_UNSUPPORTED; 771 } 772 773 struct xrt_relation_chain xrc = {0}; ··· 785 os_mutex_unlock(&pssense->lock); 786 787 m_relation_chain_resolve(&xrc, out_relation); 788 + 789 + return XRT_SUCCESS; 790 } 791 792 static xrt_result_t

+5 -3

src/xrt/drivers/psvr/psvr_device.c

··· 957 return XRT_SUCCESS; 958 } 959 960 - static void 961 psvr_device_get_tracked_pose(struct xrt_device *xdev, 962 enum xrt_input_name name, 963 int64_t at_timestamp_ns, ··· 966 struct psvr_device *psvr = psvr_device(xdev); 967 968 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 969 - PSVR_ERROR(psvr, "unknown input name"); 970 - return; 971 } 972 973 os_mutex_lock(&psvr->device_mutex); ··· 993 //! @todo Move this to the tracker. 994 // Make sure that the orientation is valid. 995 math_quat_normalize(&out_relation->pose.orientation); 996 } 997 998 static void

··· 957 return XRT_SUCCESS; 958 } 959 960 + static xrt_result_t 961 psvr_device_get_tracked_pose(struct xrt_device *xdev, 962 enum xrt_input_name name, 963 int64_t at_timestamp_ns, ··· 966 struct psvr_device *psvr = psvr_device(xdev); 967 968 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 969 + U_LOG_XDEV_UNSUPPORTED_INPUT(&psvr->base, psvr->log_level, name); 970 + return XRT_ERROR_INPUT_UNSUPPORTED; 971 } 972 973 os_mutex_lock(&psvr->device_mutex); ··· 993 //! @todo Move this to the tracker. 994 // Make sure that the orientation is valid. 995 math_quat_normalize(&out_relation->pose.orientation); 996 + 997 + return XRT_SUCCESS; 998 } 999 1000 static void

+5 -3

src/xrt/drivers/qwerty/qwerty_device.c

··· 136 } 137 } 138 139 - static void 140 qwerty_get_tracked_pose(struct xrt_device *xd, 141 enum xrt_input_name name, 142 int64_t at_timestamp_ns, ··· 146 147 if (name != XRT_INPUT_GENERIC_HEAD_POSE && name != XRT_INPUT_SIMPLE_GRIP_POSE && 148 name != XRT_INPUT_SIMPLE_AIM_POSE) { 149 - QWERTY_ERROR(qd, "Unexpected input name = 0x%04X", name >> 8); 150 - return; 151 } 152 153 // Position ··· 201 out_relation->relation_flags = 202 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 203 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT; 204 } 205 206 static void

··· 136 } 137 } 138 139 + static xrt_result_t 140 qwerty_get_tracked_pose(struct xrt_device *xd, 141 enum xrt_input_name name, 142 int64_t at_timestamp_ns, ··· 146 147 if (name != XRT_INPUT_GENERIC_HEAD_POSE && name != XRT_INPUT_SIMPLE_GRIP_POSE && 148 name != XRT_INPUT_SIMPLE_AIM_POSE) { 149 + U_LOG_XDEV_UNSUPPORTED_INPUT(&qd->base, qd->sys->log_level, name); 150 + return XRT_ERROR_INPUT_UNSUPPORTED; 151 } 152 153 // Position ··· 201 out_relation->relation_flags = 202 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 203 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT; 204 + 205 + return XRT_SUCCESS; 206 } 207 208 static void

+4 -3

src/xrt/drivers/realsense/rs_ddev.c

··· 388 * 389 */ 390 391 - static void 392 rs_ddev_get_tracked_pose(struct xrt_device *xdev, 393 enum xrt_input_name name, 394 int64_t at_timestamp_ns, ··· 397 struct rs_ddev *rs = rs_ddev(xdev); 398 399 if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { 400 - U_LOG_E("unknown input name"); 401 - return; 402 } 403 404 m_relation_history_get(rs->relation_hist, at_timestamp_ns, out_relation); 405 } 406 407 static void

··· 388 * 389 */ 390 391 + static xrt_result_t 392 rs_ddev_get_tracked_pose(struct xrt_device *xdev, 393 enum xrt_input_name name, 394 int64_t at_timestamp_ns, ··· 397 struct rs_ddev *rs = rs_ddev(xdev); 398 399 if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { 400 + U_LOG_XDEV_UNSUPPORTED_INPUT(&rs->base, u_log_get_global_level(), name); 401 + return XRT_ERROR_INPUT_UNSUPPORTED; 402 } 403 404 m_relation_history_get(rs->relation_hist, at_timestamp_ns, out_relation); 405 + return XRT_SUCCESS; 406 } 407 408 static void

+3 -1

src/xrt/drivers/realsense/rs_hdev.c

··· 273 return out_relation.pose; 274 } 275 276 - static void 277 rs_hdev_get_tracked_pose(struct xrt_device *xdev, 278 enum xrt_input_name name, 279 int64_t at_timestamp_ns, ··· 305 out_relation->relation_flags = (enum xrt_space_relation_flags)( 306 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 307 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 308 } 309 310 static void

··· 273 return out_relation.pose; 274 } 275 276 + static xrt_result_t 277 rs_hdev_get_tracked_pose(struct xrt_device *xdev, 278 enum xrt_input_name name, 279 int64_t at_timestamp_ns, ··· 305 out_relation->relation_flags = (enum xrt_space_relation_flags)( 306 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 307 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 308 + 309 + return XRT_SUCCESS; 310 } 311 312 static void

+5 -3

src/xrt/drivers/remote/r_device.c

··· 97 return XRT_SUCCESS; 98 } 99 100 - static void 101 r_device_get_tracked_pose(struct xrt_device *xdev, 102 enum xrt_input_name name, 103 int64_t at_timestamp_ns, ··· 108 109 if (name != XRT_INPUT_INDEX_AIM_POSE && name != XRT_INPUT_INDEX_GRIP_POSE && 110 name != XRT_INPUT_GENERIC_PALM_POSE) { 111 - U_LOG_E("Unknown input name: 0x%0x", name); 112 - return; 113 } 114 115 struct r_remote_controller_data *latest = rd->is_left ? &r->latest.left : &r->latest.right; ··· 133 } else { 134 out_relation->relation_flags = 0; 135 } 136 } 137 138 static void

··· 97 return XRT_SUCCESS; 98 } 99 100 + static xrt_result_t 101 r_device_get_tracked_pose(struct xrt_device *xdev, 102 enum xrt_input_name name, 103 int64_t at_timestamp_ns, ··· 108 109 if (name != XRT_INPUT_INDEX_AIM_POSE && name != XRT_INPUT_INDEX_GRIP_POSE && 110 name != XRT_INPUT_GENERIC_PALM_POSE) { 111 + U_LOG_XDEV_UNSUPPORTED_INPUT(&rd->base, u_log_get_global_level(), name); 112 + return XRT_ERROR_INPUT_UNSUPPORTED; 113 } 114 115 struct r_remote_controller_data *latest = rd->is_left ? &r->latest.left : &r->latest.right; ··· 133 } else { 134 out_relation->relation_flags = 0; 135 } 136 + 137 + return XRT_SUCCESS; 138 } 139 140 static void

+6 -2

src/xrt/drivers/remote/r_hmd.c

··· 56 u_device_free(&rh->base); 57 } 58 59 - static void 60 r_hmd_get_tracked_pose(struct xrt_device *xdev, 61 enum xrt_input_name name, 62 int64_t at_timestamp_ns, ··· 66 67 switch (name) { 68 case XRT_INPUT_GENERIC_HEAD_POSE: copy_head_center_to_relation(rh, out_relation); break; 69 - default: U_LOG_E("Unknown input name"); break; 70 } 71 } 72 73 static void

··· 56 u_device_free(&rh->base); 57 } 58 59 + static xrt_result_t 60 r_hmd_get_tracked_pose(struct xrt_device *xdev, 61 enum xrt_input_name name, 62 int64_t at_timestamp_ns, ··· 66 67 switch (name) { 68 case XRT_INPUT_GENERIC_HEAD_POSE: copy_head_center_to_relation(rh, out_relation); break; 69 + default: 70 + U_LOG_XDEV_UNSUPPORTED_INPUT(&rh->base, u_log_get_global_level(), name); 71 + return XRT_ERROR_INPUT_UNSUPPORTED; 72 } 73 + 74 + return XRT_SUCCESS; 75 } 76 77 static void

+5 -3

src/xrt/drivers/rift_s/rift_s_controller.c

··· 539 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 540 } 541 542 - static void 543 rift_s_controller_get_tracked_pose(struct xrt_device *xdev, 544 enum xrt_input_name name, 545 int64_t at_timestamp_ns, ··· 548 struct rift_s_controller *ctrl = (struct rift_s_controller *)(xdev); 549 550 if (name != XRT_INPUT_TOUCH_AIM_POSE && name != XRT_INPUT_TOUCH_GRIP_POSE) { 551 - RIFT_S_ERROR("unknown pose name requested"); 552 - return; 553 } 554 555 struct xrt_relation_chain xrc = {0}; ··· 571 os_mutex_unlock(&ctrl->mutex); 572 573 m_relation_chain_resolve(&xrc, out_relation); 574 } 575 576 static void

··· 539 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 540 } 541 542 + static xrt_result_t 543 rift_s_controller_get_tracked_pose(struct xrt_device *xdev, 544 enum xrt_input_name name, 545 int64_t at_timestamp_ns, ··· 548 struct rift_s_controller *ctrl = (struct rift_s_controller *)(xdev); 549 550 if (name != XRT_INPUT_TOUCH_AIM_POSE && name != XRT_INPUT_TOUCH_GRIP_POSE) { 551 + U_LOG_XDEV_UNSUPPORTED_INPUT(&ctrl->base, rift_s_log_level, name); 552 + return XRT_ERROR_INPUT_UNSUPPORTED; 553 } 554 555 struct xrt_relation_chain xrc = {0}; ··· 571 os_mutex_unlock(&ctrl->mutex); 572 573 m_relation_chain_resolve(&xrc, out_relation); 574 + 575 + return XRT_SUCCESS; 576 } 577 578 static void

+4 -3

src/xrt/drivers/rift_s/rift_s_hmd.c

··· 43 #define DEG_TO_RAD(D) ((D)*M_PI / 180.) 44 45 46 - static void 47 rift_s_get_tracked_pose(struct xrt_device *xdev, 48 enum xrt_input_name name, 49 int64_t at_timestamp_ns, ··· 52 struct rift_s_hmd *hmd = (struct rift_s_hmd *)(xdev); 53 54 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 55 - RIFT_S_ERROR("Unknown input name"); 56 - return; 57 } 58 59 U_ZERO(out_relation); 60 61 rift_s_tracker_get_tracked_pose(hmd->tracker, RIFT_S_TRACKER_POSE_DEVICE, at_timestamp_ns, out_relation); 62 } 63 64 void

··· 43 #define DEG_TO_RAD(D) ((D)*M_PI / 180.) 44 45 46 + static xrt_result_t 47 rift_s_get_tracked_pose(struct xrt_device *xdev, 48 enum xrt_input_name name, 49 int64_t at_timestamp_ns, ··· 52 struct rift_s_hmd *hmd = (struct rift_s_hmd *)(xdev); 53 54 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 55 + U_LOG_XDEV_UNSUPPORTED_INPUT(&hmd->base, rift_s_log_level, name); 56 + return XRT_ERROR_INPUT_UNSUPPORTED; 57 } 58 59 U_ZERO(out_relation); 60 61 rift_s_tracker_get_tracked_pose(hmd->tracker, RIFT_S_TRACKER_POSE_DEVICE, at_timestamp_ns, out_relation); 62 + return XRT_SUCCESS; 63 } 64 65 void

+8 -3

src/xrt/drivers/rift_s/rift_s_tracker.c

··· 67 //! Specifies whether the user wants to use the hand tracker. 68 DEBUG_GET_ONCE_BOOL_OPTION(rift_s_handtracking, "RIFT_S_HANDTRACKING", true) 69 70 - static void 71 rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev, 72 enum xrt_input_name name, 73 int64_t at_timestamp_ns, ··· 608 math_quat_rotate_vec3(&q, &pose->position, &pose->position); 609 } 610 611 - static void 612 rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev, 613 enum xrt_input_name name, 614 int64_t at_timestamp_ns, 615 struct xrt_space_relation *out_relation) 616 { 617 struct rift_s_tracker *tracker = (struct rift_s_tracker *)(xdev); 618 - assert(name == XRT_INPUT_GENERIC_TRACKER_POSE); 619 620 rift_s_tracker_get_tracked_pose(tracker, RIFT_S_TRACKER_POSE_IMU, at_timestamp_ns, out_relation); 621 } 622 623 void

··· 67 //! Specifies whether the user wants to use the hand tracker. 68 DEBUG_GET_ONCE_BOOL_OPTION(rift_s_handtracking, "RIFT_S_HANDTRACKING", true) 69 70 + static xrt_result_t 71 rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev, 72 enum xrt_input_name name, 73 int64_t at_timestamp_ns, ··· 608 math_quat_rotate_vec3(&q, &pose->position, &pose->position); 609 } 610 611 + static xrt_result_t 612 rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev, 613 enum xrt_input_name name, 614 int64_t at_timestamp_ns, 615 struct xrt_space_relation *out_relation) 616 { 617 struct rift_s_tracker *tracker = (struct rift_s_tracker *)(xdev); 618 + if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { 619 + U_LOG_XDEV_UNSUPPORTED_INPUT(&tracker->base, rift_s_log_level, name); 620 + return XRT_ERROR_INPUT_UNSUPPORTED; 621 + } 622 623 rift_s_tracker_get_tracked_pose(tracker, RIFT_S_TRACKER_POSE_IMU, at_timestamp_ns, out_relation); 624 + 625 + return XRT_SUCCESS; 626 } 627 628 void

+5 -3

src/xrt/drivers/rokid/rokid_hmd.c

··· 430 u_device_free(&rokid->base); 431 } 432 433 - static void 434 rokid_hmd_get_tracked_pose(struct xrt_device *xdev, 435 enum xrt_input_name name, 436 int64_t at_timestamp_ns, ··· 439 struct rokid_hmd *rokid = rokid_hmd(xdev); 440 441 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 442 - ROKID_ERROR(rokid, "unknown input name"); 443 - return; 444 } 445 os_mutex_lock(&rokid->fusion.mutex); 446 rokid_fusion_get_pose(&rokid->fusion, at_timestamp_ns, out_relation); 447 os_mutex_unlock(&rokid->fusion.mutex); 448 } 449 450 static struct xrt_device *

··· 430 u_device_free(&rokid->base); 431 } 432 433 + static xrt_result_t 434 rokid_hmd_get_tracked_pose(struct xrt_device *xdev, 435 enum xrt_input_name name, 436 int64_t at_timestamp_ns, ··· 439 struct rokid_hmd *rokid = rokid_hmd(xdev); 440 441 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 442 + U_LOG_XDEV_UNSUPPORTED_INPUT(&rokid->base, rokid->log_level, name); 443 + return XRT_ERROR_INPUT_UNSUPPORTED; 444 } 445 os_mutex_lock(&rokid->fusion.mutex); 446 rokid_fusion_get_pose(&rokid->fusion, at_timestamp_ns, out_relation); 447 os_mutex_unlock(&rokid->fusion.mutex); 448 + 449 + return XRT_SUCCESS; 450 } 451 452 static struct xrt_device *

+4 -3

src/xrt/drivers/sample/sample_hmd.c

··· 96 return XRT_SUCCESS; 97 } 98 99 - static void 100 sample_hmd_get_tracked_pose(struct xrt_device *xdev, 101 enum xrt_input_name name, 102 int64_t at_timestamp_ns, ··· 105 struct sample_hmd *hmd = sample_hmd(xdev); 106 107 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 108 - HMD_ERROR(hmd, "unknown input name"); 109 - return; 110 } 111 112 struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; ··· 125 } 126 127 *out_relation = relation; 128 } 129 130 static void

··· 96 return XRT_SUCCESS; 97 } 98 99 + static xrt_result_t 100 sample_hmd_get_tracked_pose(struct xrt_device *xdev, 101 enum xrt_input_name name, 102 int64_t at_timestamp_ns, ··· 105 struct sample_hmd *hmd = sample_hmd(xdev); 106 107 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 108 + U_LOG_XDEV_UNSUPPORTED_INPUT(&hmd->base, hmd->log_level, name); 109 + return XRT_ERROR_INPUT_UNSUPPORTED; 110 } 111 112 struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; ··· 125 } 126 127 *out_relation = relation; 128 + return XRT_SUCCESS; 129 } 130 131 static void

+5 -2

src/xrt/drivers/simula/svr_hmd.c

··· 72 u_device_free(&ns->base); 73 } 74 75 - static void 76 svr_hmd_get_tracked_pose(struct xrt_device *xdev, 77 enum xrt_input_name name, 78 int64_t at_timestamp_ns, ··· 82 83 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 84 SVR_ERROR(ns, "unknown input name"); 85 - return; 86 } 87 88 ··· 92 (struct xrt_pose)XRT_POSE_IDENTITY; // This is so that tracking overrides/multi driver just transforms us by 93 // the tracker + offset from the tracker. 94 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; 95 } 96 97 #define DEG_TO_RAD(DEG) (DEG * M_PI / 180.)

··· 72 u_device_free(&ns->base); 73 } 74 75 + static xrt_result_t 76 svr_hmd_get_tracked_pose(struct xrt_device *xdev, 77 enum xrt_input_name name, 78 int64_t at_timestamp_ns, ··· 82 83 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 84 SVR_ERROR(ns, "unknown input name"); 85 + U_LOG_XDEV_UNSUPPORTED_INPUT(&ns->base, ns->log_level, name); 86 + return XRT_ERROR_INPUT_UNSUPPORTED; 87 } 88 89 ··· 93 (struct xrt_pose)XRT_POSE_IDENTITY; // This is so that tracking overrides/multi driver just transforms us by 94 // the tracker + offset from the tracker. 95 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; 96 + 97 + return XRT_SUCCESS; 98 } 99 100 #define DEG_TO_RAD(DEG) (DEG * M_PI / 180.)

+9 -5

src/xrt/drivers/simulated/simulated_controller.c

··· 46 #define CHECK_THAT_NAME_IS_AND_ERROR(NAME) \ 47 do { \ 48 if (sd->base.name != NAME) { \ 49 - U_LOG_E("Unknown input for controller %s 0x%02x", #NAME, name); \ 50 - return; \ 51 } \ 52 } while (false) 53 ··· 118 return XRT_SUCCESS; 119 } 120 121 - static void 122 simulated_device_get_tracked_pose(struct xrt_device *xdev, 123 enum xrt_input_name name, 124 int64_t at_timestamp_ns, ··· 133 case XRT_INPUT_WMR_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_WMR_CONTROLLER); break; 134 case XRT_INPUT_ML2_CONTROLLER_GRIP_POSE: 135 case XRT_INPUT_ML2_CONTROLLER_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_ML2_CONTROLLER); break; 136 - default: U_LOG_E("Unknown input name: 0x%0x", name); return; 137 } 138 139 if (!sd->active) { 140 out_relation->pose = (struct xrt_pose)XRT_POSE_IDENTITY; 141 out_relation->relation_flags = 0; 142 - return; 143 } 144 145 struct xrt_pose pose = sd->center; ··· 160 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 161 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | 162 XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); 163 } 164 165 static void

··· 46 #define CHECK_THAT_NAME_IS_AND_ERROR(NAME) \ 47 do { \ 48 if (sd->base.name != NAME) { \ 49 + U_LOG_XDEV_UNSUPPORTED_INPUT(&sd->base, u_log_get_global_level(), name); \ 50 + return XRT_ERROR_INPUT_UNSUPPORTED; \ 51 } \ 52 } while (false) 53 ··· 118 return XRT_SUCCESS; 119 } 120 121 + static xrt_result_t 122 simulated_device_get_tracked_pose(struct xrt_device *xdev, 123 enum xrt_input_name name, 124 int64_t at_timestamp_ns, ··· 133 case XRT_INPUT_WMR_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_WMR_CONTROLLER); break; 134 case XRT_INPUT_ML2_CONTROLLER_GRIP_POSE: 135 case XRT_INPUT_ML2_CONTROLLER_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_ML2_CONTROLLER); break; 136 + default: 137 + U_LOG_XDEV_UNSUPPORTED_INPUT(&sd->base, u_log_get_global_level(), name); 138 + return XRT_ERROR_INPUT_UNSUPPORTED; 139 } 140 141 if (!sd->active) { 142 out_relation->pose = (struct xrt_pose)XRT_POSE_IDENTITY; 143 out_relation->relation_flags = 0; 144 + return XRT_SUCCESS; 145 } 146 147 struct xrt_pose pose = sd->center; ··· 162 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 163 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | 164 XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); 165 + 166 + return XRT_SUCCESS; 167 } 168 169 static void

+5 -3

src/xrt/drivers/simulated/simulated_hmd.c

··· 85 u_device_free(&dh->base); 86 } 87 88 - static void 89 simulated_hmd_get_tracked_pose(struct xrt_device *xdev, 90 enum xrt_input_name name, 91 int64_t at_timestamp_ns, ··· 94 struct simulated_hmd *hmd = simulated_hmd(xdev); 95 96 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 97 - HMD_ERROR(hmd, "unknown input name"); 98 - return; 99 } 100 101 const double time_s = time_ns_to_s(at_timestamp_ns - hmd->created_ns); ··· 142 out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | 143 XRT_SPACE_RELATION_POSITION_VALID_BIT | 144 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 145 } 146 147 static xrt_result_t

··· 85 u_device_free(&dh->base); 86 } 87 88 + static xrt_result_t 89 simulated_hmd_get_tracked_pose(struct xrt_device *xdev, 90 enum xrt_input_name name, 91 int64_t at_timestamp_ns, ··· 94 struct simulated_hmd *hmd = simulated_hmd(xdev); 95 96 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 97 + U_LOG_XDEV_UNSUPPORTED_INPUT(&hmd->base, hmd->log_level, name); 98 + return XRT_ERROR_INPUT_UNSUPPORTED; 99 } 100 101 const double time_s = time_ns_to_s(at_timestamp_ns - hmd->created_ns); ··· 142 out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | 143 XRT_SPACE_RELATION_POSITION_VALID_BIT | 144 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 145 + 146 + return XRT_SUCCESS; 147 } 148 149 static xrt_result_t

+7 -3

src/xrt/drivers/steamvr_lh/device.cpp

··· 426 return XRT_SUCCESS; 427 } 428 429 - void 430 HmdDevice::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) 431 { 432 switch (name) { 433 case XRT_INPUT_GENERIC_HEAD_POSE: Device::get_pose(at_timestamp_ns, out_relation); break; 434 - default: U_LOG_W("steamvr_lh hmd: Requested pose for unknown name %u", name); break; 435 } 436 } 437 438 - void 439 ControllerDevice::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) 440 { 441 xrt_space_relation rel = {}; ··· 453 struct xrt_pose *p = &out_relation->pose; 454 DEV_DEBUG("controller %u: GET_POSITION (%f %f %f) GET_ORIENTATION (%f, %f, %f, %f)", name, p->position.x, 455 p->position.y, p->position.z, p->orientation.x, p->orientation.y, p->orientation.z, p->orientation.w); 456 } 457 458 void

··· 426 return XRT_SUCCESS; 427 } 428 429 + xrt_result_t 430 HmdDevice::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) 431 { 432 switch (name) { 433 case XRT_INPUT_GENERIC_HEAD_POSE: Device::get_pose(at_timestamp_ns, out_relation); break; 434 + default: U_LOG_XDEV_UNSUPPORTED_INPUT(this, ctx->log_level, name); return XRT_ERROR_INPUT_UNSUPPORTED; 435 } 436 + 437 + return XRT_SUCCESS; 438 } 439 440 + xrt_result_t 441 ControllerDevice::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) 442 { 443 xrt_space_relation rel = {}; ··· 455 struct xrt_pose *p = &out_relation->pose; 456 DEV_DEBUG("controller %u: GET_POSITION (%f %f %f) GET_ORIENTATION (%f, %f, %f, %f)", name, p->position.x, 457 p->position.y, p->position.z, p->orientation.x, p->orientation.y, p->orientation.z, p->orientation.w); 458 + 459 + return XRT_SUCCESS; 460 } 461 462 void

+3 -3

src/xrt/drivers/steamvr_lh/device.hpp

··· 69 handle_properties(const vr::PropertyWrite_t *batch, uint32_t count); 70 71 //! Maps to @ref xrt_device::get_tracked_pose. 72 - virtual void 73 get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) = 0; 74 75 xrt_result_t ··· 116 117 HmdDevice(const DeviceBuilder &builder); 118 119 - void 120 get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) override; 121 122 void ··· 168 void 169 set_haptic_handle(vr::VRInputComponentHandle_t handle); 170 171 - void 172 get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) override; 173 174 IndexFingerInput *

··· 69 handle_properties(const vr::PropertyWrite_t *batch, uint32_t count); 70 71 //! Maps to @ref xrt_device::get_tracked_pose. 72 + virtual xrt_result_t 73 get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) = 0; 74 75 xrt_result_t ··· 116 117 HmdDevice(const DeviceBuilder &builder); 118 119 + xrt_result_t 120 get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) override; 121 122 void ··· 168 void 169 set_haptic_handle(vr::VRInputComponentHandle_t handle); 170 171 + xrt_result_t 172 get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) override; 173 174 IndexFingerInput *

+5 -4

src/xrt/drivers/survive/survive_driver.c

··· 324 return false; 325 } 326 327 - static void 328 survive_device_get_tracked_pose(struct xrt_device *xdev, 329 enum xrt_input_name name, 330 int64_t at_timestamp_ns, ··· 332 { 333 struct survive_device *survive = (struct survive_device *)xdev; 334 if (!verify_device_name(survive, name)) { 335 - SURVIVE_ERROR(survive, "unknown input name"); 336 - return; 337 } 338 339 if (!survive->survive_obj) { 340 // U_LOG_D("Obj not set for %p", (void*)survive); 341 - return; 342 } 343 344 // We're pretty sure libsurvive is giving us the IMU pose here, so this works. ··· 356 struct xrt_pose *p = &out_relation->pose; 357 SURVIVE_TRACE(survive, "GET_POSITION (%f %f %f) GET_ORIENTATION (%f, %f, %f, %f)", p->position.x, p->position.y, 358 p->position.z, p->orientation.x, p->orientation.y, p->orientation.z, p->orientation.w); 359 } 360 361 static xrt_result_t

··· 324 return false; 325 } 326 327 + static xrt_result_t 328 survive_device_get_tracked_pose(struct xrt_device *xdev, 329 enum xrt_input_name name, 330 int64_t at_timestamp_ns, ··· 332 { 333 struct survive_device *survive = (struct survive_device *)xdev; 334 if (!verify_device_name(survive, name)) { 335 + U_LOG_XDEV_UNSUPPORTED_INPUT(&survive->base, survive->sys->log_level, name); 336 + return XRT_ERROR_INPUT_UNSUPPORTED; 337 } 338 339 if (!survive->survive_obj) { 340 // U_LOG_D("Obj not set for %p", (void*)survive); 341 + return XRT_SUCCESS; 342 } 343 344 // We're pretty sure libsurvive is giving us the IMU pose here, so this works. ··· 356 struct xrt_pose *p = &out_relation->pose; 357 SURVIVE_TRACE(survive, "GET_POSITION (%f %f %f) GET_ORIENTATION (%f, %f, %f, %f)", p->position.x, p->position.y, 358 p->position.z, p->orientation.x, p->orientation.y, p->orientation.z, p->orientation.w); 359 + return XRT_SUCCESS; 360 } 361 362 static xrt_result_t

+6 -5

src/xrt/drivers/twrap/twrap_slam.c

··· 84 } 85 #endif 86 87 - static void 88 twrap_slam_get_tracked_pose(struct xrt_device *xdev, 89 enum xrt_input_name name, 90 int64_t at_timestamp_ns, ··· 93 struct slam_device *dx = slam_device(xdev); 94 95 if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { 96 - SLAM_ERROR(dx, "unknown input name %d", name); 97 - return; 98 } 99 #ifdef XRT_FEATURE_SLAM 100 if (!dx->use_3dof) { ··· 109 110 if (!pose_tracked) { 111 U_ZERO(&out_relation->relation_flags); 112 - return; 113 } 114 115 basalt_rel.pose = twrap_hmd_correct_pose_from_basalt(basalt_rel.pose); ··· 127 } 128 129 m_relation_chain_resolve(&xrc, out_relation); 130 - return; 131 } 132 133 #endif 134 m_relation_history_get(dx->dof3->rh, at_timestamp_ns, out_relation); 135 } 136 137 static void

··· 84 } 85 #endif 86 87 + static xrt_result_t 88 twrap_slam_get_tracked_pose(struct xrt_device *xdev, 89 enum xrt_input_name name, 90 int64_t at_timestamp_ns, ··· 93 struct slam_device *dx = slam_device(xdev); 94 95 if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { 96 + U_LOG_XDEV_UNSUPPORTED_INPUT(&dx->base, dx->log_level, name); 97 + return XRT_ERROR_INPUT_UNSUPPORTED; 98 } 99 #ifdef XRT_FEATURE_SLAM 100 if (!dx->use_3dof) { ··· 109 110 if (!pose_tracked) { 111 U_ZERO(&out_relation->relation_flags); 112 + return XRT_SUCCESS; 113 } 114 115 basalt_rel.pose = twrap_hmd_correct_pose_from_basalt(basalt_rel.pose); ··· 127 } 128 129 m_relation_chain_resolve(&xrc, out_relation); 130 + return XRT_SUCCESS; 131 } 132 133 #endif 134 m_relation_history_get(dx->dof3->rh, at_timestamp_ns, out_relation); 135 + return XRT_SUCCESS; 136 } 137 138 static void

+5 -3

src/xrt/drivers/vive/vive_controller.c

··· 410 out_value->is_active = true; 411 } 412 413 - static void 414 vive_controller_device_get_tracked_pose(struct xrt_device *xdev, 415 enum xrt_input_name name, 416 int64_t at_timestamp_ns, ··· 421 // U_LOG_D("input name %d %d", name, XRT_INPUT_VIVE_GRIP_POSE); 422 if (name != XRT_INPUT_VIVE_AIM_POSE && name != XRT_INPUT_VIVE_GRIP_POSE && name != XRT_INPUT_INDEX_AIM_POSE && 423 name != XRT_INPUT_INDEX_GRIP_POSE) { 424 - VIVE_ERROR(d, "unknown input name"); 425 - return; 426 } 427 428 get_pose(d, name, at_timestamp_ns, out_relation); 429 } 430 431 static int

··· 410 out_value->is_active = true; 411 } 412 413 + static xrt_result_t 414 vive_controller_device_get_tracked_pose(struct xrt_device *xdev, 415 enum xrt_input_name name, 416 int64_t at_timestamp_ns, ··· 421 // U_LOG_D("input name %d %d", name, XRT_INPUT_VIVE_GRIP_POSE); 422 if (name != XRT_INPUT_VIVE_AIM_POSE && name != XRT_INPUT_VIVE_GRIP_POSE && name != XRT_INPUT_INDEX_AIM_POSE && 423 name != XRT_INPUT_INDEX_GRIP_POSE) { 424 + U_LOG_XDEV_UNSUPPORTED_INPUT(&d->base, d->log_level, name); 425 + return XRT_ERROR_INPUT_UNSUPPORTED; 426 } 427 428 get_pose(d, name, at_timestamp_ns, out_relation); 429 + 430 + return XRT_SUCCESS; 431 } 432 433 static int

+14 -6

src/xrt/drivers/vive/vive_device.c

··· 106 return XRT_SUCCESS; 107 } 108 109 - static void 110 vive_device_get_3dof_tracked_pose(struct xrt_device *xdev, 111 enum xrt_input_name name, 112 uint64_t at_timestamp_ns, ··· 117 struct vive_device *d = vive_device(xdev); 118 119 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 120 - U_LOG_E("unknown input name"); 121 - return; 122 } 123 124 struct xrt_space_relation relation = {0}; ··· 131 132 *out_relation = relation; 133 d->pose = out_relation->pose; 134 } 135 136 //! Specific pose corrections for Basalt and a Valve Index headset ··· 179 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 180 } 181 182 - static void 183 vive_device_get_tracked_pose(struct xrt_device *xdev, 184 enum xrt_input_name name, 185 int64_t at_timestamp_ns, ··· 192 // Ajdust the timestamp with the offset. 193 at_timestamp_ns += (int64_t)(d->tracked_offset_ms.val * (double)U_TIME_1MS_IN_NS); 194 195 if (d->tracking.slam_enabled && d->slam_over_3dof) { 196 vive_device_get_slam_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 197 } else { 198 - vive_device_get_3dof_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 199 } 200 - math_pose_transform(&d->offset, &out_relation->pose, &out_relation->pose); 201 } 202 203 static void

··· 106 return XRT_SUCCESS; 107 } 108 109 + static xrt_result_t 110 vive_device_get_3dof_tracked_pose(struct xrt_device *xdev, 111 enum xrt_input_name name, 112 uint64_t at_timestamp_ns, ··· 117 struct vive_device *d = vive_device(xdev); 118 119 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 120 + U_LOG_XDEV_UNSUPPORTED_INPUT(&d->base, u_log_get_global_level(), name); 121 + return XRT_ERROR_INPUT_UNSUPPORTED; 122 } 123 124 struct xrt_space_relation relation = {0}; ··· 131 132 *out_relation = relation; 133 d->pose = out_relation->pose; 134 + 135 + return XRT_SUCCESS; 136 } 137 138 //! Specific pose corrections for Basalt and a Valve Index headset ··· 181 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 182 } 183 184 + static xrt_result_t 185 vive_device_get_tracked_pose(struct xrt_device *xdev, 186 enum xrt_input_name name, 187 int64_t at_timestamp_ns, ··· 194 // Ajdust the timestamp with the offset. 195 at_timestamp_ns += (int64_t)(d->tracked_offset_ms.val * (double)U_TIME_1MS_IN_NS); 196 197 + xrt_result_t xret = XRT_SUCCESS; 198 if (d->tracking.slam_enabled && d->slam_over_3dof) { 199 vive_device_get_slam_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 200 } else { 201 + xret = vive_device_get_3dof_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 202 } 203 + 204 + if (xret == XRT_SUCCESS) { 205 + math_pose_transform(&d->offset, &out_relation->pose, &out_relation->pose); 206 + } 207 + 208 + return xret; 209 } 210 211 static void

+4 -2

src/xrt/drivers/wmr/wmr_controller_base.c

··· 443 return true; 444 } 445 446 - static void 447 wmr_controller_base_get_tracked_pose(struct xrt_device *xdev, 448 enum xrt_input_name name, 449 int64_t at_timestamp_ns, ··· 479 // No prediction needed. 480 if (at_timestamp_ns < last_imu_timestamp_ns) { 481 *out_relation = relation; 482 - return; 483 } 484 485 int64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns; 486 double prediction_s = time_ns_to_s(prediction_ns); 487 488 m_predict_relation(&relation, prediction_s, out_relation); 489 } 490 491 void

··· 443 return true; 444 } 445 446 + static xrt_result_t 447 wmr_controller_base_get_tracked_pose(struct xrt_device *xdev, 448 enum xrt_input_name name, 449 int64_t at_timestamp_ns, ··· 479 // No prediction needed. 480 if (at_timestamp_ns < last_imu_timestamp_ns) { 481 *out_relation = relation; 482 + return XRT_SUCCESS; 483 } 484 485 int64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns; 486 double prediction_s = time_ns_to_s(prediction_ns); 487 488 m_predict_relation(&relation, prediction_s, out_relation); 489 + 490 + return XRT_SUCCESS; 491 } 492 493 void

+15 -7

src/xrt/drivers/wmr/wmr_hmd.c

··· 1073 * 1074 */ 1075 1076 - static void 1077 wmr_hmd_get_3dof_tracked_pose(struct xrt_device *xdev, 1078 enum xrt_input_name name, 1079 uint64_t at_timestamp_ns, ··· 1084 struct wmr_hmd *wh = wmr_hmd(xdev); 1085 1086 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 1087 - WMR_ERROR(wh, "Unknown input name"); 1088 - return; 1089 } 1090 1091 // Variables needed for prediction. ··· 1105 // No prediction needed. 1106 if (at_timestamp_ns < last_imu_timestamp_ns) { 1107 *out_relation = relation; 1108 - return; 1109 } 1110 1111 uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns; ··· 1113 1114 m_predict_relation(&relation, prediction_s, out_relation); 1115 wh->pose = out_relation->pose; 1116 } 1117 1118 //! Specific pose corrections for Basalt and a WMR headset ··· 1165 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 1166 } 1167 1168 - static void 1169 wmr_hmd_get_tracked_pose(struct xrt_device *xdev, 1170 enum xrt_input_name name, 1171 int64_t at_timestamp_ns, ··· 1177 1178 at_timestamp_ns += (int64_t)(wh->tracked_offset_ms.val * (double)U_TIME_1MS_IN_NS); 1179 1180 if (wh->tracking.slam_enabled && wh->slam_over_3dof) { 1181 wmr_hmd_get_slam_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 1182 } else { 1183 - wmr_hmd_get_3dof_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 1184 } 1185 - math_pose_transform(&wh->offset, &out_relation->pose, &out_relation->pose); 1186 } 1187 1188 static void

··· 1073 * 1074 */ 1075 1076 + static xrt_result_t 1077 wmr_hmd_get_3dof_tracked_pose(struct xrt_device *xdev, 1078 enum xrt_input_name name, 1079 uint64_t at_timestamp_ns, ··· 1084 struct wmr_hmd *wh = wmr_hmd(xdev); 1085 1086 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 1087 + U_LOG_XDEV_UNSUPPORTED_INPUT(&wh->base, wh->log_level, name); 1088 + return XRT_ERROR_INPUT_UNSUPPORTED; 1089 } 1090 1091 // Variables needed for prediction. ··· 1105 // No prediction needed. 1106 if (at_timestamp_ns < last_imu_timestamp_ns) { 1107 *out_relation = relation; 1108 + return XRT_SUCCESS; 1109 } 1110 1111 uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns; ··· 1113 1114 m_predict_relation(&relation, prediction_s, out_relation); 1115 wh->pose = out_relation->pose; 1116 + 1117 + return XRT_SUCCESS; 1118 } 1119 1120 //! Specific pose corrections for Basalt and a WMR headset ··· 1167 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 1168 } 1169 1170 + static xrt_result_t 1171 wmr_hmd_get_tracked_pose(struct xrt_device *xdev, 1172 enum xrt_input_name name, 1173 int64_t at_timestamp_ns, ··· 1179 1180 at_timestamp_ns += (int64_t)(wh->tracked_offset_ms.val * (double)U_TIME_1MS_IN_NS); 1181 1182 + xrt_result_t xret = XRT_SUCCESS; 1183 if (wh->tracking.slam_enabled && wh->slam_over_3dof) { 1184 wmr_hmd_get_slam_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 1185 } else { 1186 + xret = wmr_hmd_get_3dof_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 1187 + } 1188 + 1189 + if (xret == XRT_SUCCESS) { 1190 + math_pose_transform(&wh->offset, &out_relation->pose, &out_relation->pose); 1191 } 1192 + 1193 + return xret; 1194 } 1195 1196 static void

+4 -3

src/xrt/drivers/xreal_air/xreal_air_hmd.c

··· 1049 return XRT_SUCCESS; 1050 } 1051 1052 - static void 1053 xreal_air_hmd_get_tracked_pose(struct xrt_device *xdev, 1054 enum xrt_input_name name, 1055 int64_t at_timestamp_ns, ··· 1058 struct xreal_air_hmd *hmd = xreal_air_hmd(xdev); 1059 1060 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 1061 - XREAL_AIR_ERROR(hmd, "unknown input name"); 1062 - return; 1063 } 1064 1065 const enum xrt_space_relation_flags flags = (enum xrt_space_relation_flags)( ··· 1076 1077 // Make sure that the orientation is valid. 1078 math_quat_normalize(&out_relation->pose.orientation); 1079 } 1080 1081 static void

··· 1049 return XRT_SUCCESS; 1050 } 1051 1052 + static xrt_result_t 1053 xreal_air_hmd_get_tracked_pose(struct xrt_device *xdev, 1054 enum xrt_input_name name, 1055 int64_t at_timestamp_ns, ··· 1058 struct xreal_air_hmd *hmd = xreal_air_hmd(xdev); 1059 1060 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 1061 + U_LOG_XDEV_UNSUPPORTED_INPUT(&hmd->base, hmd->log_level, name); 1062 + return XRT_ERROR_INPUT_UNSUPPORTED; 1063 } 1064 1065 const enum xrt_space_relation_flags flags = (enum xrt_space_relation_flags)( ··· 1076 1077 // Make sure that the orientation is valid. 1078 math_quat_normalize(&out_relation->pose.orientation); 1079 + return XRT_SUCCESS; 1080 } 1081 1082 static void

+5 -6

src/xrt/include/xrt/xrt_device.h

··· 319 * 320 * @see xrt_input_name 321 */ 322 - void (*get_tracked_pose)(struct xrt_device *xdev, 323 - enum xrt_input_name name, 324 - int64_t at_timestamp_ns, 325 - struct xrt_space_relation *out_relation); 326 327 /*! 328 * @brief Get relationship of hand joints to the tracking origin space as ··· 580 int64_t at_timestamp_ns, 581 struct xrt_space_relation *out_relation) 582 { 583 - xdev->get_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 584 - return XRT_SUCCESS; 585 } 586 587 /*!

··· 319 * 320 * @see xrt_input_name 321 */ 322 + xrt_result_t (*get_tracked_pose)(struct xrt_device *xdev, 323 + enum xrt_input_name name, 324 + int64_t at_timestamp_ns, 325 + struct xrt_space_relation *out_relation); 326 327 /*! 328 * @brief Get relationship of hand joints to the tracking origin space as ··· 580 int64_t at_timestamp_ns, 581 struct xrt_space_relation *out_relation) 582 { 583 + return xdev->get_tracked_pose(xdev, name, at_timestamp_ns, out_relation); 584 } 585 586 /*!

+2 -2

src/xrt/ipc/client/ipc_client_device.c

··· 82 IPC_CHK_ALWAYS_RET(icd->ipc_c, xret, "ipc_call_device_update_input"); 83 } 84 85 - static void 86 ipc_client_device_get_tracked_pose(struct xrt_device *xdev, 87 enum xrt_input_name name, 88 int64_t at_timestamp_ns, ··· 96 name, // 97 at_timestamp_ns, // 98 out_relation); // 99 - IPC_CHK_ONLY_PRINT(icd->ipc_c, xret, "ipc_call_device_get_tracked_pose"); 100 } 101 102 static void

··· 82 IPC_CHK_ALWAYS_RET(icd->ipc_c, xret, "ipc_call_device_update_input"); 83 } 84 85 + static xrt_result_t 86 ipc_client_device_get_tracked_pose(struct xrt_device *xdev, 87 enum xrt_input_name name, 88 int64_t at_timestamp_ns, ··· 96 name, // 97 at_timestamp_ns, // 98 out_relation); // 99 + IPC_CHK_ALWAYS_RET(icd->ipc_c, xret, "ipc_call_device_get_tracked_pose"); 100 } 101 102 static void

+2 -2

src/xrt/ipc/client/ipc_client_hmd.c

··· 149 IPC_CHK_ALWAYS_RET(ich->ipc_c, xret, "ipc_call_device_update_input"); 150 } 151 152 - static void 153 ipc_client_hmd_get_tracked_pose(struct xrt_device *xdev, 154 enum xrt_input_name name, 155 int64_t at_timestamp_ns, ··· 164 name, // 165 at_timestamp_ns, // 166 out_relation); // 167 - IPC_CHK_ONLY_PRINT(ich->ipc_c, xret, "ipc_call_device_get_tracked_pose"); 168 } 169 170 static void

··· 149 IPC_CHK_ALWAYS_RET(ich->ipc_c, xret, "ipc_call_device_update_input"); 150 } 151 152 + static xrt_result_t 153 ipc_client_hmd_get_tracked_pose(struct xrt_device *xdev, 154 enum xrt_input_name name, 155 int64_t at_timestamp_ns, ··· 164 name, // 165 at_timestamp_ns, // 166 out_relation); // 167 + IPC_CHK_ALWAYS_RET(ich->ipc_c, xret, "ipc_call_device_get_tracked_pose"); 168 } 169 170 static void

+5 -3

src/xrt/targets/sdl_test/sdl_device.c

··· 14 #include "util/u_distortion_mesh.h" 15 16 17 - static void 18 sdl_hmd_get_tracked_pose(struct xrt_device *xdev, 19 enum xrt_input_name name, 20 int64_t at_timestamp_ns, ··· 23 struct sdl_program *sp = from_xdev(xdev); 24 25 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 26 - U_LOG_E("Unknown input name"); 27 - return; 28 } 29 30 struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; ··· 37 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT; // 38 39 *out_relation = relation; 40 } 41 42 static void

··· 14 #include "util/u_distortion_mesh.h" 15 16 17 + static xrt_result_t 18 sdl_hmd_get_tracked_pose(struct xrt_device *xdev, 19 enum xrt_input_name name, 20 int64_t at_timestamp_ns, ··· 23 struct sdl_program *sp = from_xdev(xdev); 24 25 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 26 + U_LOG_XDEV_UNSUPPORTED_INPUT(&sp->xdev_base, u_log_get_global_level(), name); 27 + return XRT_ERROR_INPUT_UNSUPPORTED; 28 } 29 30 struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; ··· 37 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT; // 38 39 *out_relation = relation; 40 + 41 + return XRT_SUCCESS; 42 } 43 44 static void