From e179349644b8d39407b7913d0cd41ea1cbc5f606 Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Wed, 11 Sep 2019 16:42:59 +0100 Subject: [PATCH] d/psvr: Super simple sensor "fusion" --- src/xrt/drivers/psvr/psvr_device.c | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/xrt/drivers/psvr/psvr_device.c b/src/xrt/drivers/psvr/psvr_device.c index ff38b27f3..78be0fde5 100644 --- a/src/xrt/drivers/psvr/psvr_device.c +++ b/src/xrt/drivers/psvr/psvr_device.c @@ -12,6 +12,8 @@ #include "xrt/xrt_compiler.h" +#include "math/m_api.h" + #include "util/u_var.h" #include "util/u_misc.h" #include "util/u_time.h" @@ -80,6 +82,11 @@ struct psvr_device bool last_frame; bool control; } gui; + + struct + { + struct xrt_quat rot; + } fusion; }; @@ -202,6 +209,9 @@ update_fusion(struct psvr_device *psvr, accel_from_psvr_vec(&sample->accel, &psvr->read.accel); gyro_from_psvr_vec(&sample->gyro, &psvr->read.gyro); + math_quat_integrate_velocity(&psvr->fusion.rot, &psvr->read.gyro, dt, + &psvr->fusion.rot); + //! @todo This is where we do the sensor fusion. // ofusion_update(&psvr->sensor_fusion, dt, &psvr->raw.gyro, // &psvr->raw.accel, &mag); @@ -655,7 +665,7 @@ psvr_device_get_tracked_pose(struct xrt_device *xdev, //! @todo adjust for latency here *out_timestamp = now; - out_relation->pose.orientation.w = 1.0f; + out_relation->pose.orientation = psvr->fusion.rot; //! @todo assuming that orientation is actually currently tracked. out_relation->relation_flags = (enum xrt_space_relation_flags)( @@ -729,6 +739,7 @@ psvr_device_create(struct hid_device_info *hmd_handle_info, psvr->base.destroy = psvr_device_destroy; psvr->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_RELATION; psvr->base.name = XRT_DEVICE_GENERIC_HMD; + psvr->fusion.rot.w = 1.0f; snprintf(psvr->base.str, XRT_DEVICE_NAME_LEN, "PS VR Headset");