diff --git a/src/xrt/drivers/psmv/psmv_driver.c b/src/xrt/drivers/psmv/psmv_driver.c index 074098287..9938f1d40 100644 --- a/src/xrt/drivers/psmv/psmv_driver.c +++ b/src/xrt/drivers/psmv/psmv_driver.c @@ -13,6 +13,7 @@ #include #include #include +#include #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) @@ -452,7 +583,7 @@ psmv_f32_from_wire(float *to, const struct psmv_f32_wire *from) static void psmv_from_vec3_f32_wire(struct xrt_vec3 *to, - const struct psmv_vec3_f32_wire *from) + const struct psmv_vec3_f32_wire *from) { psmv_f32_from_wire(&to->x, &from->x); psmv_f32_from_wire(&to->y, &from->y); @@ -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,18 +972,37 @@ 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"); - 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"); - u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_max_y, "zcm1.accel_max_y"); - u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_min_z, "zcm1.accel_min_z"); - u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_max_z, "zcm1.accel_max_z"); - u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.gyro_rot_x, "zcm1.gyro_rot_x"); - u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.gyro_rot_y, "zcm1.gyro_rot_y"); - u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.gyro_rot_z, "zcm1.gyro_rot_z"); - 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"); + 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"); + u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_max_y, "zcm1.accel_max_y"); + u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_min_z, "zcm1.accel_min_z"); + u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.accel_max_z, "zcm1.accel_max_z"); + u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.gyro_rot_x, "zcm1.gyro_rot_x"); + u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.gyro_rot_y, "zcm1.gyro_rot_y"); + u_var_add_vec3_i32(psmv, &psmv->calibration.zcm1.gyro_rot_z, "zcm1.gyro_rot_z"); + 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,10 +1191,9 @@ psmv_get_calibration_zcm1(struct psmv_device *psmv) } static void -psmv_parse_input_zcm1( - struct psmv_device *psmv, - struct psmv_input_zcm1 *data, - struct psmv_parsed_input *input) +psmv_parse_input_zcm1(struct psmv_device *psmv, + struct psmv_input_zcm1 *data, + struct psmv_parsed_input *input) { input->battery = data->battery; input->seq_no = data->buttons[3] & 0x0f; @@ -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; + } +} diff --git a/src/xrt/drivers/psmv/psmv_interface.h b/src/xrt/drivers/psmv/psmv_interface.h index f39c8c150..d46dd06c3 100644 --- a/src/xrt/drivers/psmv/psmv_interface.h +++ b/src/xrt/drivers/psmv/psmv_interface.h @@ -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. diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index e5f2839cf..afe5d5139 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -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