mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-04 06:06:17 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/psmv: Porting to changed API that uses timestamps instead of dt

Browse source
This commit is contained in:
Ryan Pavlik 2019-11-11 15:08:55 -06:00 committed by Jakob Bornecrantz
parent 7314bd75b7
commit aaf08a2abd
1 changed files with 15 additions and 13 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

28
src/xrt/drivers/psmv/psmv_driver.c
View file

@ -607,7 +607,8 @@ psmv_update_trigger_value(struct psmv_device *psmv, int index, int64_t now)
static void static void
update_fusion(struct psmv_device *psmv, update_fusion(struct psmv_device *psmv,
struct psmv_parsed_sample *sample, struct psmv_parsed_sample *sample,
timepoint_ns delta_ns) timepoint_ns timestamp_ns,
time_duration_ns delta_ns)
{ {
struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f}; struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
@ -641,24 +642,21 @@ update_fusion(struct psmv_device *psmv,
xrt_tracked_psmv_push_imu(psmv->ball, delta_ns, &sample); xrt_tracked_psmv_push_imu(psmv->ball, delta_ns, &sample);
} else { } else {
float dt = time_ns_to_s(delta_ns);
#if 0 #if 0
// Super simple fusion. // Super simple fusion.
math_quat_integrate_velocity( math_quat_integrate_velocity(
&psmv->fusion.rot, &psmv->read.gyro, dt, &psmv->fusion.rot); &psmv->fusion.rot, &psmv->read.gyro, dt, &psmv->fusion.rot);
#else #else
imu_fusion_incorporate_gyros(psmv->fusion.fusion, dt, imu_fusion_incorporate_gyros_and_accelerometer(
&psmv->read.gyro, psmv->fusion.fusion, timestamp_ns, &psmv->read.gyro,
&psmv->fusion.variance.gyro); &psmv->fusion.variance.gyro, &psmv->read.accel,
imu_fusion_incorporate_accelerometer(
psmv->fusion.fusion, 0, &psmv->read.accel,
&psmv->fusion.variance.accel); &psmv->fusion.variance.accel);
struct xrt_vec3 angvel_dummy; struct xrt_vec3 angvel_dummy;
imu_fusion_get_prediction(psmv->fusion.fusion, 0, imu_fusion_get_prediction(psmv->fusion.fusion, timestamp_ns,
&psmv->fusion.rot, &angvel_dummy); &psmv->fusion.rot, &angvel_dummy);
imu_fusion_get_prediction_rotation_vec(psmv->fusion.fusion, 0, imu_fusion_get_prediction_rotation_vec(
&psmv->fusion.rotvec); psmv->fusion.fusion, timestamp_ns, &psmv->fusion.rotvec);
#endif #endif
} }
} }
@ -698,6 +696,7 @@ static void *
psmv_run_thread(void *ptr) psmv_run_thread(void *ptr)
{ {
struct psmv_device *psmv = (struct psmv_device *)ptr; struct psmv_device *psmv = (struct psmv_device *)ptr;
//! @todo this should be injected at construction time
struct time_state *time = time_state_create(); struct time_state *time = time_state_create();
union { union {
@ -737,11 +736,14 @@ psmv_run_thread(void *ptr)
// Process the parsed data. // Process the parsed data.
if (num == 2) { if (num == 2) {
// ZCM1 // ZCM1
update_fusion(psmv, &input.samples[0], delta_ns / 2.0); update_fusion(psmv, &input.samples[0],
update_fusion(psmv, &input.samples[1], delta_ns / 2.0); now_ns - (delta_ns / 2.0),
(delta_ns / 2.0));
update_fusion(psmv, &input.samples[1], now_ns,
(delta_ns / 2.0));
} else if (num == 1) { } else if (num == 1) {
// ZCM2 // ZCM2
update_fusion(psmv, &input.sample, delta_ns); update_fusion(psmv, &input.sample, now_ns, delta_ns);
} else { } else {
assert(false); assert(false);
} }