mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-29 11:06:18 +00:00
d/psmv: Add support for ZCM2
This commit is contained in:
Jakob Bornecrantz
parent
e98fb81c56
commit
3d744c5959
|
|
@ -13,6 +13,7 @@
|
|||
#include <stdlib.h>
|
||||
#include <wchar.h>
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include "xrt/xrt_prober.h"
|
||||
|
||||
|
|
@ -143,6 +144,37 @@ struct psmv_vec3_f32_wire
|
|||
struct psmv_f32_wire z;
|
||||
};
|
||||
|
||||
/*!
|
||||
* Wire encoding of a single 16 bit integer, big endian.
|
||||
*
|
||||
* The values are unsigned 16-bit integers and stored as two's complement. The
|
||||
* values are shifted up to always report positive numbers. Subtract 0x8000 to
|
||||
* obtain signed values and determine direction from the sign.
|
||||
*
|
||||
* @ingroup drv_psmv
|
||||
*/
|
||||
struct psmv_u16_wire
|
||||
{
|
||||
uint8_t low;
|
||||
uint8_t high;
|
||||
};
|
||||
|
||||
/*!
|
||||
* Wire encoding of three 16 bit integers, big endian.
|
||||
*
|
||||
* The values are unsigned 16-bit integers and stored as two's complement. The
|
||||
* values are shifted up to always report positive numbers. Subtract 0x8000 to
|
||||
* obtain signed values and determine direction from the sign.
|
||||
*
|
||||
* @ingroup drv_psmv
|
||||
*/
|
||||
struct psmv_vec3_u16_wire
|
||||
{
|
||||
struct psmv_u16_wire x;
|
||||
struct psmv_u16_wire y;
|
||||
struct psmv_u16_wire z;
|
||||
};
|
||||
|
||||
/*!
|
||||
* Wire encoding of a single 16 bit integer, big endian.
|
||||
*
|
||||
|
|
@ -157,6 +189,8 @@ struct psmv_i16_wire
|
|||
/*!
|
||||
* Wire encoding of three 16 bit integers, big endian.
|
||||
*
|
||||
* The values are signed 16-bit integers and stored as two's complement.
|
||||
*
|
||||
* @ingroup drv_psmv
|
||||
*/
|
||||
struct psmv_vec3_i16_wire
|
||||
|
|
@ -166,29 +200,6 @@ struct psmv_vec3_i16_wire
|
|||
struct psmv_i16_wire z;
|
||||
};
|
||||
|
||||
/*!
|
||||
* Input package.
|
||||
*
|
||||
* @ingroup drv_psmv
|
||||
*/
|
||||
struct psmv_input_zcm1
|
||||
{
|
||||
uint8_t header;
|
||||
uint8_t buttons[4];
|
||||
uint8_t trigger_f1;
|
||||
uint8_t trigger_f2;
|
||||
uint8_t unknown[4];
|
||||
uint8_t timestamp_high;
|
||||
uint8_t battery;
|
||||
struct psmv_vec3_i16_wire accel_f1;
|
||||
struct psmv_vec3_i16_wire accel_f2;
|
||||
struct psmv_vec3_i16_wire gyro_f1;
|
||||
struct psmv_vec3_i16_wire gyro_f2;
|
||||
uint8_t temp_mag[6];
|
||||
uint8_t timestamp_low;
|
||||
uint8_t pad[49 - 44];
|
||||
};
|
||||
|
||||
/*!
|
||||
* Part of a calibration data, multiple packets make up a single data packet.
|
||||
*
|
||||
|
|
@ -211,26 +222,26 @@ struct psmv_calibration_zcm1
|
|||
uint8_t id;
|
||||
uint8_t which;
|
||||
uint16_t _pad0;
|
||||
struct psmv_vec3_i16_wire accel_max_z;
|
||||
struct psmv_vec3_i16_wire accel_min_x;
|
||||
struct psmv_vec3_i16_wire accel_min_z;
|
||||
struct psmv_vec3_i16_wire accel_max_x;
|
||||
struct psmv_vec3_i16_wire accel_max_y;
|
||||
struct psmv_vec3_i16_wire accel_min_y;
|
||||
struct psmv_vec3_u16_wire accel_max_z;
|
||||
struct psmv_vec3_u16_wire accel_min_x;
|
||||
struct psmv_vec3_u16_wire accel_min_z;
|
||||
struct psmv_vec3_u16_wire accel_max_x;
|
||||
struct psmv_vec3_u16_wire accel_max_y;
|
||||
struct psmv_vec3_u16_wire accel_min_y;
|
||||
uint16_t _pad1;
|
||||
struct psmv_vec3_i16_wire gyro_bias_0;
|
||||
struct psmv_vec3_u16_wire gyro_bias_0;
|
||||
uint16_t _pad2;
|
||||
struct psmv_vec3_i16_wire gyro_bias_1;
|
||||
struct psmv_vec3_u16_wire gyro_bias_1;
|
||||
uint8_t _pad3[7];
|
||||
uint8_t _pad4;
|
||||
uint16_t _pad5;
|
||||
uint16_t _pad6;
|
||||
uint16_t _pad7;
|
||||
struct psmv_vec3_i16_wire gyro_rot_x;
|
||||
struct psmv_vec3_u16_wire gyro_rot_x;
|
||||
uint16_t _pad8;
|
||||
struct psmv_vec3_i16_wire gyro_rot_y;
|
||||
struct psmv_vec3_u16_wire gyro_rot_y;
|
||||
uint16_t _pad9;
|
||||
struct psmv_vec3_i16_wire gyro_rot_z;
|
||||
struct psmv_vec3_u16_wire gyro_rot_z;
|
||||
uint16_t _pad10;
|
||||
struct psmv_vec3_f32_wire unknown_vec3;
|
||||
struct psmv_vec3_f32_wire gyro_fact;
|
||||
|
|
@ -239,6 +250,81 @@ struct psmv_calibration_zcm1
|
|||
uint8_t _pad[17];
|
||||
};
|
||||
|
||||
/*!
|
||||
* Calibration data, multiple packets goes into this.
|
||||
*
|
||||
* @ingroup drv_psmv
|
||||
*/
|
||||
struct psmv_calibration_zcm2
|
||||
{
|
||||
uint8_t id;
|
||||
uint8_t which;
|
||||
struct psmv_vec3_i16_wire accel_max_x;
|
||||
struct psmv_vec3_i16_wire accel_min_x;
|
||||
struct psmv_vec3_i16_wire accel_max_y;
|
||||
struct psmv_vec3_i16_wire accel_min_y;
|
||||
struct psmv_vec3_i16_wire accel_max_z;
|
||||
struct psmv_vec3_i16_wire accel_min_z;
|
||||
uint16_t _pad0;
|
||||
uint16_t _pad1;
|
||||
uint16_t _pad2;
|
||||
uint16_t _pad3;
|
||||
uint16_t _pad4;
|
||||
struct psmv_vec3_i16_wire gyro_pos_x;
|
||||
struct psmv_vec3_i16_wire gyro_pos_y;
|
||||
struct psmv_vec3_i16_wire gyro_pos_z;
|
||||
struct psmv_vec3_i16_wire gyro_neg_x;
|
||||
struct psmv_vec3_i16_wire gyro_neg_y;
|
||||
struct psmv_vec3_i16_wire gyro_neg_z;
|
||||
uint16_t _pad[6];
|
||||
};
|
||||
|
||||
/*!
|
||||
* Input package.
|
||||
*
|
||||
* @ingroup drv_psmv
|
||||
*/
|
||||
struct psmv_input_zcm1
|
||||
{
|
||||
uint8_t header;
|
||||
uint8_t buttons[4];
|
||||
uint8_t trigger_f1;
|
||||
uint8_t trigger_f2;
|
||||
uint8_t unknown[4];
|
||||
uint8_t timestamp_high;
|
||||
uint8_t battery;
|
||||
struct psmv_vec3_u16_wire accel_f1;
|
||||
struct psmv_vec3_u16_wire accel_f2;
|
||||
struct psmv_vec3_u16_wire gyro_f1;
|
||||
struct psmv_vec3_u16_wire gyro_f2;
|
||||
uint8_t temp_mag[6];
|
||||
uint8_t timestamp_low;
|
||||
uint8_t pad[49 - 44];
|
||||
};
|
||||
|
||||
/*!
|
||||
* Input package.
|
||||
*
|
||||
* @ingroup drv_psmv
|
||||
*/
|
||||
struct psmv_input_zcm2
|
||||
{
|
||||
uint8_t header;
|
||||
uint8_t buttons[4];
|
||||
uint8_t trigger;
|
||||
uint8_t trigger_low_pass;
|
||||
uint8_t pad0[4];
|
||||
uint8_t timestamp_high;
|
||||
uint8_t battery;
|
||||
struct psmv_vec3_i16_wire accel;
|
||||
struct psmv_vec3_i16_wire accel_copy;
|
||||
struct psmv_vec3_i16_wire gyro;
|
||||
struct psmv_vec3_i16_wire gyro_copy;
|
||||
uint8_t temp[2];
|
||||
uint8_t pad1[2];
|
||||
uint8_t timestamp_low;
|
||||
};
|
||||
|
||||
struct psmv_parsed_sample
|
||||
{
|
||||
struct xrt_vec3_i32 accel;
|
||||
|
|
@ -289,11 +375,22 @@ struct psmv_parsed_calibration_zcm1
|
|||
float unknown_float_0, unknown_float_1;
|
||||
};
|
||||
|
||||
struct psmv_parsed_calibration
|
||||
struct psmv_parsed_calibration_zcm2
|
||||
{
|
||||
struct psmv_parsed_calibration_zcm1 zcm1;
|
||||
};
|
||||
struct xrt_vec3_i32 accel_min_x;
|
||||
struct xrt_vec3_i32 accel_max_x;
|
||||
struct xrt_vec3_i32 accel_min_y;
|
||||
struct xrt_vec3_i32 accel_max_y;
|
||||
struct xrt_vec3_i32 accel_min_z;
|
||||
struct xrt_vec3_i32 accel_max_z;
|
||||
|
||||
struct xrt_vec3_i32 gyro_neg_x;
|
||||
struct xrt_vec3_i32 gyro_pos_x;
|
||||
struct xrt_vec3_i32 gyro_neg_y;
|
||||
struct xrt_vec3_i32 gyro_pos_y;
|
||||
struct xrt_vec3_i32 gyro_neg_z;
|
||||
struct xrt_vec3_i32 gyro_pos_z;
|
||||
};
|
||||
|
||||
/*!
|
||||
* A single PlayStation Move Controller.
|
||||
|
|
@ -334,6 +431,7 @@ struct psmv_device
|
|||
{
|
||||
union {
|
||||
struct psmv_parsed_calibration_zcm1 zcm1;
|
||||
struct psmv_parsed_calibration_zcm2 zcm2;
|
||||
};
|
||||
|
||||
struct
|
||||
|
|
@ -370,6 +468,9 @@ struct psmv_device
|
|||
struct xrt_vec3 gyro;
|
||||
} read;
|
||||
|
||||
// Product ID used to tell the difference between ZCM1 and ZCM2.
|
||||
uint16_t pid;
|
||||
|
||||
bool print_spew;
|
||||
bool print_debug;
|
||||
|
||||
|
|
@ -382,6 +483,21 @@ struct psmv_device
|
|||
};
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Pre-declare some functions.
|
||||
*
|
||||
*/
|
||||
|
||||
static int
|
||||
psmv_get_calibration(struct psmv_device *psmv);
|
||||
|
||||
static void
|
||||
psmv_parse_input(struct psmv_device *psmv,
|
||||
void *data,
|
||||
struct psmv_parsed_input *input);
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Internal functions.
|
||||
|
|
@ -423,11 +539,26 @@ psmv_clamp_zero_to_one_float_to_u8(float v)
|
|||
}
|
||||
|
||||
static void
|
||||
psmv_i32_from_i16_wire(int32_t *to, const struct psmv_i16_wire *from)
|
||||
psmv_i32_from_u16_wire(int32_t *to, const struct psmv_u16_wire *from)
|
||||
{
|
||||
*to = (from->low | from->high << 8) - 0x8000;
|
||||
}
|
||||
|
||||
static void
|
||||
psmv_i32_from_i16_wire(int32_t *to, const struct psmv_i16_wire *from)
|
||||
{
|
||||
*to = (int16_t)(from->low | from->high << 8);
|
||||
}
|
||||
|
||||
static void
|
||||
psmv_from_vec3_u16_wire(struct xrt_vec3_i32 *to,
|
||||
const struct psmv_vec3_u16_wire *from)
|
||||
{
|
||||
psmv_i32_from_u16_wire(&to->x, &from->x);
|
||||
psmv_i32_from_u16_wire(&to->y, &from->y);
|
||||
psmv_i32_from_u16_wire(&to->z, &from->z);
|
||||
}
|
||||
|
||||
static void
|
||||
psmv_from_vec3_i16_wire(struct xrt_vec3_i32 *to,
|
||||
const struct psmv_vec3_i16_wire *from)
|
||||
|
|
@ -518,14 +649,8 @@ psmv_read_one_packet(struct psmv_device *psmv, uint8_t *buffer, size_t size)
|
|||
return false;
|
||||
}
|
||||
|
||||
static void
|
||||
psmv_parse_input_zcm1(
|
||||
struct psmv_device *psmv,
|
||||
struct psmv_input_zcm1 *data,
|
||||
struct psmv_parsed_input *input);
|
||||
|
||||
|
||||
static void *psmv_run_thread(void *ptr)
|
||||
static void *
|
||||
psmv_run_thread(void *ptr)
|
||||
{
|
||||
struct psmv_device *psmv = (struct psmv_device *)ptr;
|
||||
struct time_state *time = time_state_create();
|
||||
|
|
@ -553,7 +678,7 @@ static void *psmv_run_thread(void *ptr)
|
|||
|
||||
timepoint_ns now_ns = time_state_get_now(time);
|
||||
|
||||
psmv_parse_input_zcm1(psmv, &data.input, &input);
|
||||
psmv_parse_input(psmv, data.buffer, &input);
|
||||
|
||||
float dt = time_ns_to_s(now_ns - then_ns);
|
||||
then_ns = now_ns;
|
||||
|
|
@ -586,7 +711,7 @@ psmv_send_led_control(struct psmv_device *psmv,
|
|||
{
|
||||
struct psmv_set_led msg;
|
||||
U_ZERO(&msg);
|
||||
msg.id = 0x02;
|
||||
msg.id = 0x06;
|
||||
msg.red = red;
|
||||
msg.green = green;
|
||||
msg.blue = blue;
|
||||
|
|
@ -636,9 +761,6 @@ psmv_led_and_trigger_update(struct psmv_device *psmv, int64_t time)
|
|||
psmv->state.led.b, psmv->state.rumble);
|
||||
}
|
||||
|
||||
static int
|
||||
psmv_get_calibration_zcm1(struct psmv_device *psmv);
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
|
|
@ -760,6 +882,12 @@ psmv_found(struct xrt_prober *xp,
|
|||
return 0;
|
||||
}
|
||||
|
||||
switch (devices[index]->product_id) {
|
||||
case PSMV_PID_ZCM1: break;
|
||||
case PSMV_PID_ZCM2: break;
|
||||
default: return -1;
|
||||
}
|
||||
|
||||
ret = xrt_prober_open_hid_interface(xp, devices[index], 0, &hid);
|
||||
if (ret != 0) {
|
||||
return -1;
|
||||
|
|
@ -776,6 +904,7 @@ psmv_found(struct xrt_prober *xp,
|
|||
psmv->base.set_output = psmv_device_set_output;
|
||||
psmv->base.name = XRT_DEVICE_PSMV;
|
||||
psmv->fusion.rot.w = 1.0f;
|
||||
psmv->pid = devices[index]->product_id;
|
||||
snprintf(psmv->base.str, XRT_DEVICE_NAME_LEN, "%s",
|
||||
"PS Move Controller");
|
||||
psmv->hid = hid;
|
||||
|
|
@ -822,7 +951,7 @@ psmv_found(struct xrt_prober *xp,
|
|||
}
|
||||
|
||||
// Get calibration data.
|
||||
ret = psmv_get_calibration_zcm1(psmv);
|
||||
ret = psmv_get_calibration(psmv);
|
||||
if (ret != 0) {
|
||||
PSMV_ERROR(psmv, "Failed to get calibration data!");
|
||||
psmv_device_destroy(&psmv->base);
|
||||
|
|
@ -843,6 +972,8 @@ psmv_found(struct xrt_prober *xp,
|
|||
// clang-format off
|
||||
u_var_add_root(psmv, "PSMV Controller", true);
|
||||
u_var_add_gui_header(psmv, &psmv->gui.calibration, "Calibration");
|
||||
switch (psmv->pid) {
|
||||
case PSMV_PID_ZCM1:
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_min_x, "zcm1.accel_min_x");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_max_x, "zcm1.accel_max_x");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_min_y, "zcm1.accel_min_y");
|
||||
|
|
@ -855,6 +986,23 @@ psmv_found(struct xrt_prober *xp,
|
|||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.gyro_bias_0, "zcm1.gyro_bias_0");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.gyro_bias_1, "zcm1.gyro_bias_1");
|
||||
u_var_add_vec3_f32(psmv, &psmv->calibration.zcm1.gyro_fact, "zcm1.gyro_fact");
|
||||
break;
|
||||
case PSMV_PID_ZCM2:
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.accel_min_x, "zcm2.accel_min_x");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.accel_max_x, "zcm2.accel_max_x");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.accel_min_y, "zcm2.accel_min_y");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.accel_max_y, "zcm2.accel_max_y");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.accel_min_z, "zcm2.accel_min_z");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.accel_max_z, "zcm2.accel_max_z");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.gyro_neg_x, "zcm2.gyro_neg_x");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.gyro_pos_x, "zcm2.gyro_pos_x");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.gyro_neg_y, "zcm2.gyro_neg_y");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.gyro_pos_y, "zcm2.gyro_pos_y");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.gyro_neg_z, "zcm2.gyro_neg_z");
|
||||
u_var_add_vec3_i32(psmv, &psmv->calibration.zcm2.gyro_pos_z, "zcm2.gyro_pos_z");
|
||||
break;
|
||||
default: assert(false);
|
||||
}
|
||||
u_var_add_vec3_f32(psmv, &psmv->calibration.accel.factor, "accel.factor");
|
||||
u_var_add_vec3_f32(psmv, &psmv->calibration.accel.bias, "accel.bias");
|
||||
u_var_add_vec3_f32(psmv, &psmv->calibration.gyro.factor, "gyro.factor");
|
||||
|
|
@ -930,17 +1078,17 @@ psmv_get_calibration_zcm1(struct psmv_device *psmv)
|
|||
sizeof(part) - src_offset);
|
||||
}
|
||||
|
||||
psmv_from_vec3_i16_wire(&zcm1->accel_min_x, &data.accel_min_x);
|
||||
psmv_from_vec3_i16_wire(&zcm1->accel_max_x, &data.accel_max_x);
|
||||
psmv_from_vec3_i16_wire(&zcm1->accel_min_y, &data.accel_min_y);
|
||||
psmv_from_vec3_i16_wire(&zcm1->accel_max_y, &data.accel_max_y);
|
||||
psmv_from_vec3_i16_wire(&zcm1->accel_min_z, &data.accel_min_z);
|
||||
psmv_from_vec3_i16_wire(&zcm1->accel_max_z, &data.accel_max_z);
|
||||
psmv_from_vec3_i16_wire(&zcm1->gyro_bias_0, &data.gyro_bias_0);
|
||||
psmv_from_vec3_i16_wire(&zcm1->gyro_bias_1, &data.gyro_bias_1);
|
||||
psmv_from_vec3_i16_wire(&zcm1->gyro_rot_x, &data.gyro_rot_x);
|
||||
psmv_from_vec3_i16_wire(&zcm1->gyro_rot_y, &data.gyro_rot_y);
|
||||
psmv_from_vec3_i16_wire(&zcm1->gyro_rot_z, &data.gyro_rot_z);
|
||||
psmv_from_vec3_u16_wire(&zcm1->accel_min_x, &data.accel_min_x);
|
||||
psmv_from_vec3_u16_wire(&zcm1->accel_max_x, &data.accel_max_x);
|
||||
psmv_from_vec3_u16_wire(&zcm1->accel_min_y, &data.accel_min_y);
|
||||
psmv_from_vec3_u16_wire(&zcm1->accel_max_y, &data.accel_max_y);
|
||||
psmv_from_vec3_u16_wire(&zcm1->accel_min_z, &data.accel_min_z);
|
||||
psmv_from_vec3_u16_wire(&zcm1->accel_max_z, &data.accel_max_z);
|
||||
psmv_from_vec3_u16_wire(&zcm1->gyro_bias_0, &data.gyro_bias_0);
|
||||
psmv_from_vec3_u16_wire(&zcm1->gyro_bias_1, &data.gyro_bias_1);
|
||||
psmv_from_vec3_u16_wire(&zcm1->gyro_rot_x, &data.gyro_rot_x);
|
||||
psmv_from_vec3_u16_wire(&zcm1->gyro_rot_y, &data.gyro_rot_y);
|
||||
psmv_from_vec3_u16_wire(&zcm1->gyro_rot_z, &data.gyro_rot_z);
|
||||
psmv_from_vec3_f32_wire(&zcm1->gyro_fact, &data.gyro_fact);
|
||||
psmv_from_vec3_f32_wire(&zcm1->unknown_vec3, &data.unknown_vec3);
|
||||
psmv_f32_from_wire(&zcm1->unknown_float_0, &data.unknown_float_0);
|
||||
|
|
@ -1043,8 +1191,7 @@ psmv_get_calibration_zcm1(struct psmv_device *psmv)
|
|||
}
|
||||
|
||||
static void
|
||||
psmv_parse_input_zcm1(
|
||||
struct psmv_device *psmv,
|
||||
psmv_parse_input_zcm1(struct psmv_device *psmv,
|
||||
struct psmv_input_zcm1 *data,
|
||||
struct psmv_parsed_input *input)
|
||||
{
|
||||
|
|
@ -1059,12 +1206,12 @@ psmv_parse_input_zcm1(
|
|||
input->timestamp |= data->timestamp_high << 8;
|
||||
|
||||
input->sample[0].trigger = data->trigger_f1;
|
||||
psmv_from_vec3_i16_wire(&input->sample[0].accel, &data->accel_f1);
|
||||
psmv_from_vec3_i16_wire(&input->sample[0].gyro, &data->gyro_f1);
|
||||
psmv_from_vec3_u16_wire(&input->sample[0].accel, &data->accel_f1);
|
||||
psmv_from_vec3_u16_wire(&input->sample[0].gyro, &data->gyro_f1);
|
||||
|
||||
input->sample[1].trigger = data->trigger_f2;
|
||||
psmv_from_vec3_i16_wire(&input->sample[1].accel, &data->accel_f2);
|
||||
psmv_from_vec3_i16_wire(&input->sample[1].gyro, &data->gyro_f2);
|
||||
psmv_from_vec3_u16_wire(&input->sample[1].accel, &data->accel_f2);
|
||||
psmv_from_vec3_u16_wire(&input->sample[1].gyro, &data->gyro_f2);
|
||||
|
||||
uint32_t diff = psmv_calc_delta_and_handle_rollover(
|
||||
input->timestamp, psmv->last.timestamp);
|
||||
|
|
@ -1098,3 +1245,251 @@ psmv_parse_input_zcm1(
|
|||
input->sample[0].gyro.z, input->sample[1].trigger,
|
||||
input->timestamp, diff, input->seq_no);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Packet functions ZCM2
|
||||
*
|
||||
*/
|
||||
|
||||
static int
|
||||
psmv_get_calibration_zcm2(struct psmv_device *psmv)
|
||||
{
|
||||
struct psmv_parsed_calibration_zcm2 *zcm2 = &psmv->calibration.zcm2;
|
||||
struct psmv_calibration_zcm2 data;
|
||||
uint8_t *dst = (uint8_t *)&data;
|
||||
int ret = 0;
|
||||
size_t src_offset, dst_offset;
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
struct psmv_calibration_part part = {0};
|
||||
uint8_t *src = (uint8_t *)∂
|
||||
|
||||
part.id = 0x10;
|
||||
|
||||
ret = os_hid_get_feature(psmv->hid, 0x10, src, sizeof(part));
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (ret != (int)sizeof(part)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
switch (part.which) {
|
||||
case 0x00:
|
||||
src_offset = 0;
|
||||
dst_offset = 0;
|
||||
break;
|
||||
case 0x81:
|
||||
src_offset = 2;
|
||||
dst_offset = sizeof(part);
|
||||
break;
|
||||
default: return -1;
|
||||
}
|
||||
|
||||
memcpy(dst + dst_offset, src + src_offset,
|
||||
sizeof(part) - src_offset);
|
||||
}
|
||||
|
||||
psmv_from_vec3_i16_wire(&zcm2->accel_min_x, &data.accel_min_x);
|
||||
psmv_from_vec3_i16_wire(&zcm2->accel_max_x, &data.accel_max_x);
|
||||
psmv_from_vec3_i16_wire(&zcm2->accel_min_y, &data.accel_min_y);
|
||||
psmv_from_vec3_i16_wire(&zcm2->accel_max_y, &data.accel_max_y);
|
||||
psmv_from_vec3_i16_wire(&zcm2->accel_min_z, &data.accel_min_z);
|
||||
psmv_from_vec3_i16_wire(&zcm2->accel_max_z, &data.accel_max_z);
|
||||
|
||||
psmv_from_vec3_i16_wire(&zcm2->gyro_neg_x, &data.gyro_neg_x);
|
||||
psmv_from_vec3_i16_wire(&zcm2->gyro_pos_x, &data.gyro_pos_x);
|
||||
psmv_from_vec3_i16_wire(&zcm2->gyro_neg_y, &data.gyro_neg_y);
|
||||
psmv_from_vec3_i16_wire(&zcm2->gyro_pos_y, &data.gyro_pos_y);
|
||||
psmv_from_vec3_i16_wire(&zcm2->gyro_neg_z, &data.gyro_neg_z);
|
||||
psmv_from_vec3_i16_wire(&zcm2->gyro_pos_z, &data.gyro_pos_z);
|
||||
|
||||
|
||||
/*
|
||||
* Acceleration
|
||||
*/
|
||||
|
||||
psmv->calibration.accel.factor.x =
|
||||
(zcm2->accel_max_x.x - zcm2->accel_min_x.x) / 2.0;
|
||||
psmv->calibration.accel.factor.y =
|
||||
(zcm2->accel_max_y.y - zcm2->accel_min_y.y) / 2.0;
|
||||
psmv->calibration.accel.factor.z =
|
||||
(zcm2->accel_max_z.z - zcm2->accel_min_z.z) / 2.0;
|
||||
|
||||
psmv->calibration.accel.bias.x =
|
||||
(zcm2->accel_min_y.x + zcm2->accel_max_y.x + zcm2->accel_min_z.x +
|
||||
zcm2->accel_max_z.x) /
|
||||
4.0;
|
||||
psmv->calibration.accel.bias.y =
|
||||
(zcm2->accel_min_x.y + zcm2->accel_max_x.y + zcm2->accel_min_z.y +
|
||||
zcm2->accel_max_z.y) /
|
||||
4.0;
|
||||
psmv->calibration.accel.bias.z =
|
||||
(zcm2->accel_min_x.z + zcm2->accel_max_x.z + zcm2->accel_min_y.z +
|
||||
zcm2->accel_max_y.z) /
|
||||
4.0;
|
||||
|
||||
|
||||
/*
|
||||
* Gyro
|
||||
*/
|
||||
|
||||
double gx = (zcm2->gyro_pos_x.x - zcm2->gyro_neg_x.x) / 2.0;
|
||||
double gy = (zcm2->gyro_pos_y.y - zcm2->gyro_neg_y.y) / 2.0;
|
||||
double gz = (zcm2->gyro_pos_z.z - zcm2->gyro_neg_z.z) / 2.0;
|
||||
|
||||
psmv->calibration.gyro.factor.x = (60.0 * gx) / (2.0 * M_PI * 90.0);
|
||||
psmv->calibration.gyro.factor.y = (60.0 * gy) / (2.0 * M_PI * 90.0);
|
||||
psmv->calibration.gyro.factor.z = (60.0 * gz) / (2.0 * M_PI * 90.0);
|
||||
|
||||
psmv->calibration.gyro.bias.x =
|
||||
(zcm2->gyro_neg_y.x + zcm2->gyro_pos_y.x + zcm2->gyro_neg_z.x +
|
||||
zcm2->gyro_pos_z.x) /
|
||||
4.0;
|
||||
psmv->calibration.gyro.bias.y =
|
||||
(zcm2->gyro_neg_x.y + zcm2->gyro_pos_x.y + zcm2->gyro_neg_z.y +
|
||||
zcm2->gyro_pos_z.y) /
|
||||
4.0;
|
||||
psmv->calibration.gyro.bias.z =
|
||||
(zcm2->gyro_neg_x.z + zcm2->gyro_pos_x.z + zcm2->gyro_neg_y.z +
|
||||
zcm2->gyro_pos_y.z) /
|
||||
4.0;
|
||||
|
||||
|
||||
/*
|
||||
* Print
|
||||
*/
|
||||
|
||||
PSMV_DEBUG(
|
||||
psmv,
|
||||
"Calibration:\n"
|
||||
"\taccel_min_x: %i %i %i\n"
|
||||
"\taccel_max_x: %i %i %i\n"
|
||||
"\taccel_min_y: %i %i %i\n"
|
||||
"\taccel_max_y: %i %i %i\n"
|
||||
"\taccel_min_z: %i %i %i\n"
|
||||
"\taccel_max_z: %i %i %i\n"
|
||||
"\tgyro_neg_x: %i %i %i\n"
|
||||
"\tgyro_pos_x: %i %i %i\n"
|
||||
"\tgyro_neg_y: %i %i %i\n"
|
||||
"\tgyro_pos_y: %i %i %i\n"
|
||||
"\tgyro_neg_z: %i %i %i\n"
|
||||
"\tgyro_pos_z: %i %i %i\n"
|
||||
"Calculated:\n"
|
||||
"\taccel.factor: %f %f %f\n"
|
||||
"\taccel.bias: %f %f %f\n"
|
||||
"\tgyro.factor: %f %f %f\n"
|
||||
"\tgyro.bias: %f %f %f\n",
|
||||
zcm2->accel_min_x.x, zcm2->accel_min_x.y, zcm2->accel_min_x.z,
|
||||
zcm2->accel_max_x.x, zcm2->accel_max_x.y, zcm2->accel_max_x.z,
|
||||
zcm2->accel_min_y.x, zcm2->accel_min_y.y, zcm2->accel_min_y.z,
|
||||
zcm2->accel_max_y.x, zcm2->accel_max_y.y, zcm2->accel_max_y.z,
|
||||
zcm2->accel_min_z.x, zcm2->accel_min_z.y, zcm2->accel_min_z.z,
|
||||
zcm2->accel_max_z.x, zcm2->accel_max_z.y, zcm2->accel_max_z.z,
|
||||
zcm2->gyro_neg_x.x, zcm2->gyro_neg_x.y, zcm2->gyro_neg_x.z,
|
||||
zcm2->gyro_pos_x.x, zcm2->gyro_pos_x.y, zcm2->gyro_pos_x.z,
|
||||
zcm2->gyro_neg_y.x, zcm2->gyro_neg_y.y, zcm2->gyro_neg_y.z,
|
||||
zcm2->gyro_pos_y.x, zcm2->gyro_pos_y.y, zcm2->gyro_pos_y.z,
|
||||
zcm2->gyro_neg_z.x, zcm2->gyro_neg_z.y, zcm2->gyro_neg_z.z,
|
||||
zcm2->gyro_pos_z.x, zcm2->gyro_pos_z.y, zcm2->gyro_pos_z.z,
|
||||
psmv->calibration.accel.factor.x, psmv->calibration.accel.factor.y,
|
||||
psmv->calibration.accel.factor.z, psmv->calibration.accel.bias.x,
|
||||
psmv->calibration.accel.bias.y, psmv->calibration.accel.bias.z,
|
||||
psmv->calibration.gyro.factor.x, psmv->calibration.gyro.factor.y,
|
||||
psmv->calibration.gyro.factor.z, psmv->calibration.gyro.bias.x,
|
||||
psmv->calibration.gyro.bias.y, psmv->calibration.gyro.bias.z);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void
|
||||
psmv_parse_input_zcm2(struct psmv_device *psmv,
|
||||
struct psmv_input_zcm2 *data,
|
||||
struct psmv_parsed_input *input)
|
||||
{
|
||||
input->battery = data->battery;
|
||||
input->seq_no = data->buttons[3] & 0x0f;
|
||||
|
||||
input->buttons |= data->buttons[0] << 24;
|
||||
input->buttons |= data->buttons[1] << 16;
|
||||
input->buttons |= data->buttons[2] << 8;
|
||||
input->buttons |= data->buttons[3] & 0xf0;
|
||||
input->timestamp |= data->timestamp_low;
|
||||
input->timestamp |= data->timestamp_high << 8;
|
||||
|
||||
input->sample[0].trigger = data->trigger_low_pass;
|
||||
input->sample[1].trigger = data->trigger;
|
||||
|
||||
psmv_from_vec3_i16_wire(&input->sample[0].accel, &data->accel);
|
||||
psmv_from_vec3_i16_wire(&input->sample[0].gyro, &data->gyro);
|
||||
|
||||
psmv_from_vec3_i16_wire(&input->sample[1].accel, &data->accel_copy);
|
||||
psmv_from_vec3_i16_wire(&input->sample[1].gyro, &data->gyro_copy);
|
||||
|
||||
uint32_t diff = psmv_calc_delta_and_handle_rollover(
|
||||
input->timestamp, psmv->last.timestamp);
|
||||
bool missed = input->seq_no != ((psmv->last.seq_no + 1) & 0x0f);
|
||||
|
||||
|
||||
/*
|
||||
* Print
|
||||
*/
|
||||
|
||||
PSMV_SPEW(psmv,
|
||||
"\n\t"
|
||||
"missed: %s\n\t"
|
||||
"buttons: %08x\n\t"
|
||||
"battery: %x\n\t"
|
||||
"sample[0].accel: %6i %6i %6i\n\t"
|
||||
"sample[1].accel: %6i %6i %6i\n\t"
|
||||
"sample[0].gyro: %6i %6i %6i\n\t"
|
||||
"sample[1].gyro: %6i %6i %6i\n\t"
|
||||
"sample[0].trigger: %02x\n\t"
|
||||
"sample[1].trigger: %02x\n\t"
|
||||
"timestamp: %i\n\t"
|
||||
"diff: %i\n\t"
|
||||
"seq_no: %x\n",
|
||||
missed ? "yes" : "no", input->buttons, input->battery,
|
||||
input->sample[0].accel.x, input->sample[0].accel.y,
|
||||
input->sample[0].accel.z, input->sample[1].accel.x,
|
||||
input->sample[1].accel.y, input->sample[1].accel.z,
|
||||
input->sample[0].gyro.x, input->sample[0].gyro.y,
|
||||
input->sample[0].gyro.z, input->sample[1].gyro.x,
|
||||
input->sample[1].gyro.y, input->sample[1].gyro.z,
|
||||
input->sample[0].trigger, input->sample[1].trigger,
|
||||
input->timestamp, diff, input->seq_no);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Small dispatch functions.
|
||||
*
|
||||
*/
|
||||
|
||||
static int
|
||||
psmv_get_calibration(struct psmv_device *psmv)
|
||||
{
|
||||
switch (psmv->pid) {
|
||||
case PSMV_PID_ZCM1: return psmv_get_calibration_zcm1(psmv);
|
||||
case PSMV_PID_ZCM2: return psmv_get_calibration_zcm2(psmv);
|
||||
default: return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void
|
||||
psmv_parse_input(struct psmv_device *psmv,
|
||||
void *data,
|
||||
struct psmv_parsed_input *input)
|
||||
{
|
||||
switch (psmv->pid) {
|
||||
case PSMV_PID_ZCM1: psmv_parse_input_zcm1(psmv, data, input); break;
|
||||
case PSMV_PID_ZCM2: psmv_parse_input_zcm2(psmv, data, input); break;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,7 +22,8 @@ extern "C" {
|
|||
|
||||
|
||||
#define PSMV_VID 0x054c
|
||||
#define PSMV_PID 0x03d5
|
||||
#define PSMV_PID_ZCM1 0x03d5
|
||||
#define PSMV_PID_ZCM2 0x0c5e
|
||||
|
||||
/*!
|
||||
* Probing function for the PS Move devices.
|
||||
|
|
|
|||
|
|
@ -50,7 +50,8 @@
|
|||
*/
|
||||
struct xrt_prober_entry target_entry_list[] = {
|
||||
#ifdef XRT_BUILD_PSMV
|
||||
{PSMV_VID, PSMV_PID, psmv_found, "PS Move"},
|
||||
{PSMV_VID, PSMV_PID_ZCM1, psmv_found, "PS Move Controller (ZCM1)"},
|
||||
{PSMV_VID, PSMV_PID_ZCM2, psmv_found, "PS Move Controller (ZCM2)"},
|
||||
#endif // XRT_BUILD_PSMV
|
||||
|
||||
#ifdef XRT_BUILD_HYDRA
|
||||
|
|
|
|||
Loading…
Reference in a new issue