The open source OpenXR runtime
d/pssense: Use IMU timestamp for fusion and read battery state
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2301>
+107
-18
+107
-18
src/xrt/drivers/pssense/pssense_driver.c
+107
-18
src/xrt/drivers/pssense/pssense_driver.c
···
132
132
//! A single point of resistance at the beginning of the trigger, right before the click flag is activated
133
133
const uint8_t TRIGGER_FEEDBACK_MODE_CATCH = 0x02;
134
134
135
+
const uint8_t CHARGE_STATE_DISCHARGING = 0x00;
136
+
const uint8_t CHARGE_STATE_CHARGING = 0x01;
137
+
const uint8_t CHARGE_STATE_FULL = 0x02;
138
+
const uint8_t CHARGE_STATE_ABNORMAL_VOLTAGE = 0x0A;
139
+
const uint8_t CHARGE_STATE_ABNORMAL_TEMP = 0x0B;
140
+
const uint8_t CHARGE_STATE_CHARGING_ERROR = 0x0F;
141
+
135
142
/**
136
143
* 16-bit little-endian int
137
144
*/
···
171
178
struct pssense_i32_le seq_no;
172
179
struct pssense_i16_le gyro[3];
173
180
struct pssense_i16_le accel[3];
174
-
uint8_t unknown3[3];
175
-
uint8_t unknown4; // Increments occasionally
176
-
uint8_t battery_level; // Range appears to be 0x00-0x0e
177
-
uint8_t unknown5[10];
178
-
uint8_t charging_state; // 0x00 when unplugged, 0x20 when charging
179
-
uint8_t unknown6[29];
181
+
struct pssense_i32_le imu_ticks;
182
+
uint8_t temperature;
183
+
uint8_t unknown3[9];
184
+
uint8_t battery_state; // High bits charge level 0x00-0x0a, low bits battery state
185
+
uint8_t plug_state; // Flags for USB data and/or power connected
186
+
struct pssense_i32_le host_timestamp;
187
+
struct pssense_i32_le device_timestamp;
188
+
uint8_t unknown4[4];
189
+
uint8_t aes_cmac[8];
190
+
uint8_t unknown5;
191
+
uint8_t crc_failure_count;
192
+
uint8_t padding[7];
180
193
struct pssense_i32_le crc;
181
194
};
182
195
static_assert(sizeof(struct pssense_input_report) == INPUT_REPORT_LENGTH, "Incorrect input report struct length");
···
188
201
struct pssense_output_report
189
202
{
190
203
uint8_t report_id;
191
-
uint8_t seq_no; // High bits only; low bits are always 0
192
-
uint8_t tag; // Needs to be 0x10. Nobody seems to know why.
204
+
uint8_t bt_seq_no; // High bits only; low bits are always 0
205
+
uint8_t tag; // Needs to be 0x10 for this report
193
206
uint8_t feedback_flags; // Vibrate mode and enable flags to set vibrate and trigger feedback in this report
194
207
uint8_t unknown;
195
208
uint8_t vibration_amplitude; // Vibration amplitude from 0x00-0xff. Sending 0 turns vibration off.
196
209
uint8_t unknown2;
197
210
uint8_t trigger_feedback_mode; // Constant or sticky trigger resistance
198
-
uint8_t unknown3[66];
211
+
uint8_t ffb[10];
212
+
struct pssense_i32_le host_timestamp;
213
+
uint8_t unknown3[19];
214
+
uint8_t counter;
215
+
uint8_t haptics[32];
199
216
struct pssense_i32_le crc;
200
217
};
201
218
static_assert(sizeof(struct pssense_output_report) == OUTPUT_REPORT_LENGTH, "Incorrect output report struct length");
···
244
261
bool thumbstick_touch;
245
262
struct xrt_vec2 thumbstick;
246
263
264
+
uint32_t imu_ticks_last;
265
+
uint64_t imu_ticks_total;
247
266
struct xrt_vec3_i32 gyro_raw;
248
267
struct xrt_vec3_i32 accel_raw;
268
+
269
+
bool battery_state_valid;
270
+
bool battery_charging;
271
+
//! 0..1
272
+
float battery_charge_percent;
249
273
};
250
274
251
275
/*!
···
271
295
272
296
//! Input state parsed from most recent packet
273
297
struct pssense_input_state state;
274
-
//! Last output state sent to device
298
+
//! Pending output state to send to device
275
299
struct
276
300
{
277
301
uint8_t next_seq_no;
···
427
451
input->thumbstick_click = (data->buttons[1] & 8) != 0;
428
452
}
429
453
430
-
input->gyro_raw.x = pssense_i16_le_to_i16(&data->gyro[0]);
431
-
input->gyro_raw.y = pssense_i16_le_to_i16(&data->gyro[1]);
432
-
input->gyro_raw.z = pssense_i16_le_to_i16(&data->gyro[2]);
454
+
uint32_t imu_ticks = pssense_i32_le_to_u32(&data->imu_ticks);
455
+
int64_t imu_ticks_delta = imu_ticks - input->imu_ticks_last;
456
+
if (imu_ticks_delta >= 0) {
457
+
input->imu_ticks_total += imu_ticks_delta;
458
+
input->imu_ticks_last = imu_ticks;
459
+
460
+
input->gyro_raw.x = pssense_i16_le_to_i16(&data->gyro[0]);
461
+
input->gyro_raw.y = pssense_i16_le_to_i16(&data->gyro[1]);
462
+
input->gyro_raw.z = pssense_i16_le_to_i16(&data->gyro[2]);
463
+
464
+
input->accel_raw.x = pssense_i16_le_to_i16(&data->accel[0]);
465
+
input->accel_raw.y = pssense_i16_le_to_i16(&data->accel[1]);
466
+
input->accel_raw.z = pssense_i16_le_to_i16(&data->accel[2]);
467
+
} else {
468
+
PSSENSE_WARN(pssense, "Time went backwards. Check your play area for black holes.");
469
+
}
470
+
471
+
uint8_t battery_state = data->battery_state >> 4;
472
+
// Charge values go from 0..10, so add 5% and cap at 100% so we never show 0% charge
473
+
float battery_percent = MIN(1.0f, (data->battery_state & 0xf) * .1f + .05);
474
+
bool valid, charging;
475
+
if (battery_state == CHARGE_STATE_DISCHARGING) {
476
+
valid = true;
477
+
charging = false;
478
+
} else if (battery_state == CHARGE_STATE_CHARGING) {
479
+
valid = true;
480
+
charging = true;
481
+
} else if (battery_state == CHARGE_STATE_FULL) {
482
+
valid = true;
483
+
charging = true;
484
+
battery_percent = 1.0f;
485
+
} else if (battery_state == CHARGE_STATE_ABNORMAL_VOLTAGE) {
486
+
valid = false;
487
+
PSSENSE_WARN(pssense, "Unable to determine charge state: abnormal voltage");
488
+
} else if (battery_state == CHARGE_STATE_ABNORMAL_TEMP) {
489
+
valid = false;
490
+
PSSENSE_WARN(pssense, "Unable to determine charge state: abnormal temp");
491
+
} else if (battery_state == CHARGE_STATE_CHARGING_ERROR) {
492
+
valid = false;
493
+
PSSENSE_WARN(pssense, "Unable to determine charge state: charging error");
494
+
} else {
495
+
valid = false;
496
+
PSSENSE_WARN(pssense, "Unable to determine charge state: unknown reason");
497
+
}
433
498
434
-
input->accel_raw.x = pssense_i16_le_to_i16(&data->accel[0]);
435
-
input->accel_raw.y = pssense_i16_le_to_i16(&data->accel[1]);
436
-
input->accel_raw.z = pssense_i16_le_to_i16(&data->accel[2]);
499
+
input->battery_state_valid = valid;
500
+
if (valid) {
501
+
if (charging != input->battery_charging || battery_percent != input->battery_charge_percent) {
502
+
PSSENSE_DEBUG(pssense, "Battery at %.f%%, %s", battery_percent * 100,
503
+
charging ? "charging" : "discharging");
504
+
}
505
+
input->battery_charging = charging;
506
+
input->battery_charge_percent = battery_percent;
507
+
}
437
508
438
509
return true;
439
510
}
···
453
524
454
525
// TODO: Apply correction from calibration data
455
526
456
-
m_imu_3dof_update(&pssense->fusion, pssense->state.timestamp_ns, &accel, &gyro);
527
+
// Each IMU tick is .33μs
528
+
m_imu_3dof_update(&pssense->fusion, pssense->state.imu_ticks_total * 333, &accel, &gyro);
457
529
pssense->pose.orientation = pssense->fusion.rot;
458
530
}
459
531
···
464
536
465
537
struct pssense_output_report report = {0};
466
538
report.report_id = OUTPUT_REPORT_ID;
467
-
report.seq_no = pssense->output.next_seq_no << 4;
539
+
report.bt_seq_no = pssense->output.next_seq_no << 4;
468
540
report.tag = OUTPUT_REPORT_TAG;
469
541
470
542
if (timestamp_ns >= pssense->output.vibration_end_timestamp_ns) {
···
713
785
m_relation_chain_resolve(&xrc, out_relation);
714
786
}
715
787
788
+
static xrt_result_t
789
+
pssense_get_battery_status(struct xrt_device *xdev, bool *out_present, bool *out_charging, float *out_charge)
790
+
{
791
+
struct pssense_device *pssense = (struct pssense_device *)xdev;
792
+
if (!pssense->state.battery_state_valid) {
793
+
*out_present = false;
794
+
return XRT_SUCCESS;
795
+
}
796
+
797
+
*out_present = true;
798
+
*out_charging = pssense->state.battery_charging;
799
+
*out_charge = pssense->state.battery_charge_percent;
800
+
return XRT_SUCCESS;
801
+
}
802
+
716
803
/**
717
804
* Retrieving the calibration data report will switch the Sense controller from compat mode into full mode.
718
805
*/
···
802
889
pssense->base.update_inputs = pssense_device_update_inputs;
803
890
pssense->base.set_output = pssense_set_output;
804
891
pssense->base.get_tracked_pose = pssense_get_tracked_pose;
892
+
pssense->base.get_battery_status = pssense_get_battery_status;
805
893
pssense->base.destroy = pssense_device_destroy;
806
894
pssense->base.orientation_tracking_supported = true;
895
+
pssense->base.battery_status_supported = true;
807
896
808
897
pssense->base.binding_profiles = binding_profiles_pssense;
809
898
pssense->base.binding_profile_count = ARRAY_SIZE(binding_profiles_pssense);