mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-29 19:16:21 +00:00
d/pssense: Implement controller vibration and CRC check for input reports
This commit is contained in:
Jarett Millard
Jakob Bornecrantz
parent
e3ce528703
commit
fe6d584712
|
|
@ -52,13 +52,17 @@ static struct xrt_binding_input_pair simple_inputs_pssense[4] = {
|
|||
{XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_PSSENSE_AIM_POSE},
|
||||
};
|
||||
|
||||
static struct xrt_binding_output_pair simple_outputs_pssense[1] = {
|
||||
{XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_PSSENSE_VIBRATION},
|
||||
};
|
||||
|
||||
static struct xrt_binding_profile binding_profiles_pssense[1] = {
|
||||
{
|
||||
.name = XRT_DEVICE_SIMPLE_CONTROLLER,
|
||||
.inputs = simple_inputs_pssense,
|
||||
.input_count = ARRAY_SIZE(simple_inputs_pssense),
|
||||
.outputs = NULL,
|
||||
.output_count = 0,
|
||||
.outputs = simple_outputs_pssense,
|
||||
.output_count = ARRAY_SIZE(simple_outputs_pssense),
|
||||
},
|
||||
};
|
||||
|
||||
|
|
@ -92,20 +96,33 @@ enum pssense_input_index
|
|||
PSSENSE_INDEX_AIM_POSE,
|
||||
};
|
||||
|
||||
const uint8_t HID_PACKET_REPORT_ID = 0x31;
|
||||
const uint8_t INPUT_REPORT_ID = 0x31;
|
||||
const uint8_t OUTPUT_REPORT_ID = 0x31;
|
||||
const uint8_t OUTPUT_REPORT_TAG = 0x10;
|
||||
const uint8_t CALIBRATION_DATA_FEATURE_REPORT_ID = 0x05;
|
||||
const uint8_t CALIBRATION_DATA_PART_ID_1 = 0;
|
||||
const uint8_t CALIBRATION_DATA_PART_ID_2 = 0x81;
|
||||
|
||||
/**
|
||||
* Gyro read value range is +-32768.
|
||||
*/
|
||||
const uint8_t INPUT_REPORT_CRC32_SEED = 0xa1;
|
||||
const uint8_t OUTPUT_REPORT_CRC32_SEED = 0xa2;
|
||||
const uint8_t FEATURE_REPORT_CRC32_SEED = 0xa3;
|
||||
|
||||
//! Gyro read value range is +-32768.
|
||||
const double PSSENSE_GYRO_SCALE_DEG = 180.0 / 1024;
|
||||
/**
|
||||
* Accelerometer read value range is +-32768 and covers +-8 g.
|
||||
*/
|
||||
//! Accelerometer read value range is +-32768 and covers +-8 g.
|
||||
const double PSSENSE_ACCEL_SCALE = MATH_GRAVITY_M_S2 / 4096;
|
||||
|
||||
//! Flag bits to enable setting vibration in an output report
|
||||
const uint8_t VIBRATE_ENABLE_BITS = 0x03;
|
||||
//! Pure 120Hz vibration
|
||||
const uint8_t VIBRATE_MODE_HIGH_120HZ = 0x00;
|
||||
//! Pure 60Hz vibration
|
||||
const uint8_t VIBRATE_MODE_LOW_60HZ = 0x20;
|
||||
//! Emulates a legacy vibration motor
|
||||
const uint8_t VIBRATE_MODE_CLASSIC_RUMBLE = 0x40;
|
||||
//! Softer rumble emulation, like an engine running
|
||||
const uint8_t VIBRATE_MODE_DIET_RUMBLE = 0x60;
|
||||
|
||||
/**
|
||||
* 16-bit little-endian int
|
||||
*/
|
||||
|
|
@ -126,10 +143,11 @@ struct pssense_i32_le
|
|||
uint8_t highest;
|
||||
};
|
||||
|
||||
#define INPUT_REPORT_LENGTH 78
|
||||
/*!
|
||||
* HID data packet.
|
||||
* HID input report data packet.
|
||||
*/
|
||||
struct pssense_data_packet
|
||||
struct pssense_input_report
|
||||
{
|
||||
uint8_t report_id;
|
||||
uint8_t bt_header;
|
||||
|
|
@ -150,8 +168,26 @@ struct pssense_data_packet
|
|||
uint8_t unknown5[10];
|
||||
uint8_t charging_state; // 0x00 when unplugged, 0x20 when charging
|
||||
uint8_t unknown6[29];
|
||||
uint8_t crc[4];
|
||||
struct pssense_i32_le crc;
|
||||
};
|
||||
static_assert(sizeof(struct pssense_input_report) == INPUT_REPORT_LENGTH, "Incorrect input report struct length");
|
||||
|
||||
#define OUTPUT_REPORT_LENGTH 78
|
||||
/**
|
||||
* HID output report data packet.
|
||||
*/
|
||||
struct pssense_output_report
|
||||
{
|
||||
uint8_t report_id;
|
||||
uint8_t seq_no; // High bits only; low bits are always 0
|
||||
uint8_t tag; // Needs to be 0x10. Nobody seems to know why.
|
||||
uint8_t vibration_flags; // Vibrate mode and enable flags to set vibrate in this report
|
||||
uint8_t unknown;
|
||||
uint8_t vibration_amplitude; // Vibration amplitude from 0x00-0xff. Sending 0 turns vibration off.
|
||||
uint8_t unknown3[68];
|
||||
struct pssense_i32_le crc;
|
||||
};
|
||||
static_assert(sizeof(struct pssense_output_report) == OUTPUT_REPORT_LENGTH, "Incorrect output report struct length");
|
||||
|
||||
/*!
|
||||
* PlayStation Sense state parsed from a data packet.
|
||||
|
|
@ -210,6 +246,15 @@ struct pssense_device
|
|||
|
||||
//! Input state parsed from most recent packet
|
||||
struct pssense_input_state state;
|
||||
//! Last output state sent to device
|
||||
struct
|
||||
{
|
||||
uint8_t next_seq_no;
|
||||
uint8_t vibration_amplitude;
|
||||
uint8_t vibration_mode;
|
||||
uint64_t vibration_end_timestamp_ns;
|
||||
uint64_t resend_timestamp_ns;
|
||||
} output;
|
||||
|
||||
struct m_imu_3dof fusion;
|
||||
struct xrt_pose pose;
|
||||
|
|
@ -227,6 +272,19 @@ pssense_i32_le_to_u32(const struct pssense_i32_le *from)
|
|||
return (uint32_t)(from->lowest | from->lower << 8 | from->higher << 16 | from->highest << 24);
|
||||
}
|
||||
|
||||
static struct pssense_i32_le
|
||||
pssense_u32_to_i32_le(uint32_t from)
|
||||
{
|
||||
struct pssense_i32_le ret = {
|
||||
.lowest = (from >> 0) & 0x0ff,
|
||||
.lower = (from >> 8) & 0x0ff,
|
||||
.higher = (from >> 16) & 0x0ff,
|
||||
.highest = (from >> 24) & 0x0ff,
|
||||
};
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int16_t
|
||||
pssense_i16_le_to_i16(const struct pssense_i16_le *from)
|
||||
{
|
||||
|
|
@ -234,6 +292,20 @@ pssense_i16_le_to_i16(const struct pssense_i16_le *from)
|
|||
return (int16_t)(from->low | from->high << 8);
|
||||
}
|
||||
|
||||
const uint32_t CRC_POLYNOMIAL = 0xedb88320;
|
||||
static uint32_t
|
||||
crc32_le(uint32_t crc, uint8_t const *p, size_t len)
|
||||
{
|
||||
int i;
|
||||
crc ^= 0xffffffff;
|
||||
while (len--) {
|
||||
crc ^= *p++;
|
||||
for (i = 0; i < 8; i++)
|
||||
crc = (crc >> 1) ^ ((crc & 1) ? CRC_POLYNOMIAL : 0);
|
||||
}
|
||||
return crc ^ 0xffffffff;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Reads one packet from the device, handles time out, locking and checking if
|
||||
* the thread has been told to shut down.
|
||||
|
|
@ -273,14 +345,22 @@ pssense_read_one_packet(struct pssense_device *pssense, uint8_t *buffer, size_t
|
|||
|
||||
static bool
|
||||
pssense_parse_packet(struct pssense_device *pssense,
|
||||
struct pssense_data_packet *data,
|
||||
struct pssense_input_report *data,
|
||||
struct pssense_input_state *input)
|
||||
{
|
||||
if (data->report_id != HID_PACKET_REPORT_ID) {
|
||||
if (data->report_id != INPUT_REPORT_ID) {
|
||||
PSSENSE_WARN(pssense, "Unrecognized HID report id %u", data->report_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t expected_crc = pssense_i32_le_to_u32(&data->crc);
|
||||
uint32_t crc = crc32_le(0, &INPUT_REPORT_CRC32_SEED, 1);
|
||||
crc = crc32_le(crc, (uint8_t *)data, sizeof(struct pssense_input_report) - 4);
|
||||
if (crc != expected_crc) {
|
||||
PSSENSE_WARN(pssense, "CRC mismatch; skipping input. Expected %08X but got %08X", expected_crc, crc);
|
||||
return false;
|
||||
}
|
||||
|
||||
input->timestamp_ns = os_monotonic_get_ns();
|
||||
|
||||
uint32_t seq_no = pssense_i32_le_to_u32(&data->seq_no);
|
||||
|
|
@ -349,6 +429,43 @@ pssense_update_fusion(struct pssense_device *pssense)
|
|||
pssense->pose.orientation = pssense->fusion.rot;
|
||||
}
|
||||
|
||||
static void
|
||||
pssense_send_output_report_locked(struct pssense_device *pssense)
|
||||
{
|
||||
uint64_t timestamp_ns = os_monotonic_get_ns();
|
||||
|
||||
if (timestamp_ns >= pssense->output.vibration_end_timestamp_ns) {
|
||||
pssense->output.vibration_amplitude = 0;
|
||||
}
|
||||
|
||||
struct pssense_output_report report = {0};
|
||||
report.report_id = OUTPUT_REPORT_ID;
|
||||
report.seq_no = pssense->output.next_seq_no << 4;
|
||||
report.tag = OUTPUT_REPORT_TAG;
|
||||
report.vibration_flags = pssense->output.vibration_mode | VIBRATE_ENABLE_BITS;
|
||||
report.vibration_amplitude = pssense->output.vibration_amplitude;
|
||||
|
||||
pssense->output.next_seq_no = (pssense->output.next_seq_no + 1) % 16;
|
||||
|
||||
uint32_t crc = crc32_le(0, &OUTPUT_REPORT_CRC32_SEED, 1);
|
||||
crc = crc32_le(crc, (uint8_t *)&report, sizeof(struct pssense_output_report) - 4);
|
||||
report.crc = pssense_u32_to_i32_le(crc);
|
||||
|
||||
PSSENSE_DEBUG(pssense, "Setting vibration amplitude: %u, mode: %02X", pssense->output.vibration_amplitude,
|
||||
pssense->output.vibration_mode);
|
||||
int ret = os_hid_write(pssense->hid, (uint8_t *)&report, sizeof(struct pssense_output_report));
|
||||
if (ret == sizeof(struct pssense_output_report)) {
|
||||
// Controller will vibrate for 5 sec unless we resend the output report. Resend every 2 sec to be safe.
|
||||
pssense->output.resend_timestamp_ns = timestamp_ns + 2000000000;
|
||||
if (pssense->output.resend_timestamp_ns > pssense->output.vibration_end_timestamp_ns) {
|
||||
pssense->output.resend_timestamp_ns = pssense->output.vibration_end_timestamp_ns;
|
||||
}
|
||||
} else {
|
||||
PSSENSE_WARN(pssense, "Failed to send output report: %d", ret);
|
||||
pssense->output.resend_timestamp_ns = timestamp_ns;
|
||||
}
|
||||
}
|
||||
|
||||
static void *
|
||||
pssense_run_thread(void *ptr)
|
||||
{
|
||||
|
|
@ -357,23 +474,27 @@ pssense_run_thread(void *ptr)
|
|||
struct pssense_device *pssense = (struct pssense_device *)ptr;
|
||||
|
||||
union {
|
||||
uint8_t buffer[sizeof(struct pssense_data_packet)];
|
||||
struct pssense_data_packet packet;
|
||||
uint8_t buffer[sizeof(struct pssense_input_report)];
|
||||
struct pssense_input_report report;
|
||||
} data;
|
||||
struct pssense_input_state input_state = {0};
|
||||
|
||||
// The Sense controller starts in compat mode with a different HID report ID and format.
|
||||
// We need to discard packets until we get a correct report.
|
||||
while (pssense_read_one_packet(pssense, data.buffer, sizeof(data), false) &&
|
||||
data.packet.report_id != HID_PACKET_REPORT_ID) {
|
||||
data.report.report_id != INPUT_REPORT_ID) {
|
||||
PSSENSE_DEBUG(pssense, "Discarding compat mode HID report");
|
||||
}
|
||||
|
||||
while (pssense_read_one_packet(pssense, data.buffer, sizeof(data), true)) {
|
||||
if (pssense_parse_packet(pssense, &data.packet, &input_state)) {
|
||||
if (pssense_parse_packet(pssense, &data.report, &input_state)) {
|
||||
os_mutex_lock(&pssense->lock);
|
||||
pssense->state = input_state;
|
||||
pssense_update_fusion(pssense);
|
||||
if (pssense->output.vibration_amplitude > 0 &&
|
||||
pssense->state.timestamp_ns >= pssense->output.resend_timestamp_ns) {
|
||||
pssense_send_output_report_locked(pssense);
|
||||
}
|
||||
os_mutex_unlock(&pssense->lock);
|
||||
}
|
||||
}
|
||||
|
|
@ -442,6 +563,37 @@ pssense_device_update_inputs(struct xrt_device *xdev)
|
|||
os_mutex_unlock(&pssense->lock);
|
||||
}
|
||||
|
||||
static void
|
||||
pssense_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value)
|
||||
{
|
||||
struct pssense_device *pssense = (struct pssense_device *)xdev;
|
||||
|
||||
if (name != XRT_OUTPUT_NAME_PSSENSE_VIBRATION) {
|
||||
PSSENSE_ERROR(pssense, "Unknown output name requested %u", name);
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t vibration_amplitude = (uint8_t)(value->vibration.amplitude * 255.0f);
|
||||
uint8_t vibration_mode = VIBRATE_MODE_CLASSIC_RUMBLE;
|
||||
if (value->vibration.frequency != XRT_FREQUENCY_UNSPECIFIED) {
|
||||
if (value->vibration.frequency <= 70) {
|
||||
vibration_mode = VIBRATE_MODE_LOW_60HZ;
|
||||
} else if (value->vibration.frequency >= 110) {
|
||||
vibration_mode = VIBRATE_MODE_HIGH_120HZ;
|
||||
}
|
||||
}
|
||||
|
||||
os_mutex_lock(&pssense->lock);
|
||||
if (vibration_amplitude != pssense->output.vibration_amplitude ||
|
||||
vibration_mode != pssense->output.vibration_mode) {
|
||||
pssense->output.vibration_amplitude = vibration_amplitude;
|
||||
pssense->output.vibration_mode = vibration_mode;
|
||||
pssense->output.vibration_end_timestamp_ns = os_monotonic_get_ns() + value->vibration.duration_ns;
|
||||
pssense_send_output_report_locked(pssense);
|
||||
}
|
||||
os_mutex_unlock(&pssense->lock);
|
||||
}
|
||||
|
||||
static void
|
||||
pssense_get_fusion_pose(struct pssense_device *pssense,
|
||||
enum xrt_input_name name,
|
||||
|
|
@ -563,12 +715,13 @@ pssense_found(struct xrt_prober *xp,
|
|||
}
|
||||
|
||||
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
|
||||
struct pssense_device *pssense = U_DEVICE_ALLOCATE(struct pssense_device, flags, 23, 0);
|
||||
struct pssense_device *pssense = U_DEVICE_ALLOCATE(struct pssense_device, flags, 23, 1);
|
||||
PSSENSE_DEBUG(pssense, "PlayStation Sense controller found");
|
||||
|
||||
pssense->base.name = XRT_DEVICE_PSSENSE;
|
||||
snprintf(pssense->base.str, XRT_DEVICE_NAME_LEN, "%s", product_name);
|
||||
pssense->base.update_inputs = pssense_device_update_inputs;
|
||||
pssense->base.set_output = pssense_set_output;
|
||||
pssense->base.get_tracked_pose = pssense_get_tracked_pose;
|
||||
pssense->base.destroy = pssense_device_destroy;
|
||||
pssense->base.orientation_tracking_supported = true;
|
||||
|
|
@ -617,6 +770,8 @@ pssense_found(struct xrt_prober *xp,
|
|||
SET_INPUT(GRIP_POSE);
|
||||
SET_INPUT(AIM_POSE);
|
||||
|
||||
pssense->base.outputs[0].name = XRT_OUTPUT_NAME_PSSENSE_VIBRATION;
|
||||
|
||||
ret = os_mutex_init(&pssense->lock);
|
||||
if (ret != 0) {
|
||||
PSSENSE_ERROR(pssense, "Failed to init mutex!");
|
||||
|
|
|
|||
|
|
@ -1123,6 +1123,8 @@ enum xrt_output_name
|
|||
XRT_OUTPUT_NAME_G2_CONTROLLER_HAPTIC = XRT_OUTPUT_NAME(0x0090, VIBRATION),
|
||||
XRT_OUTPUT_NAME_ODYSSEY_CONTROLLER_HAPTIC = XRT_OUTPUT_NAME(0x00A0, VIBRATION),
|
||||
XRT_OUTPUT_NAME_ML2_CONTROLLER_VIBRATION = XRT_OUTPUT_NAME(0x00B0, VIBRATION),
|
||||
|
||||
XRT_OUTPUT_NAME_PSSENSE_VIBRATION = XRT_OUTPUT_NAME(0x00C0, VIBRATION),
|
||||
// clang-format on
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue