mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-02-05 13:28:16 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/psvr: Refactor and improve packet reading

Browse source
This commit is contained in:
Jakob Bornecrantz 2019-09-11 16:06:58 +01:00
parent bf9bcf8e26
commit c2c230b3de
3 changed files with 77 additions and 65 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

60
src/xrt/drivers/psvr/psvr_device.c
View file

@ -49,7 +49,7 @@ struct psvr_device
hid_device *hmd_handle;
hid_device *hmd_control;
struct psvr_sensor_packet sensor;
struct psvr_parsed_sensor last;
struct
{
@ -65,7 +65,7 @@ struct psvr_device
{
struct xrt_vec3 gyro;
struct xrt_vec3 accel;
} raw;
} read;
uint16_t buttons;
@ -168,30 +168,24 @@ scale_led_power(uint8_t power)
}
static void
accel_from_psvr_vec(const int16_t smp[3], struct xrt_vec3 *out_vec)
accel_from_psvr_vec(const struct xrt_vec3_i32 *accel, struct xrt_vec3 *out_vec)
{
//! @todo Figure out calibration data and use here.
// clang-format off
out_vec->x = (float)(smp[1] * (9.81 / 16384.0));
out_vec->y = (float)(smp[0] * (9.81 / 16384.0));
out_vec->z = (float)(smp[2] * -(9.81 / 16384.0));
// clang-format on
out_vec->x = accel->x * (9.81 / 16384.0);
out_vec->y = accel->y * (9.81 / 16384.0);
out_vec->z = accel->z * (9.81 / 16384.0);
}
static void
gyro_from_psvr_vec(const int16_t smp[3], struct xrt_vec3 *out_vec)
gyro_from_psvr_vec(const struct xrt_vec3_i32 *gyro, struct xrt_vec3 *out_vec)
{
//! @todo Figure out calibration data and use here.
out_vec->x = (float)(smp[1] * 0.00105);
out_vec->y = (float)(smp[0] * 0.00105);
out_vec->z = (float)(smp[2] * 0.00105 * -1.0);
out_vec->x = gyro->x * 0.00105;
out_vec->y = gyro->y * 0.00105;
out_vec->z = gyro->z * 0.00105;
}
static void
update_fusion(struct psvr_device *psvr,
struct psvr_sensor_sample *sample,
struct psvr_parsed_sample *sample,
uint32_t tick_delta)
{
struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
@ -199,8 +193,8 @@ update_fusion(struct psvr_device *psvr,
(void)mag;
(void)dt;
accel_from_psvr_vec(sample->accel, &psvr->raw.accel);
gyro_from_psvr_vec(sample->gyro, &psvr->raw.gyro);
accel_from_psvr_vec(&sample->accel, &psvr->read.accel);
gyro_from_psvr_vec(&sample->gyro, &psvr->read.gyro);
//! @todo This is where we do the sensor fusion.
// ofusion_update(&psvr->sensor_fusion, dt, &psvr->raw.gyro,
@ -226,13 +220,13 @@ handle_tracker_sensor_msg(struct psvr_device *psvr,
unsigned char *buffer,
int size)
{
uint32_t last_sample_tick = psvr->sensor.samples[1].tick;
uint32_t last_sample_tick = psvr->last.samples[1].tick;
if (!psvr_parse_sensor_packet(&psvr->sensor, buffer, size)) {
if (!psvr_parse_sensor_packet(&psvr->last, buffer, size)) {
PSVR_ERROR(psvr, "couldn't decode tracker sensor message");
}
struct psvr_sensor_packet *s = &psvr->sensor;
struct psvr_parsed_sensor *s = &psvr->last;
// Simplest is the buttons.
psvr->buttons = s->buttons;
@ -270,9 +264,9 @@ handle_control_status_msg(struct psvr_device *psvr,
unsigned char *buffer,
int size)
{
struct psvr_status_packet packet;
struct psvr_parsed_status status;
if (!psvr_parse_status_packet(&packet, buffer, size)) {
if (!psvr_parse_status_packet(&status, buffer, size)) {
PSVR_ERROR(psvr, "couldn't decode tracker sensor message");
}
@ -281,16 +275,16 @@ handle_control_status_msg(struct psvr_device *psvr,
* Power
*/
if (packet.status & PSVR_STATUS_BIT_POWER) {
if (status.status & PSVR_STATUS_BIT_POWER) {
if (!psvr->powered_on) {
PSVR_DEBUG(psvr, "Device powered on! '%02x'",
packet.status);
status.status);
}
psvr->powered_on = true;
} else {
if (psvr->powered_on) {
PSVR_DEBUG(psvr, "Device powered off! '%02x'",
packet.status);
status.status);
}
psvr->powered_on = false;
}
@ -300,16 +294,16 @@ handle_control_status_msg(struct psvr_device *psvr,
* VR-Mode
*/
if (packet.vr_mode == PSVR_STATUS_VR_MODE_OFF) {
if (status.vr_mode == PSVR_STATUS_VR_MODE_OFF) {
if (psvr->in_vr_mode) {
PSVR_DEBUG(psvr, "Device not in vr-mode! '%02x'",
packet.vr_mode);
status.vr_mode);
}
psvr->in_vr_mode = false;
} else if (packet.vr_mode == PSVR_STATUS_VR_MODE_ON) {
} else if (status.vr_mode == PSVR_STATUS_VR_MODE_ON) {
if (!psvr->in_vr_mode) {
PSVR_DEBUG(psvr, "Device in vr-mode! '%02x'",
packet.vr_mode);
status.vr_mode);
}
psvr->in_vr_mode = true;
} else {
@ -663,8 +657,8 @@ psvr_device_get_tracked_pose(struct xrt_device *xdev,
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
PSVR_SPEW(psvr, "\n\taccel = %f %f %f\n\tgyro = %f %f %f",
psvr->raw.accel.x, psvr->raw.accel.y, psvr->raw.accel.z,
psvr->raw.gyro.x, psvr->raw.gyro.y, psvr->raw.gyro.z);
psvr->read.accel.x, psvr->read.accel.y, psvr->read.accel.z,
psvr->read.gyro.x, psvr->read.gyro.y, psvr->read.gyro.z);
}
static void

23
src/xrt/drivers/psvr/psvr_device.h
View file

@ -64,39 +64,40 @@ enum psvr_status_bits
*/
/*!
* A single gyro, accel and tick sample.
* A parsed single gyro, accel and tick sample.
*
* @ingroup drv_psvr
*/
struct psvr_sensor_sample
struct psvr_parsed_sample
{
int16_t accel[3];
int16_t gyro[3];
struct xrt_vec3_i32 accel;
struct xrt_vec3_i32 gyro;
uint32_t tick;
};
/*!
* A parsed sensor packet from the headset.
* Over the wire sensor packet from the headset.
*
* @ingroup drv_psvr
*/
struct psvr_sensor_packet
struct psvr_parsed_sensor
{
uint8_t buttons;
uint8_t state;
uint16_t volume;
struct psvr_sensor_sample samples[2];
uint16_t button_raw;
uint16_t proximity;
uint8_t seq;
struct psvr_parsed_sample samples[2];
};
/*!
* A parsed status packet from the headset.
* A status packet from the headset in wire format.
*
* @ingroup drv_psvr
*/
struct psvr_status_packet
struct psvr_parsed_status
{
uint8_t status;
uint8_t volume;
@ -118,12 +119,12 @@ psvr_device_create(struct hid_device_info *hmd_handle_info,
bool print_debug);
bool
psvr_parse_sensor_packet(struct psvr_sensor_packet *packet,
psvr_parse_sensor_packet(struct psvr_parsed_sensor *packet,
const uint8_t *buffer,
int size);
bool
psvr_parse_status_packet(struct psvr_status_packet *packet,
psvr_parse_status_packet(struct psvr_parsed_status *packet,
const uint8_t *buffer,
int size);

59
src/xrt/drivers/psvr/psvr_packet.c
View file

@ -55,6 +55,23 @@ read_i16(const uint8_t **buffer, int16_t *out_value)
*buffer += 2;
}
inline static void
read_i16_to_i32(const uint8_t **buffer, int32_t *out_value)
{
int16_t v = (*(*buffer + 0) << 0) | // Byte 0
(*(*buffer + 1) << 8); // Byte 1
*out_value = v; // Properly sign extend.
*buffer += 2;
}
inline static void
read_i16_to_i32_neg(const uint8_t **buffer, int32_t *out_value)
{
int32_t v;
read_i16_to_i32(buffer, &v);
*out_value = -v;
}
inline static void
read_u32(const uint8_t **buffer, uint32_t *out_value)
{
@ -66,20 +83,20 @@ read_u32(const uint8_t **buffer, uint32_t *out_value)
}
static void
read_sensor(const uint8_t **buffer, struct psvr_sensor_sample *sample)
read_sample(const uint8_t **buffer, struct psvr_parsed_sample *sample)
{
// Tick.
read_u32(buffer, &sample->tick);
// Rotation.
read_i16(buffer, &sample->gyro[0]);
read_i16(buffer, &sample->gyro[1]);
read_i16(buffer, &sample->gyro[2]);
read_i16_to_i32(buffer, &sample->gyro.y);
read_i16_to_i32(buffer, &sample->gyro.x);
read_i16_to_i32_neg(buffer, &sample->gyro.z);
// Acceleration.
read_i16(buffer, &sample->accel[0]);
read_i16(buffer, &sample->accel[1]);
read_i16(buffer, &sample->accel[2]);
read_i16_to_i32(buffer, &sample->accel.y);
read_i16_to_i32(buffer, &sample->accel.x);
read_i16_to_i32_neg(buffer, &sample->accel.z);
}
@ -90,7 +107,7 @@ read_sensor(const uint8_t **buffer, struct psvr_sensor_sample *sample)
*/
bool
psvr_parse_sensor_packet(struct psvr_sensor_packet *packet,
psvr_parse_sensor_packet(struct psvr_parsed_sensor *sensor,
const uint8_t *buffer,
int size)
{
@ -101,47 +118,47 @@ psvr_parse_sensor_packet(struct psvr_sensor_packet *packet,
}
// Buttons.
read_u8(&buffer, &packet->buttons);
read_u8(&buffer, &sensor->buttons);
// Unknown, skip 1 bytes.
skip(&buffer, 1);
// Volume.
read_u16(&buffer, &packet->volume);
read_u16(&buffer, &sensor->volume);
// Unknown, skip 1 bytes.
skip(&buffer, 1);
// State.
read_u8(&buffer, &packet->state);
read_u8(&buffer, &sensor->state);
// Unknown, skip 10 bytes.
skip(&buffer, 10);
// Two sensors.
read_sensor(&buffer, &packet->samples[0]);
read_sensor(&buffer, &packet->samples[1]);
read_sample(&buffer, &sensor->samples[0]);
read_sample(&buffer, &sensor->samples[1]);
// unknown, skip 5 bytes.
skip(&buffer, 5);
// Raw button data.
read_u16(&buffer, &packet->button_raw);
read_u16(&buffer, &sensor->button_raw);
// Proximity, ~150 (nothing) to 1023 (headset is on).
read_u16(&buffer, &packet->proximity);
read_u16(&buffer, &sensor->proximity);
// Unknown, skip 6 bytes.
skip(&buffer, 6);
// Finally a sequence number.
read_u8(&buffer, &packet->seq);
read_u8(&buffer, &sensor->seq);
return (size_t)buffer - (size_t)start == 64;
}
bool
psvr_parse_status_packet(struct psvr_status_packet *packet,
psvr_parse_status_packet(struct psvr_parsed_status *status,
const uint8_t *buffer,
int size)
{
@ -155,22 +172,22 @@ psvr_parse_status_packet(struct psvr_status_packet *packet,
skip(&buffer, 4);
// Status bits.
read_u8(&buffer, &packet->status);
read_u8(&buffer, &status->status);
// Volume.
read_u8(&buffer, &packet->volume);
read_u8(&buffer, &status->volume);
// Unknown, 0x00, 0x00.
skip(&buffer, 2);
// Display time in minutes.
read_u8(&buffer, &packet->display_time);
read_u8(&buffer, &status->display_time);
// Unknown, 0xFF, 0x00.
skip(&buffer, 2);
// VR Mode Active.
read_u8(&buffer, &packet->vr_mode);
read_u8(&buffer, &status->vr_mode);
// Unknown, 0x12, 0x00...
skip(&buffer, 8);