From e37d8f4cf5f9e5aaae2ef0b1799baf3798606a9a Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Thu, 16 Apr 2020 13:23:12 +0100 Subject: [PATCH] xrt: Remove timestate from xrt_device interface --- doc/changes/xrt/mr.280.md | 4 + src/xrt/auxiliary/tracking/t_tracker_psmv.cpp | 4 +- src/xrt/auxiliary/tracking/t_tracker_psvr.cpp | 4 +- src/xrt/drivers/arduino/arduino_device.c | 16 +-- src/xrt/drivers/daydream/daydream_device.c | 17 ++-- src/xrt/drivers/dummy/dummy_hmd.c | 12 ++- src/xrt/drivers/hdk/hdk_device.cpp | 17 ++-- src/xrt/drivers/hydra/hydra_driver.c | 22 +++-- src/xrt/drivers/north_star/ns_hmd.c | 19 ++-- src/xrt/drivers/ohmd/oh_device.c | 20 ++-- src/xrt/drivers/psmv/psmv_driver.c | 33 +++---- src/xrt/drivers/psvr/psvr_device.c | 31 ++---- src/xrt/drivers/realsense/rs_6dof.c | 12 ++- src/xrt/drivers/vive/vive_device.c | 16 +-- src/xrt/include/xrt/xrt_device.h | 34 +++---- src/xrt/include/xrt/xrt_tracking.h | 8 +- src/xrt/state_trackers/gui/gui_prober.c | 2 +- src/xrt/state_trackers/oxr/oxr_input.c | 9 +- src/xrt/state_trackers/oxr/oxr_objects.h | 24 ++++- src/xrt/state_trackers/oxr/oxr_session.c | 36 +++---- src/xrt/state_trackers/oxr/oxr_space.c | 8 +- src/xrt/state_trackers/oxr/oxr_xdev.c | 97 ++++++++++++++----- 22 files changed, 243 insertions(+), 202 deletions(-) create mode 100644 doc/changes/xrt/mr.280.md diff --git a/doc/changes/xrt/mr.280.md b/doc/changes/xrt/mr.280.md new file mode 100644 index 000000000..60ae82afd --- /dev/null +++ b/doc/changes/xrt/mr.280.md @@ -0,0 +1,4 @@ +Remove the `struct timestate` argument from the `struct xrt_device` interface. +It should be easy to write a driver and the state tracker should be the one +that tracks this state. It was mostly triggered by the out of process +compositor work. diff --git a/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp b/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp index e46021132..417f4b1a1 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp @@ -370,7 +370,6 @@ run(TrackerPSMV &t) static void get_pose(TrackerPSMV &t, enum xrt_input_name name, - struct time_state *timestate, timepoint_ns when_ns, struct xrt_space_relation *out_relation) { @@ -462,12 +461,11 @@ t_psmv_push_imu(struct xrt_tracked_psmv *xtmv, extern "C" void t_psmv_get_tracked_pose(struct xrt_tracked_psmv *xtmv, enum xrt_input_name name, - struct time_state *timestate, timepoint_ns when_ns, struct xrt_space_relation *out_relation) { auto &t = *container_of(xtmv, TrackerPSMV, base); - get_pose(t, name, timestate, when_ns, out_relation); + get_pose(t, name, when_ns, out_relation); } extern "C" void diff --git a/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp b/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp index 0d6e37bee..87e2e0962 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp @@ -100,7 +100,6 @@ run(TrackerPSVR &t) static void get_pose(TrackerPSVR &t, - struct time_state *timestate, timepoint_ns when_ns, struct xrt_space_relation *out_relation) { @@ -192,12 +191,11 @@ t_psvr_push_imu(struct xrt_tracked_psvr *xtvr, extern "C" void t_psvr_get_tracked_pose(struct xrt_tracked_psvr *xtvr, - struct time_state *timestate, timepoint_ns when_ns, struct xrt_space_relation *out_relation) { auto &t = *container_of(xtvr, TrackerPSVR, base); - get_pose(t, timestate, when_ns, out_relation); + get_pose(t, when_ns, out_relation); } extern "C" void diff --git a/src/xrt/drivers/arduino/arduino_device.c b/src/xrt/drivers/arduino/arduino_device.c index 42d82a8e4..e7cf89d78 100644 --- a/src/xrt/drivers/arduino/arduino_device.c +++ b/src/xrt/drivers/arduino/arduino_device.c @@ -296,7 +296,6 @@ arduino_run_thread(void *ptr) static void arduino_get_fusion_pose(struct arduino_device *ad, enum xrt_input_name name, - timepoint_ns when, struct xrt_space_relation *out_relation) { out_relation->pose.orientation = ad->fusion.rot; @@ -331,12 +330,11 @@ arduino_device_destroy(struct xrt_device *xdev) } static void -arduino_device_update_inputs(struct xrt_device *xdev, - struct time_state *timekeeping) +arduino_device_update_inputs(struct xrt_device *xdev) { struct arduino_device *ad = arduino_device(xdev); - int64_t now = time_state_get_now(timekeeping); + uint64_t now = os_monotonic_get_ns(); // Lock the data. os_mutex_lock(&ad->lock); @@ -357,15 +355,17 @@ arduino_device_update_inputs(struct xrt_device *xdev, static void arduino_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct arduino_device *ad = arduino_device(xdev); - timepoint_ns now = time_state_get_now(timekeeping); + uint64_t now = os_monotonic_get_ns(); - arduino_get_fusion_pose(ad, name, now, out_relation); + (void)at_timestamp_ns; + arduino_get_fusion_pose(ad, name, out_relation); + *out_relation_timestamp_ns = now; } diff --git a/src/xrt/drivers/daydream/daydream_device.c b/src/xrt/drivers/daydream/daydream_device.c index 2c2e4f2c9..3000f2f7d 100644 --- a/src/xrt/drivers/daydream/daydream_device.c +++ b/src/xrt/drivers/daydream/daydream_device.c @@ -252,7 +252,6 @@ daydream_get_calibration(struct daydream_device *daydream) static void daydream_get_fusion_pose(struct daydream_device *daydream, enum xrt_input_name name, - timepoint_ns when, struct xrt_space_relation *out_relation) { out_relation->pose.orientation = daydream->fusion.rot; @@ -287,12 +286,11 @@ daydream_device_destroy(struct xrt_device *xdev) } static void -daydream_device_update_inputs(struct xrt_device *xdev, - struct time_state *timekeeping) +daydream_device_update_inputs(struct xrt_device *xdev) { struct daydream_device *daydream = daydream_device(xdev); - int64_t now = time_state_get_now(timekeeping); + uint64_t now = os_monotonic_get_ns(); // Lock the data. os_mutex_lock(&daydream->lock); @@ -318,15 +316,16 @@ daydream_device_update_inputs(struct xrt_device *xdev, static void daydream_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct daydream_device *daydream = daydream_device(xdev); + uint64_t now = os_monotonic_get_ns(); - timepoint_ns now = time_state_get_now(timekeeping); - - daydream_get_fusion_pose(daydream, name, now, out_relation); + (void)at_timestamp_ns; + daydream_get_fusion_pose(daydream, name, out_relation); + *out_relation_timestamp_ns = now; } diff --git a/src/xrt/drivers/dummy/dummy_hmd.c b/src/xrt/drivers/dummy/dummy_hmd.c index 00b81c834..63ca09eff 100644 --- a/src/xrt/drivers/dummy/dummy_hmd.c +++ b/src/xrt/drivers/dummy/dummy_hmd.c @@ -15,6 +15,7 @@ #include #include +#include "os/os_time.h" #include "math/m_api.h" #include "xrt/xrt_device.h" #include "util/u_var.h" @@ -24,6 +25,7 @@ #include "util/u_time.h" #include "util/u_distortion_mesh.h" + /* * * Structs and defines. @@ -93,7 +95,7 @@ dummy_hmd_destroy(struct xrt_device *xdev) } static void -dummy_hmd_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) +dummy_hmd_update_inputs(struct xrt_device *xdev) { // Empty } @@ -101,8 +103,8 @@ dummy_hmd_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) static void dummy_hmd_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct dummy_hmd *dh = dummy_hmd(xdev); @@ -112,9 +114,9 @@ dummy_hmd_get_tracked_pose(struct xrt_device *xdev, return; } - int64_t now = time_state_get_now(timekeeping); + uint64_t now = os_monotonic_get_ns(); - *out_timestamp = now; + *out_relation_timestamp_ns = now; out_relation->pose = dh->pose; out_relation->relation_flags = (enum xrt_space_relation_flags)( XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | diff --git a/src/xrt/drivers/hdk/hdk_device.cpp b/src/xrt/drivers/hdk/hdk_device.cpp index 56b01d6d3..212cb1a51 100644 --- a/src/xrt/drivers/hdk/hdk_device.cpp +++ b/src/xrt/drivers/hdk/hdk_device.cpp @@ -23,12 +23,16 @@ #include #include "xrt/xrt_device.h" + +#include "os/os_hid.h" +#include "os/os_time.h" + #include "math/m_api.h" + #include "util/u_debug.h" #include "util/u_misc.h" #include "util/u_device.h" #include "util/u_time.h" -#include "os/os_hid.h" #include "hdk_device.h" @@ -101,8 +105,7 @@ hdk_device_destroy(struct xrt_device *xdev) } static void -hdk_device_update_inputs(struct xrt_device *xdev, - struct time_state *timekeeping) +hdk_device_update_inputs(struct xrt_device *xdev) { // Empty } @@ -201,8 +204,8 @@ hdk_device_update(struct hdk_device *hd) static void hdk_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t requested_timestamp_ns, + uint64_t *out_actual_timestamp_ns, struct xrt_space_relation *out_relation) { struct hdk_device *hd = hdk_device(xdev); @@ -212,11 +215,11 @@ hdk_device_get_tracked_pose(struct xrt_device *xdev, return; } - int64_t now = time_state_get_now(timekeeping); + uint64_t now = os_monotonic_get_ns(); // Adjusting for latency - 14ms, found empirically. now -= 14000000; - *out_timestamp = now; + *out_actual_timestamp_ns = now; if (!hd->quat_valid) { out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; diff --git a/src/xrt/drivers/hydra/hydra_driver.c b/src/xrt/drivers/hydra/hydra_driver.c index 99b669750..475ac346b 100644 --- a/src/xrt/drivers/hydra/hydra_driver.c +++ b/src/xrt/drivers/hydra/hydra_driver.c @@ -19,12 +19,14 @@ #include "xrt/xrt_prober.h" +#include "os/os_hid.h" +#include "os/os_time.h" + #include "math/m_api.h" #include "util/u_debug.h" #include "util/u_device.h" #include "util/u_misc.h" #include "util/u_time.h" -#include "os/os_hid.h" #include "hydra_interface.h" @@ -32,6 +34,7 @@ #include #endif + /* * * Defines & structs. @@ -430,10 +433,10 @@ hydra_system_enter_motion_control(struct hydra_system *hs, timepoint_ns now) * */ static int -hydra_system_update(struct hydra_system *hs, struct time_state *timekeeping) +hydra_system_update(struct hydra_system *hs) { assert(hs); - timepoint_ns now = time_state_get_now(timekeeping); + timepoint_ns now = os_monotonic_get_ns(); // In all states of the state machine: // Try reading a report: will only return >0 if we get a full motion @@ -490,13 +493,12 @@ hydra_device_update_input_click(struct hydra_device *hd, */ static void -hydra_device_update_inputs(struct xrt_device *xdev, - struct time_state *timekeeping) +hydra_device_update_inputs(struct xrt_device *xdev) { struct hydra_device *hd = hydra_device(xdev); struct hydra_system *hs = hydra_system(xdev->tracking_origin); - hydra_system_update(hs, timekeeping); + hydra_system_update(hs); if (hd->input_time != hs->report_time) { timepoint_ns now = hs->report_time; @@ -538,16 +540,16 @@ hydra_device_update_inputs(struct xrt_device *xdev, static void hydra_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct hydra_device *hd = hydra_device(xdev); struct hydra_system *hs = hydra_system(xdev->tracking_origin); - hydra_system_update(hs, timekeeping); + hydra_system_update(hs); - *out_timestamp = hs->report_time; + *out_relation_timestamp_ns = hs->report_time; out_relation->pose = hd->state.pose; //! @todo how do we report this is not (necessarily) the same base space diff --git a/src/xrt/drivers/north_star/ns_hmd.c b/src/xrt/drivers/north_star/ns_hmd.c index 121f00d09..8d865d98d 100644 --- a/src/xrt/drivers/north_star/ns_hmd.c +++ b/src/xrt/drivers/north_star/ns_hmd.c @@ -16,6 +16,8 @@ #include #include +#include "os/os_time.h" + #include "ns_hmd.h" #include "util/u_var.h" @@ -48,20 +50,20 @@ ns_hmd_destroy(struct xrt_device *xdev) } static void -ns_hmd_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) +ns_hmd_update_inputs(struct xrt_device *xdev) { struct ns_hmd *ns = ns_hmd(xdev); if (ns->tracker != NULL) { - ns->tracker->update_inputs(ns->tracker, timekeeping); + xrt_device_update_inputs(ns->tracker); } } static void ns_hmd_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct ns_hmd *ns = ns_hmd(xdev); @@ -69,8 +71,9 @@ ns_hmd_get_tracked_pose(struct xrt_device *xdev, // If the tracking device is created use it. if (ns->tracker != NULL) { - ns->tracker->get_tracked_pose(ns->tracker, name, timekeeping, - out_timestamp, out_relation); + xrt_device_get_tracked_pose(ns->tracker, name, at_timestamp_ns, + out_relation_timestamp_ns, + out_relation); return; } @@ -79,9 +82,9 @@ ns_hmd_get_tracked_pose(struct xrt_device *xdev, return; } - int64_t now = time_state_get_now(timekeeping); + uint64_t now = os_monotonic_get_ns(); - *out_timestamp = now; + *out_relation_timestamp_ns = now; out_relation->pose = ns->pose; out_relation->relation_flags = (enum xrt_space_relation_flags)( XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | diff --git a/src/xrt/drivers/ohmd/oh_device.c b/src/xrt/drivers/ohmd/oh_device.c index 43577907e..7d4529231 100644 --- a/src/xrt/drivers/ohmd/oh_device.c +++ b/src/xrt/drivers/ohmd/oh_device.c @@ -15,6 +15,8 @@ #include #include +#include "os/os_time.h" + #include "openhmd.h" #include "math/m_api.h" @@ -52,7 +54,7 @@ oh_device_destroy(struct xrt_device *xdev) } static void -oh_device_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) +oh_device_update_inputs(struct xrt_device *xdev) { // Empty } @@ -60,8 +62,8 @@ oh_device_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) static void oh_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct oh_device *ohd = oh_device(xdev); @@ -74,9 +76,10 @@ oh_device_get_tracked_pose(struct xrt_device *xdev, } ohmd_ctx_update(ohd->ctx); - int64_t now = time_state_get_now(timekeeping); + uint64_t now = os_monotonic_get_ns(); + //! @todo adjust for latency here - *out_timestamp = now; + *out_relation_timestamp_ns = now; ohmd_device_getf(ohd->dev, OHMD_ROTATION_QUAT, &quat.x); ohmd_device_getf(ohd->dev, OHMD_POSITION_VECTOR, &pos.x); out_relation->pose.orientation = quat; @@ -115,7 +118,7 @@ oh_device_get_tracked_pose(struct xrt_device *xdev, /*! @todo this is a hack - should really get a timestamp on the * USB data and use that instead. */ - *out_timestamp = ohd->last_update; + *out_relation_timestamp_ns = ohd->last_update; *out_relation = ohd->last_relation; OH_SPEW(ohd, "GET_TRACKED_POSE - no new data"); return; @@ -127,7 +130,8 @@ oh_device_get_tracked_pose(struct xrt_device *xdev, */ if (ohd->enable_finite_difference && !have_ang_vel) { // No angular velocity - float dt = time_ns_to_s(*out_timestamp - ohd->last_update); + float dt = + time_ns_to_s(*out_relation_timestamp_ns - ohd->last_update); if (ohd->last_update == 0) { // This is the first report, so just print a warning // instead of estimating ang vel. @@ -160,7 +164,7 @@ oh_device_get_tracked_pose(struct xrt_device *xdev, } // Update state within driver - ohd->last_update = *out_timestamp; + ohd->last_update = *out_relation_timestamp_ns; ohd->last_relation = *out_relation; } diff --git a/src/xrt/drivers/psmv/psmv_driver.c b/src/xrt/drivers/psmv/psmv_driver.c index 43c0633d6..5340e51f3 100644 --- a/src/xrt/drivers/psmv/psmv_driver.c +++ b/src/xrt/drivers/psmv/psmv_driver.c @@ -13,6 +13,7 @@ #include "os/os_threading.h" #include "os/os_hid.h" +#include "os/os_time.h" #include "math/m_api.h" #include "math/m_imu_pre.h" @@ -678,8 +679,6 @@ static void * psmv_run_thread(void *ptr) { struct psmv_device *psmv = (struct psmv_device *)ptr; - //! @todo this should be injected at construction time - struct time_state *time = time_state_create(); union { uint8_t buffer[256]; @@ -694,16 +693,14 @@ psmv_run_thread(void *ptr) // Now wait for a package to sync up, it's discarded but that's okay. if (!psmv_read_one_packet(psmv, data.buffer, sizeof(data))) { - // Does null checking and sets to null. - time_state_destroy(&time); return NULL; } - timepoint_ns then_ns = time_state_get_now(time); + timepoint_ns then_ns = os_monotonic_get_ns(); while (psmv_read_one_packet(psmv, data.buffer, sizeof(data))) { - timepoint_ns now_ns = time_state_get_now(time); + timepoint_ns now_ns = os_monotonic_get_ns(); int num = psmv_parse_input(psmv, data.buffer, &input); @@ -735,9 +732,6 @@ psmv_run_thread(void *ptr) os_mutex_unlock(&psmv->lock); } - // Does null checking and sets to null. - time_state_destroy(&time); - return NULL; } @@ -836,12 +830,11 @@ psmv_device_destroy(struct xrt_device *xdev) } static void -psmv_device_update_inputs(struct xrt_device *xdev, - struct time_state *timekeeping) +psmv_device_update_inputs(struct xrt_device *xdev) { struct psmv_device *psmv = psmv_device(xdev); - int64_t now = time_state_get_now(timekeeping); + int64_t now = os_monotonic_get_ns(); psmv_led_and_trigger_update(psmv, now); @@ -870,29 +863,29 @@ psmv_device_update_inputs(struct xrt_device *xdev, static void psmv_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct psmv_device *psmv = psmv_device(xdev); - timepoint_ns now = time_state_get_now(timekeeping); //! @todo transform pose based on input. // We have no tracking, don't return a position. if (psmv->ball != NULL) { - timepoint_ns when_ns = now; - xrt_tracked_psmv_get_tracked_pose(psmv->ball, name, timekeeping, - when_ns, out_relation); + xrt_tracked_psmv_get_tracked_pose( + psmv->ball, name, at_timestamp_ns, out_relation); + *out_relation_timestamp_ns = at_timestamp_ns; } else { + uint64_t now = os_monotonic_get_ns(); psmv_get_fusion_pose(psmv, name, now, out_relation); + *out_relation_timestamp_ns = now; } } static void psmv_device_set_output(struct xrt_device *xdev, enum xrt_output_name name, - struct time_state *timekeeping, union xrt_output_value *value) { struct psmv_device *psmv = psmv_device(xdev); @@ -905,7 +898,7 @@ psmv_device_set_output(struct xrt_device *xdev, psmv_clamp_zero_to_one_float_to_u8(value->vibration.amplitude); // Resend if the rumble has been changed. - int64_t now = time_state_get_now(timekeeping); + int64_t now = os_monotonic_get_ns(); psmv_led_and_trigger_update(psmv, now); } diff --git a/src/xrt/drivers/psvr/psvr_device.c b/src/xrt/drivers/psvr/psvr_device.c index d0bbf575a..9bfe1efcf 100644 --- a/src/xrt/drivers/psvr/psvr_device.c +++ b/src/xrt/drivers/psvr/psvr_device.c @@ -59,7 +59,6 @@ struct psvr_device struct xrt_tracked_psvr *tracker; - struct time_state *timekeeping; timepoint_ns last_sensor_time; struct psvr_parsed_sensor last; @@ -356,7 +355,7 @@ handle_tracker_sensor_msg(struct psvr_device *psvr, unsigned char *buffer, int size) { - timepoint_ns now = time_state_get_now_and_update(psvr->timekeeping); + timepoint_ns now = os_monotonic_get_ns(); uint32_t last_sample_tick = psvr->last.samples[1].tick; if (!psvr_parse_sensor_packet(&psvr->last, buffer, size)) { @@ -893,14 +892,6 @@ teardown(struct psvr_device *psvr) hid_close(psvr->hmd_handle); psvr->hmd_handle = NULL; } - - /* - * This needs to happen last because when waiting for - * device control changes we can get IMU update packets. - * - * Does null checking and setting of null. - */ - time_state_destroy(&psvr->timekeeping); } @@ -911,8 +902,7 @@ teardown(struct psvr_device *psvr) */ static void -psvr_device_update_inputs(struct xrt_device *xdev, - struct time_state *timekeeping) +psvr_device_update_inputs(struct xrt_device *xdev) { struct psvr_device *psvr = psvr_device(xdev); @@ -923,8 +913,8 @@ psvr_device_update_inputs(struct xrt_device *xdev, static void psvr_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct psvr_device *psvr = psvr_device(xdev); @@ -941,9 +931,6 @@ psvr_device_get_tracked_pose(struct xrt_device *xdev, // Clear out the relation. U_ZERO(out_relation); - int64_t when = time_state_get_now(timekeeping); - *out_timestamp = when; - // We have no tracking, don't return a position. if (psvr->tracker == NULL) { #if 0 @@ -958,9 +945,12 @@ psvr_device_get_tracked_pose(struct xrt_device *xdev, out_relation->relation_flags = (enum xrt_space_relation_flags)( XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + + *out_relation_timestamp_ns = os_monotonic_get_ns(); } else { - psvr->tracker->get_tracked_pose(psvr->tracker, timekeeping, - when, out_relation); + xrt_tracked_psvr_get_tracked_pose( + psvr->tracker, at_timestamp_ns, out_relation); + *out_relation_timestamp_ns = at_timestamp_ns; } } @@ -1049,9 +1039,6 @@ psvr_device_create(struct hid_device_info *hmd_handle_info, u_distortion_mesh_from_panotools(&vals, &vals, psvr->base.hmd); } - //! @todo inject this, don't create it - psvr->timekeeping = time_state_create(); - #if 0 psvr->fusion = imu_fusion_create(); #else diff --git a/src/xrt/drivers/realsense/rs_6dof.c b/src/xrt/drivers/realsense/rs_6dof.c index d19c70d8c..214da2f05 100644 --- a/src/xrt/drivers/realsense/rs_6dof.c +++ b/src/xrt/drivers/realsense/rs_6dof.c @@ -12,6 +12,8 @@ #include "xrt/xrt_defines.h" #include "xrt/xrt_device.h" +#include "os/os_time.h" + #include "util/u_time.h" #include "util/u_device.h" @@ -203,7 +205,7 @@ update(struct rs_6dof *rs, struct xrt_pose *out_pose) } static void -rs_6dof_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) +rs_6dof_update_inputs(struct xrt_device *xdev) { // Empty } @@ -211,8 +213,8 @@ rs_6dof_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) static void rs_6dof_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct rs_6dof *rs = rs_6dof(xdev); @@ -222,8 +224,8 @@ rs_6dof_get_tracked_pose(struct xrt_device *xdev, return; } - int64_t now = time_state_get_now(timekeeping); - *out_timestamp = now; + uint64_t now = os_monotonic_get_ns(); + *out_relation_timestamp_ns = now; update(rs, &rs->pose); diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index beacdff7d..2f4386612 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -22,11 +22,12 @@ #include "math/m_api.h" #include "os/os_hid.h" - +#include "os/os_time.h" #include "vive_device.h" #include "vive_protocol.h" + #define VIVE_CLOCK_FREQ 48e6 // 48 MHz DEBUG_GET_ONCE_BOOL_OPTION(vive_spew, "VIVE_PRINT_SPEW", false) @@ -77,8 +78,7 @@ vive_device_destroy(struct xrt_device *xdev) } static void -vive_device_update_inputs(struct xrt_device *xdev, - struct time_state *timekeeping) +vive_device_update_inputs(struct xrt_device *xdev) { struct vive_device *d = vive_device(xdev); VIVE_SPEW(d, "ENTER!"); @@ -87,8 +87,8 @@ vive_device_update_inputs(struct xrt_device *xdev, static void vive_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct vive_device *d = vive_device(xdev); @@ -101,8 +101,10 @@ vive_device_get_tracked_pose(struct xrt_device *xdev, // Clear out the relation. U_ZERO(out_relation); - int64_t when = time_state_get_now(timekeeping); - *out_timestamp = when; + //! @todo Use this properly. + (void)at_timestamp_ns; + uint64_t when = os_monotonic_get_ns(); + *out_relation_timestamp_ns = when; os_thread_helper_lock(&d->sensors_thread); diff --git a/src/xrt/include/xrt/xrt_device.h b/src/xrt/include/xrt/xrt_device.h index 8c7ac4ba5..9a3351868 100644 --- a/src/xrt/include/xrt/xrt_device.h +++ b/src/xrt/include/xrt/xrt_device.h @@ -17,7 +17,6 @@ extern "C" { #endif -struct time_state; struct xrt_tracking; @@ -231,8 +230,7 @@ struct xrt_device * @param[in] xdev The device. * @param[in] timekeeping Shared time synchronization struct. */ - void (*update_inputs)(struct xrt_device *xdev, - struct time_state *timekeeping); + void (*update_inputs)(struct xrt_device *xdev); /*! * Get relationship of a tracked device to the device "base space". @@ -245,16 +243,19 @@ struct xrt_device * @param[in] name Some devices may have multiple poses on * them, select the one using this field. For * HMDs use @p XRT_INPUT_GENERIC_HEAD_POSE. - * @param[in] timekeeping Shared time synchronization struct. - * @param[out] out_timestamp Timestamp when this relation was captured. - * @param[out] out_relation The relation read from the device. + * @param[in] at_timestamp_ns If the device can predict or has a history + * of positions, this is when the caller + * wants the pose to be from. + * @param[out] out_relation_timestamp_ns Timestamp when this relation + * was captured. + * @param[out] out_relation The relation read from the device. * * @see xrt_input_name */ void (*get_tracked_pose)(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t at_timestamp_ns, + uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation); /*! @@ -264,7 +265,6 @@ struct xrt_device */ void (*set_output)(struct xrt_device *xdev, enum xrt_output_name name, - struct time_state *timekeeping, union xrt_output_value *value); /*! @@ -295,10 +295,9 @@ struct xrt_device * Helper function for @ref xrt_device::update_inputs. */ static inline void -xrt_device_update_inputs(struct xrt_device *xdev, - struct time_state *timekeeping) +xrt_device_update_inputs(struct xrt_device *xdev) { - xdev->update_inputs(xdev, timekeeping); + xdev->update_inputs(xdev); } /*! @@ -307,12 +306,12 @@ xrt_device_update_inputs(struct xrt_device *xdev, static inline void xrt_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, - struct time_state *timekeeping, - int64_t *out_timestamp, + uint64_t requested_timestamp_ns, + uint64_t *out_actual_timestamp_ns, struct xrt_space_relation *out_relation) { - xdev->get_tracked_pose(xdev, name, timekeeping, out_timestamp, - out_relation); + xdev->get_tracked_pose(xdev, name, requested_timestamp_ns, + out_actual_timestamp_ns, out_relation); } /*! @@ -321,10 +320,9 @@ xrt_device_get_tracked_pose(struct xrt_device *xdev, static inline void xrt_device_set_output(struct xrt_device *xdev, enum xrt_output_name name, - struct time_state *timekeeping, union xrt_output_value *value) { - xdev->set_output(xdev, name, timekeeping, value); + xdev->set_output(xdev, name, value); } /*! diff --git a/src/xrt/include/xrt/xrt_tracking.h b/src/xrt/include/xrt/xrt_tracking.h index cd05e62be..36f5952fe 100644 --- a/src/xrt/include/xrt/xrt_tracking.h +++ b/src/xrt/include/xrt/xrt_tracking.h @@ -130,7 +130,6 @@ struct xrt_tracked_psmv */ void (*get_tracked_pose)(struct xrt_tracked_psmv *, enum xrt_input_name name, - struct time_state *timekeeper, timepoint_ns when_ns, struct xrt_space_relation *out_relation); @@ -165,7 +164,6 @@ struct xrt_tracked_psvr * the psvr in the tracking space at the given time. */ void (*get_tracked_pose)(struct xrt_tracked_psvr *, - struct time_state *timekeeper, timepoint_ns when_ns, struct xrt_space_relation *out_relation); @@ -185,11 +183,10 @@ struct xrt_tracked_psvr static inline void xrt_tracked_psmv_get_tracked_pose(struct xrt_tracked_psmv *psmv, enum xrt_input_name name, - struct time_state *timekeeper, timepoint_ns when_ns, struct xrt_space_relation *out_relation) { - psmv->get_tracked_pose(psmv, name, timekeeper, when_ns, out_relation); + psmv->get_tracked_pose(psmv, name, when_ns, out_relation); } static inline void @@ -214,11 +211,10 @@ xrt_tracked_psmv_destroy(struct xrt_tracked_psmv **xtmv_ptr) static inline void xrt_tracked_psvr_get_tracked_pose(struct xrt_tracked_psvr *psvr, - struct time_state *timekeeper, timepoint_ns when_ns, struct xrt_space_relation *out_relation) { - psvr->get_tracked_pose(psvr, timekeeper, when_ns, out_relation); + psvr->get_tracked_pose(psvr, when_ns, out_relation); } static inline void diff --git a/src/xrt/state_trackers/gui/gui_prober.c b/src/xrt/state_trackers/gui/gui_prober.c index c1d543365..4620c4080 100644 --- a/src/xrt/state_trackers/gui/gui_prober.c +++ b/src/xrt/state_trackers/gui/gui_prober.c @@ -82,7 +82,7 @@ gui_prober_update(struct gui_program *p) continue; } - p->xdevs[i]->update_inputs(p->xdevs[i], p->timekeeping); + p->xdevs[i]->update_inputs(p->xdevs[i]); } } diff --git a/src/xrt/state_trackers/oxr/oxr_input.c b/src/xrt/state_trackers/oxr/oxr_input.c index ea3a4822b..0f0502483 100644 --- a/src/xrt/state_trackers/oxr/oxr_input.c +++ b/src/xrt/state_trackers/oxr/oxr_input.c @@ -564,8 +564,7 @@ oxr_source_cache_stop_output(struct oxr_logger *log, struct oxr_source_output *output = &cache->outputs[i]; struct xrt_device *xdev = output->xdev; - xdev->set_output(xdev, output->name, - sess->sys->inst->timekeeping, &value); + xrt_device_set_output(xdev, output->name, &value); } } @@ -950,8 +949,7 @@ oxr_action_sync_data(struct oxr_logger *log, // Loop over all xdev devices. for (size_t i = 0; i < sess->sys->num_xdevs; i++) { - oxr_xdev_update(sess->sys->xdevs[i], - sess->sys->inst->timekeeping); + oxr_xdev_update(sess->sys->xdevs[i]); } // Reset all requested source sets. @@ -1196,8 +1194,7 @@ set_source_output_vibration(struct oxr_session *sess, struct oxr_source_output *output = &cache->outputs[i]; struct xrt_device *xdev = output->xdev; - xdev->set_output(xdev, output->name, - sess->sys->inst->timekeeping, &value); + xrt_device_set_output(xdev, output->name, &value); } } diff --git a/src/xrt/state_trackers/oxr/oxr_objects.h b/src/xrt/state_trackers/oxr/oxr_objects.h index dee57a648..014eee313 100644 --- a/src/xrt/state_trackers/oxr/oxr_objects.h +++ b/src/xrt/state_trackers/oxr/oxr_objects.h @@ -673,7 +673,7 @@ void oxr_xdev_destroy(struct xrt_device **xdev_ptr); void -oxr_xdev_update(struct xrt_device *xdev, struct time_state *timekeeping); +oxr_xdev_update(struct xrt_device *xdev); /*! * Return true if it finds an input of that name on this device. @@ -691,13 +691,31 @@ oxr_xdev_find_output(struct xrt_device *xdev, enum xrt_output_name name, struct xrt_output **out_output); +/*! + * Returns the pose of the named input from the device, if the pose isn't valid + * uses the device offset instead. + */ void oxr_xdev_get_pose_at(struct oxr_logger *log, struct oxr_instance *inst, struct xrt_device *xdev, enum xrt_input_name name, - struct xrt_pose *pose, - int64_t *timestamp); + XrTime at_time, + uint64_t *out_pose_timestamp_ns, + struct xrt_pose *out_pose); + +/*! + * Returns the relation of the named input from the device, always ensures + * that position and orientation is valid by using the device offset. + */ +void +oxr_xdev_get_relation_at(struct oxr_logger *log, + struct oxr_instance *inst, + struct xrt_device *xdev, + enum xrt_input_name name, + XrTime at_time, + uint64_t *out_relation_timestamp_ns, + struct xrt_space_relation *out_relation); /* diff --git a/src/xrt/state_trackers/oxr/oxr_session.c b/src/xrt/state_trackers/oxr/oxr_session.c index ecf0f6d07..11b09128b 100644 --- a/src/xrt/state_trackers/oxr/oxr_session.c +++ b/src/xrt/state_trackers/oxr/oxr_session.c @@ -15,9 +15,10 @@ #include "util/u_debug.h" #include "util/u_misc.h" -#include "math/m_api.h" #include "util/u_time.h" +#include "math/m_api.h" + #include "xrt/xrt_device.h" #include "xrt/xrt_gfx_xlib.h" #include "xrt/xrt_gfx_vk.h" @@ -199,36 +200,23 @@ oxr_session_get_view_pose_at(struct oxr_logger *log, // get at least a slightly better position. struct xrt_device *xdev = sess->sys->head; - struct xrt_pose *offset = &xdev->tracking_origin->offset; - struct xrt_space_relation relation; - int64_t timestamp; - xdev->get_tracked_pose(xdev, XRT_INPUT_GENERIC_HEAD_POSE, - sess->sys->inst->timekeeping, ×tamp, - &relation); + uint64_t timestamp; - // Add in the offset from the tracking system. - math_relation_apply_offset(offset, &relation); + // Applies the offset in the function. + oxr_xdev_get_relation_at(log, sess->sys->inst, xdev, + XRT_INPUT_GENERIC_HEAD_POSE, at_time, + ×tamp, &relation); // clang-format off - bool valid_pos = (relation.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; - bool valid_ori = (relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0; + // Function above always makes the pose valid. + assert((relation.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0); + assert((relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0); + *pose = relation.pose; + bool valid_vel = (relation.relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0; // clang-format on - if (valid_ori) { - pose->orientation = relation.pose.orientation; - } else { - // If the orientation is not valid just use the offset. - pose->orientation = offset->orientation; - } - - if (valid_pos) { - pose->position = relation.pose.position; - } else { - // If the position is not valid just use the offset. - pose->position = offset->position; - } if (valid_vel) { //! @todo Forcing a fixed amount of prediction for now since diff --git a/src/xrt/state_trackers/oxr/oxr_space.c b/src/xrt/state_trackers/oxr/oxr_space.c index 245575624..ded14ff07 100644 --- a/src/xrt/state_trackers/oxr/oxr_space.c +++ b/src/xrt/state_trackers/oxr/oxr_space.c @@ -194,12 +194,12 @@ oxr_space_action_relation(struct oxr_logger *log, struct oxr_session *sess, struct oxr_space *spc, struct oxr_space *baseSpc, - XrTime time, + XrTime at_time, struct xrt_space_relation *out_relation) { struct oxr_source_input *input = NULL; struct oxr_space *act_spc, *ref_spc = NULL; - int64_t timestamp = 0; + uint64_t timestamp = 0; bool invert = false; @@ -245,8 +245,8 @@ oxr_space_action_relation(struct oxr_logger *log, } oxr_xdev_get_pose_at(log, sess->sys->inst, input->xdev, - input->input->name, &out_relation->pose, - ×tamp); + input->input->name, at_time, ×tamp, + &out_relation->pose); out_relation->relation_flags = (enum xrt_space_relation_flags)( XRT_SPACE_RELATION_POSITION_VALID_BIT | diff --git a/src/xrt/state_trackers/oxr/oxr_xdev.c b/src/xrt/state_trackers/oxr/oxr_xdev.c index d08d83eb9..dcacfcd7f 100644 --- a/src/xrt/state_trackers/oxr/oxr_xdev.c +++ b/src/xrt/state_trackers/oxr/oxr_xdev.c @@ -7,11 +7,15 @@ * @ingroup oxr_main */ +#include "os/os_time.h" #include "math/m_api.h" +#include "util/u_time.h" #include "util/u_misc.h" #include "oxr_objects.h" +#include + void oxr_xdev_destroy(struct xrt_device **xdev_ptr) @@ -27,10 +31,10 @@ oxr_xdev_destroy(struct xrt_device **xdev_ptr) } void -oxr_xdev_update(struct xrt_device *xdev, struct time_state *timekeeping) +oxr_xdev_update(struct xrt_device *xdev) { if (xdev != NULL) { - xdev->update_inputs(xdev, timekeeping); + xdev->update_inputs(xdev); } } @@ -75,40 +79,83 @@ oxr_xdev_find_output(struct xrt_device *xdev, return false; } +static void +ensure_valid_position_and_orientation(struct xrt_space_relation *relation, + const struct xrt_pose *fallback) +{ + // clang-format off + bool valid_pos = (relation->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; + bool valid_ori = (relation->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0; + // clang-format on + + if (!valid_ori) { + relation->pose.orientation = fallback->orientation; + } + + if (!valid_pos) { + relation->pose.position = fallback->position; + } + + relation->relation_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT; + relation->relation_flags |= XRT_SPACE_RELATION_ORIENTATION_VALID_BIT; +} + +void +oxr_xdev_get_relation_at(struct oxr_logger *log, + struct oxr_instance *inst, + struct xrt_device *xdev, + enum xrt_input_name name, + XrTime at_time, + uint64_t *out_relation_timestamp_ns, + struct xrt_space_relation *out_relation) +{ + struct xrt_pose *offset = &xdev->tracking_origin->offset; + + //! @todo Convert at_time to monotonic and give to device. + uint64_t at_timestamp_ns = os_monotonic_get_ns(); + (void)at_time; + + uint64_t relation_timestamp_ns = 0; + + struct xrt_space_relation relation; + U_ZERO(&relation); + + xrt_device_get_tracked_pose(xdev, name, at_timestamp_ns, + &relation_timestamp_ns, &relation); + + // Add in the offset from the tracking system. + math_relation_apply_offset(offset, &relation); + + // Always make those to base things valid. + ensure_valid_position_and_orientation(&relation, offset); + + *out_relation_timestamp_ns = time_state_from_monotonic_ns( + inst->timekeeping, relation_timestamp_ns); + + *out_relation = relation; +} + void oxr_xdev_get_pose_at(struct oxr_logger *log, struct oxr_instance *inst, struct xrt_device *xdev, enum xrt_input_name name, - struct xrt_pose *pose, - int64_t *timestamp) + XrTime at_time, + uint64_t *out_pose_timestamp_ns, + struct xrt_pose *out_pose) { - struct xrt_pose *offset = &xdev->tracking_origin->offset; - struct xrt_space_relation relation; U_ZERO(&relation); - xdev->get_tracked_pose(xdev, name, inst->timekeeping, timestamp, - &relation); - // Add in the offset from the tracking system. - math_relation_apply_offset(offset, &relation); + oxr_xdev_get_relation_at(log, inst, xdev, name, at_time, + out_pose_timestamp_ns, &relation); + // Function above makes them valid. // clang-format off - bool valid_pos = (relation.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; - bool valid_ori = (relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0; + assert((relation.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0); + assert((relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0); // clang-format on - if (valid_ori) { - pose->orientation = relation.pose.orientation; - } else { - // If the orientation is not valid just use the offset. - pose->orientation = offset->orientation; - } - - if (valid_pos) { - pose->position = relation.pose.position; - } else { - // If the position is not valid just use the offset. - pose->position = offset->position; - } + out_pose->position = relation.pose.position; + out_pose->orientation = relation.pose.orientation; }