The open source OpenXR runtime
d/psmv: Don't use the copy sample on ZCM2
+94
-56
+94
-56
src/xrt/drivers/psmv/psmv_driver.c
+94
-56
src/xrt/drivers/psmv/psmv_driver.c
···
318
318
{
319
319
struct xrt_vec3_i32 accel;
320
320
struct xrt_vec3_i32 gyro;
321
-
uint8_t trigger;
322
321
};
323
322
324
323
/*!
···
331
330
uint8_t battery;
332
331
uint8_t seq_no;
333
332
334
-
struct psmv_parsed_sample sample[2];
333
+
334
+
union {
335
+
//! Trigger for the last two frames (ZCM1).
336
+
uint8_t trigger_values[2];
337
+
338
+
struct
339
+
{
340
+
//! Low-pass filtered versio of trigger (ZCM2).
341
+
uint8_t trigger_low_pass;
342
+
343
+
//! Trigger (ZCM2).
344
+
uint8_t trigger;
345
+
};
346
+
};
347
+
348
+
union {
349
+
//! Accelerometer and gyro scope samples (ZCM1).
350
+
struct psmv_parsed_sample samples[2];
351
+
352
+
struct
353
+
{
354
+
//! Accelerometer and gyro scope samples (ZCM2).
355
+
struct psmv_parsed_sample sample;
356
+
357
+
//! Copy of above (ZCM2).
358
+
struct psmv_parsed_sample sample_copy;
359
+
};
360
+
};
335
361
};
336
362
337
363
/*!
···
490
516
static int
491
517
psmv_get_calibration(struct psmv_device *psmv);
492
518
493
-
static void
519
+
static int
494
520
psmv_parse_input(struct psmv_device *psmv,
495
521
void *data,
496
522
struct psmv_parsed_input *input);
···
551
577
psmv_update_trigger_value(struct psmv_device *psmv, int index, int64_t now)
552
578
{
553
579
psmv->base.inputs[index].timestamp = now;
554
-
psmv->base.inputs[index].value.vec1.x =
555
-
psmv->last.sample[1].trigger / 255.0f;
580
+
psmv->base.inputs[index].value.vec1.x = psmv->last.trigger / 255.0f;
556
581
}
557
582
558
583
···
653
678
654
679
timepoint_ns now_ns = time_state_get_now(time);
655
680
656
-
psmv_parse_input(psmv, data.buffer, &input);
681
+
int num = psmv_parse_input(psmv, data.buffer, &input);
657
682
658
683
float dt = time_ns_to_s(now_ns - then_ns);
659
684
then_ns = now_ns;
···
665
690
psmv->last = input;
666
691
667
692
// Process the parsed data.
668
-
update_fusion(psmv, &input.sample[0], dt / 2.0);
669
-
update_fusion(psmv, &input.sample[1], dt / 2.0);
693
+
if (num == 2) {
694
+
// ZCM1
695
+
update_fusion(psmv, &input.samples[0], dt / 2.0);
696
+
update_fusion(psmv, &input.samples[1], dt / 2.0);
697
+
} else if (num == 1) {
698
+
// ZCM2
699
+
update_fusion(psmv, &input.sample, dt);
700
+
} else {
701
+
assert(false);
702
+
}
670
703
671
704
// Now done.
672
705
os_mutex_unlock(&psmv->lock);
···
970
1003
u_var_add_vec3_f32(psmv, &psmv->calibration.gyro.factor, "gyro.factor");
971
1004
u_var_add_vec3_f32(psmv, &psmv->calibration.gyro.bias, "gyro.bias");
972
1005
u_var_add_gui_header(psmv, &psmv->gui.last_frame, "Last data");
973
-
u_var_add_ro_vec3_i32(psmv, &psmv->last.sample[0].accel, "last.sample[0].accel");
974
-
u_var_add_ro_vec3_i32(psmv, &psmv->last.sample[1].accel, "last.sample[1].accel");
975
-
u_var_add_ro_vec3_i32(psmv, &psmv->last.sample[0].gyro, "last.sample[0].gyro");
976
-
u_var_add_ro_vec3_i32(psmv, &psmv->last.sample[1].gyro, "last.sample[1].gyro");
1006
+
u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[0].accel, "last.samples[0].accel");
1007
+
u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[1].accel, "last.samples[1].accel");
1008
+
u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[0].gyro, "last.samples[0].gyro");
1009
+
u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[1].gyro, "last.samples[1].gyro");
977
1010
u_var_add_ro_vec3_f32(psmv, &psmv->read.accel, "read.accel");
978
1011
u_var_add_ro_vec3_f32(psmv, &psmv->read.gyro, "read.gyro");
979
1012
u_var_add_gui_header(psmv, &psmv->gui.control, "Control");
···
1213
1246
return 0;
1214
1247
}
1215
1248
1216
-
static void
1249
+
static int
1217
1250
psmv_parse_input_zcm1(struct psmv_device *psmv,
1218
1251
struct psmv_input_zcm1 *data,
1219
1252
struct psmv_parsed_input *input)
···
1228
1261
input->timestamp |= data->timestamp_low;
1229
1262
input->timestamp |= data->timestamp_high << 8;
1230
1263
1231
-
input->sample[0].trigger = data->trigger_f1;
1232
-
psmv_from_vec3_u16_wire(&input->sample[0].accel, &data->accel_f1);
1233
-
psmv_from_vec3_u16_wire(&input->sample[0].gyro, &data->gyro_f1);
1264
+
input->trigger_values[0] = data->trigger_f1;
1265
+
input->trigger_values[1] = data->trigger_f2;
1234
1266
1235
-
input->sample[1].trigger = data->trigger_f2;
1236
-
psmv_from_vec3_u16_wire(&input->sample[1].accel, &data->accel_f2);
1237
-
psmv_from_vec3_u16_wire(&input->sample[1].gyro, &data->gyro_f2);
1267
+
psmv_from_vec3_u16_wire(&input->samples[0].accel, &data->accel_f1);
1268
+
psmv_from_vec3_u16_wire(&input->samples[0].gyro, &data->gyro_f1);
1269
+
1270
+
psmv_from_vec3_u16_wire(&input->samples[1].accel, &data->accel_f2);
1271
+
psmv_from_vec3_u16_wire(&input->samples[1].gyro, &data->gyro_f2);
1238
1272
1239
1273
uint32_t diff = psmv_calc_delta_and_handle_rollover(
1240
1274
input->timestamp, psmv->last.timestamp);
···
1250
1284
"missed: %s\n\t"
1251
1285
"buttons: %08x\n\t"
1252
1286
"battery: %x\n\t"
1253
-
"sample[0].accel: %6i %6i %6i\n\t"
1254
-
"sample[1].accel: %6i %6i %6i\n\t"
1255
-
"sample[0].gyro: %6i %6i %6i\n\t"
1256
-
"sample[1].gyro: %6i %6i %6i\n\t"
1257
-
"sample[0].trigger: %02x\n\t"
1258
-
"sample[1].trigger: %02x\n\t"
1287
+
"samples[0].accel: %6i %6i %6i\n\t"
1288
+
"samples[1].accel: %6i %6i %6i\n\t"
1289
+
"samples[0].gyro: %6i %6i %6i\n\t"
1290
+
"samples[1].gyro: %6i %6i %6i\n\t"
1291
+
"trigger_values[0]: %02x\n\t"
1292
+
"trigger_values[1]: %02x\n\t"
1259
1293
"timestamp: %i\n\t"
1260
1294
"diff: %i\n\t"
1261
1295
"seq_no: %x\n",
1262
1296
missed ? "yes" : "no", input->buttons, input->battery,
1263
-
input->sample[0].accel.x, input->sample[0].accel.y,
1264
-
input->sample[0].accel.z, input->sample[1].accel.x,
1265
-
input->sample[1].accel.y, input->sample[1].accel.z,
1266
-
input->sample[0].gyro.x, input->sample[0].gyro.y,
1267
-
input->sample[0].gyro.z, input->sample[1].gyro.x,
1268
-
input->sample[1].gyro.y, input->sample[1].gyro.z,
1269
-
input->sample[0].trigger, input->sample[1].trigger,
1297
+
input->samples[0].accel.x, input->samples[0].accel.y,
1298
+
input->samples[0].accel.z, input->samples[1].accel.x,
1299
+
input->samples[1].accel.y, input->samples[1].accel.z,
1300
+
input->samples[0].gyro.x, input->samples[0].gyro.y,
1301
+
input->samples[0].gyro.z, input->samples[1].gyro.x,
1302
+
input->samples[1].gyro.y, input->samples[1].gyro.z,
1303
+
input->trigger_values[0], input->trigger_values[1],
1270
1304
input->timestamp, diff, input->seq_no);
1305
+
1306
+
return 2;
1271
1307
}
1272
1308
1273
1309
···
1430
1466
return 0;
1431
1467
}
1432
1468
1433
-
static void
1469
+
static int
1434
1470
psmv_parse_input_zcm2(struct psmv_device *psmv,
1435
1471
struct psmv_input_zcm2 *data,
1436
1472
struct psmv_parsed_input *input)
···
1445
1481
input->timestamp |= data->timestamp_low;
1446
1482
input->timestamp |= data->timestamp_high << 8;
1447
1483
1448
-
input->sample[0].trigger = data->trigger_low_pass;
1449
-
input->sample[1].trigger = data->trigger;
1484
+
input->trigger_low_pass = data->trigger_low_pass;
1485
+
input->trigger = data->trigger;
1450
1486
1451
-
psmv_from_vec3_i16_wire(&input->sample[0].accel, &data->accel);
1452
-
psmv_from_vec3_i16_wire(&input->sample[0].gyro, &data->gyro);
1487
+
psmv_from_vec3_i16_wire(&input->sample.accel, &data->accel);
1488
+
psmv_from_vec3_i16_wire(&input->sample.gyro, &data->gyro);
1453
1489
1454
-
psmv_from_vec3_i16_wire(&input->sample[1].accel, &data->accel_copy);
1455
-
psmv_from_vec3_i16_wire(&input->sample[1].gyro, &data->gyro_copy);
1490
+
psmv_from_vec3_i16_wire(&input->sample_copy.accel, &data->accel_copy);
1491
+
psmv_from_vec3_i16_wire(&input->sample_copy.gyro, &data->gyro_copy);
1456
1492
1457
1493
uint32_t diff = psmv_calc_delta_and_handle_rollover(
1458
1494
input->timestamp, psmv->last.timestamp);
···
1468
1504
"missed: %s\n\t"
1469
1505
"buttons: %08x\n\t"
1470
1506
"battery: %x\n\t"
1471
-
"sample[0].accel: %6i %6i %6i\n\t"
1472
-
"sample[1].accel: %6i %6i %6i\n\t"
1473
-
"sample[0].gyro: %6i %6i %6i\n\t"
1474
-
"sample[1].gyro: %6i %6i %6i\n\t"
1507
+
"sample.accel: %6i %6i %6i\n\t"
1508
+
"sample_copy.accel: %6i %6i %6i\n\t"
1509
+
"sample.gyro: %6i %6i %6i\n\t"
1510
+
"sample_copy.gyro: %6i %6i %6i\n\t"
1511
+
"sample.trigger: %02x\n\t"
1475
1512
"sample.trigger_low_pass: %02x\n\t"
1476
-
"sample.trigger: %02x\n\t"
1477
1513
"timestamp: %i\n\t"
1478
1514
"diff: %i\n\t"
1479
1515
"seq_no: %x\n",
1480
1516
missed ? "yes" : "no", input->buttons, input->battery,
1481
-
input->sample[0].accel.x, input->sample[0].accel.y,
1482
-
input->sample[0].accel.z, input->sample[1].accel.x,
1483
-
input->sample[1].accel.y, input->sample[1].accel.z,
1484
-
input->sample[0].gyro.x, input->sample[0].gyro.y,
1485
-
input->sample[0].gyro.z, input->sample[1].gyro.x,
1486
-
input->sample[1].gyro.y, input->sample[1].gyro.z,
1487
-
input->sample[0].trigger, input->sample[1].trigger,
1488
-
input->timestamp, diff, input->seq_no);
1517
+
input->samples[0].accel.x, input->samples[0].accel.y,
1518
+
input->samples[0].accel.z, input->samples[1].accel.x,
1519
+
input->samples[1].accel.y, input->samples[1].accel.z,
1520
+
input->samples[0].gyro.x, input->samples[0].gyro.y,
1521
+
input->samples[0].gyro.z, input->samples[1].gyro.x,
1522
+
input->samples[1].gyro.y, input->samples[1].gyro.z,
1523
+
input->trigger_low_pass, input->trigger, input->timestamp,
1524
+
diff, input->seq_no);
1525
+
1526
+
return 1;
1489
1527
}
1490
1528
1491
1529
···
1507
1545
return 0;
1508
1546
}
1509
1547
1510
-
static void
1548
+
static int
1511
1549
psmv_parse_input(struct psmv_device *psmv,
1512
1550
void *data,
1513
1551
struct psmv_parsed_input *input)
1514
1552
{
1515
1553
switch (psmv->pid) {
1516
-
case PSMV_PID_ZCM1: psmv_parse_input_zcm1(psmv, data, input); break;
1517
-
case PSMV_PID_ZCM2: psmv_parse_input_zcm2(psmv, data, input); break;
1518
-
default: break;
1554
+
case PSMV_PID_ZCM1: return psmv_parse_input_zcm1(psmv, data, input);
1555
+
case PSMV_PID_ZCM2: return psmv_parse_input_zcm2(psmv, data, input);
1556
+
default: return 0;
1519
1557
}
1520
1558
}
1521
1559