mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-25 16:11:45 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity
monado/src/xrt/drivers/psmv/psmv_driver.c
Jakob Bornecrantz a10e7fe968 d/psmv: Move led and rumble update to thread
2020-04-25 16:24:28 +01:00

1750 lines
49 KiB
C
Raw Blame History

// Copyright 2019-2020, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief PlayStation Move motion controller prober and driver code.
* @author Jakob Bornecrantz <jakob@collabora.com>
* @author Ryan Pavlik <ryan.pavlik@collabora.com>
* @ingroup drv_psmv
*/
#include "xrt/xrt_prober.h"
#include "xrt/xrt_tracking.h"
#include "os/os_threading.h"
#include "os/os_hid.h"
#include "os/os_time.h"
#include "math/m_api.h"
#include "math/m_imu_pre.h"
#include "tracking/t_imu.h"
#include "util/u_var.h"
#include "util/u_time.h"
#include "util/u_misc.h"
#include "util/u_debug.h"
#include "util/u_device.h"
#include "psmv_interface.h"
#include <stdio.h>
#include <math.h>
#include <assert.h>
/*!
* @ingroup drv_psmv
* @{
*/
/*
*
* Defines & structs.
*
*/
#define PSMV_SPEW(p, ...) \
do { \
if (p->print_spew) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define PSMV_DEBUG(p, ...) \
do { \
if (p->print_debug) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define PSMV_ERROR(p, ...) \
do { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} while (false)
DEBUG_GET_ONCE_BOOL_OPTION(psmv_spew, "PSMV_PRINT_SPEW", false)
DEBUG_GET_ONCE_BOOL_OPTION(psmv_debug, "PSMV_PRINT_DEBUG", false)
/*!
* Indices where each input is in the input list.
*/
enum psmv_input_index
{
PSMV_INDEX_PS_CLICK,
PSMV_INDEX_MOVE_CLICK,
PSMV_INDEX_START_CLICK,
PSMV_INDEX_SELECT_CLICK,
PSMV_INDEX_SQUARE_CLICK,
PSMV_INDEX_CROSS_CLICK,
PSMV_INDEX_CIRCLE_CLICK,
PSMV_INDEX_TRIANGLE_CLICK,
PSMV_INDEX_TRIGGER_VALUE,
PSMV_INDEX_BODY_CENTER_POSE,
PSMV_INDEX_BALL_CENTER_POSE,
PSMV_INDEX_BALL_TIP_POSE,
};
/*!
* Mask for the button in the button uint32_t.
*/
enum psmv_button_bit
{
// clang-format off
PSMV_BUTTON_BIT_MOVE_F2 = (1 << 6),
PSMV_BUTTON_BIT_TRIGGER_F2 = (1 << 7),
PSMV_BUTTON_BIT_PS = (1 << 8),
PSMV_BUTTON_BIT_MOVE_F1 = (1 << 11),
PSMV_BUTTON_BIT_TRIGGER_F1 = (1 << 12),
PSMV_BUTTON_BIT_TRIANGLE = (1 << 20),
PSMV_BUTTON_BIT_CIRCLE = (1 << 21),
PSMV_BUTTON_BIT_CROSS = (1 << 22),
PSMV_BUTTON_BIT_SQUARE = (1 << 23),
PSMV_BUTTON_BIT_START = (1 << 27),
PSMV_BUTTON_BIT_SELECT = (1 << 24),
PSMV_BUTTON_BIT_MOVE_ANY = PSMV_BUTTON_BIT_MOVE_F1 |
PSMV_BUTTON_BIT_MOVE_F2,
PSMV_BUTTON_BIT_TRIGGER_ANY = PSMV_BUTTON_BIT_TRIGGER_F1 |
PSMV_BUTTON_BIT_TRIGGER_F2,
// clang-format on
};
/*!
* Led setting packet.
*/
struct psmv_set_led
{
uint8_t id;
uint8_t zero;
uint8_t red;
uint8_t green;
uint8_t blue;
uint8_t _unknown;
uint8_t rumble;
uint8_t _pad[49 - 7];
};
/*!
* Wire encoding of a single 32 bit float, "little" endian.
*/
struct psmv_f32_wire
{
uint8_t val[4];
};
/*!
* Wire encoding of three 32 bit float, "little" endian.
*/
struct psmv_vec3_f32_wire
{
struct psmv_f32_wire x;
struct psmv_f32_wire y;
struct psmv_f32_wire z;
};
/*!
* Wire encoding of a single 16 bit integer, little 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.
*/
struct psmv_u16_wire
{
uint8_t low;
uint8_t high;
};
/*!
* Wire encoding of three 16 bit integers, little 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.
*/
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, little endian.
*/
struct psmv_i16_wire
{
uint8_t low;
uint8_t high;
};
/*!
* Wire encoding of three 16 bit integers, little endian.
*
* The values are signed 16-bit integers and stored as two's complement.
*/
struct psmv_vec3_i16_wire
{
struct psmv_i16_wire x;
struct psmv_i16_wire y;
struct psmv_i16_wire z;
};
/*!
* Part of a calibration data, multiple packets make up a single data packet.
*/
struct psmv_calibration_part
{
uint8_t id;
uint8_t which;
uint8_t data[49 - 2];
};
/*!
* Calibration data, multiple packets goes into this.
*/
struct psmv_calibration_zcm1
{
uint8_t id;
uint8_t which;
uint8_t _pad0[2];
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;
uint8_t _pad1[2];
struct psmv_vec3_u16_wire gyro_bias_0;
uint8_t _pad2[2];
struct psmv_vec3_u16_wire gyro_bias_1;
uint8_t _pad3[7];
uint8_t _pad4;
uint8_t _pad5[2];
uint8_t _pad6[2];
uint8_t _pad7[2];
struct psmv_vec3_u16_wire gyro_rot_x;
uint8_t _pad8[2];
struct psmv_vec3_u16_wire gyro_rot_y;
uint8_t _pad9[2];
struct psmv_vec3_u16_wire gyro_rot_z;
uint8_t _pad10[2];
struct psmv_vec3_f32_wire unknown_vec3;
struct psmv_vec3_f32_wire gyro_fact;
struct psmv_f32_wire unknown_float_0;
struct psmv_f32_wire unknown_float_1;
uint8_t _pad[17];
};
/*!
* Parsed calibration data from a ZCM1 device.
*/
struct psmv_parsed_calibration_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;
/*!
* From: https://github.com/nitsch/moveonpc/wiki/Calibration-data
*
* Coded as the one before. The values are very near to 1.0.
*
* I observed, that when I multiply this vector with the gyro bias
* vector before subtracting from the gyro 80rpm measures, I get a
* better calibration.
*
* So in order to get the accurate 80rpm measures:
* GyroMeasure80rpm-(GyroBias1*UnknownVector2) or
* GyroMeasure80rpm-(GyroBias2*UnknownVector2)
*/
struct xrt_vec3 gyro_fact;
struct xrt_vec3_i32 gyro_bias_0;
struct xrt_vec3_i32 gyro_bias_1;
struct xrt_vec3_i32 gyro_rot_x;
struct xrt_vec3_i32 gyro_rot_y;
struct xrt_vec3_i32 gyro_rot_z;
struct xrt_vec3 unknown_vec3;
float unknown_float_0, unknown_float_1;
};
/*!
* Calibration data, multiple packets goes into this.
*/
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;
//! Pretty sure this is gryo bias.
struct psmv_vec3_i16_wire gyro_bias;
uint8_t _pad0[4];
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;
uint8_t _pad1[12];
};
/*!
* Parsed calibration data from a ZCM2 device.
*/
struct psmv_parsed_calibration_zcm2
{
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;
//! Pretty sure this is gryo bias.
struct xrt_vec3_i32 gyro_bias;
};
/*!
* Input package for ZCM1.
*/
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 for ZCM2.
*/
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_copy;
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 timestamp_low;
uint8_t timestamp_high;
uint8_t pad1[2];
uint8_t timestamp_low_copy;
};
/*!
* A parsed sample of accel and gyro.
*/
struct psmv_parsed_sample
{
struct xrt_vec3_i32 accel;
struct xrt_vec3_i32 gyro;
};
/*!
* A parsed input packet.
*/
struct psmv_parsed_input
{
uint32_t buttons;
uint16_t timestamp;
uint16_t timestamp_copy;
uint8_t battery;
uint8_t seq_no;
union {
//! Trigger for the last two frames (ZCM1).
uint8_t trigger_values[2];
struct
{
//! Low-pass filtered version of trigger (ZCM2).
uint8_t trigger_low_pass;
//! Trigger (ZCM2).
uint8_t trigger;
};
};
union {
//! Accelerometer and gyro scope samples (ZCM1).
struct psmv_parsed_sample samples[2];
struct
{
//! Accelerometer and gyro scope samples (ZCM2).
struct psmv_parsed_sample sample;
//! Copy of above (ZCM2).
struct psmv_parsed_sample sample_copy;
};
};
};
/*!
* A single PlayStation Move Controller.
*
* A note about coordinate system. If you stand the controller in front of you
* so that the ball is pointing upward, buttons towards you. Then think of the
* ball as a head that is looking away from you. The buttons then are is it's
* back, the trigger the front.
*
* Translated to axis that means the ball is on the Y+ axis, the buttons on the
* Z+ axis, the trigger on the Z- axis, the USB port on the Y- axis, the start
* button on the X+ axis, select button on the X- axis.
*/
struct psmv_device
{
struct xrt_device base;
struct os_hid_device *hid;
struct xrt_tracked_psmv *ball;
struct os_thread_helper oth;
struct
{
int64_t resend_time;
struct xrt_colour_rgb_u8 led;
uint8_t rumble;
} wants; //!< What should be set.
struct
{
struct xrt_colour_rgb_u8 led;
uint8_t rumble;
} state; //!< What is currently set on the device.
struct
{
union {
struct psmv_parsed_calibration_zcm1 zcm1;
struct psmv_parsed_calibration_zcm2 zcm2;
};
struct m_imu_pre_filter prefilter;
} calibration;
struct
{
//! Lock for last and fusion.
struct os_mutex lock;
//! Last sensor read.
struct psmv_parsed_input last;
struct
{
struct xrt_quat rot;
struct xrt_vec3 rotvec;
struct imu_fusion *fusion;
struct
{
struct xrt_vec3 accel;
struct xrt_vec3 gyro;
} variance;
} fusion;
};
struct
{
//! Last adjusted accelerator value.
struct xrt_vec3 accel;
//! Last adjusted gyro value.
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;
struct
{
bool control;
bool calibration;
bool last_frame;
bool fusion;
} gui;
};
/*
*
* Pre-declare some functions.
*
*/
static int
psmv_get_calibration(struct psmv_device *psmv);
static int
psmv_parse_input(struct psmv_device *psmv,
void *data,
struct psmv_parsed_input *input);
/*
*
* Smaller helper functions.
*
*/
static inline struct psmv_device *
psmv_device(struct xrt_device *xdev)
{
return (struct psmv_device *)xdev;
}
static uint32_t
psmv_calc_delta_and_handle_rollover(uint32_t next, uint32_t last)
{
uint32_t tick_delta = next - last;
// The 16-bit tick counter has rolled over,
// adjust the "negative" value to be positive.
if (tick_delta > 0xffff) {
tick_delta += 0x10000;
}
return tick_delta;
}
static inline uint8_t
psmv_clamp_zero_to_one_float_to_u8(float v)
{
float vf = v * 255.0f;
if (vf >= 255.0f) {
return 0xff;
}
if (vf >= 0.0f) {
return (uint8_t)vf;
}
return 0x00;
}
static void
psmv_update_input_click(struct psmv_device *psmv,
int index,
int64_t now,
uint32_t bit)
{
psmv->base.inputs[index].timestamp = now;
psmv->base.inputs[index].value.boolean =
(psmv->last.buttons & bit) != 0;
}
static void
psmv_update_trigger_value(struct psmv_device *psmv, int index, int64_t now)
{
psmv->base.inputs[index].timestamp = now;
psmv->base.inputs[index].value.vec1.x = psmv->last.trigger / 255.0f;
}
/*
*
* Internal functions.
*
*/
/*!
* Does the actual sending of the led control package to the device.
*/
static int
psmv_send_led_control(struct psmv_device *psmv,
uint8_t red,
uint8_t green,
uint8_t blue,
uint8_t rumble)
{
struct psmv_set_led msg;
U_ZERO(&msg);
msg.id = 0x06;
msg.red = red;
msg.green = green;
msg.blue = blue;
msg.rumble = rumble;
return os_hid_write(psmv->hid, (uint8_t *)&msg, sizeof(msg));
}
static void
psmv_led_and_trigger_update_locked(struct psmv_device *psmv, int64_t time)
{
// Need to keep sending led control packets to keep the leds on.
if (psmv->wants.resend_time > time &&
psmv->state.led.r == psmv->wants.led.r &&
psmv->state.led.g == psmv->wants.led.g &&
psmv->state.led.b == psmv->wants.led.b &&
psmv->state.rumble == psmv->wants.rumble) {
return;
}
psmv->state.led.r = psmv->wants.led.r;
psmv->state.led.g = psmv->wants.led.g;
psmv->state.led.b = psmv->wants.led.b;
psmv->state.rumble = psmv->wants.rumble;
psmv->wants.resend_time = time + 1000000000;
psmv_send_led_control(psmv, psmv->state.led.r, psmv->state.led.g,
psmv->state.led.b, psmv->state.rumble);
}
static void
psmv_led_and_trigger_update(struct psmv_device *psmv, int64_t time)
{
os_mutex_lock(&psmv->lock);
psmv_led_and_trigger_update_locked(psmv, time);
os_mutex_unlock(&psmv->lock);
}
static void
update_fusion(struct psmv_device *psmv,
struct psmv_parsed_sample *sample,
timepoint_ns timestamp_ns,
time_duration_ns delta_ns)
{
struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
(void)mag;
struct xrt_vec3_i32 *ra = &sample->accel;
struct xrt_vec3_i32 *rg = &sample->gyro;
m_imu_pre_filter_data(&psmv->calibration.prefilter, ra, rg,
&psmv->read.accel, &psmv->read.gyro);
if (psmv->ball != NULL) {
// We have positional tracking
struct xrt_tracking_sample sample;
sample.accel_m_s2 = psmv->read.accel;
sample.gyro_rad_secs = psmv->read.gyro;
xrt_tracked_psmv_push_imu(psmv->ball, timestamp_ns, &sample);
} else {
// Orientation-only tracking
#if 0
// Super simple fusion.
math_quat_integrate_velocity(
&psmv->fusion.rot, &psmv->read.gyro, dt, &psmv->fusion.rot);
#else
imu_fusion_incorporate_gyros_and_accelerometer(
psmv->fusion.fusion, timestamp_ns, &psmv->read.gyro,
&psmv->fusion.variance.gyro, &psmv->read.accel,
&psmv->fusion.variance.accel, NULL);
struct xrt_vec3 angvel_dummy;
imu_fusion_get_prediction(psmv->fusion.fusion, timestamp_ns,
&psmv->fusion.rot, &angvel_dummy);
imu_fusion_get_prediction_rotation_vec(
psmv->fusion.fusion, timestamp_ns, &psmv->fusion.rotvec);
#endif
}
}
/*!
* Reads one packet from the device, handles time out, locking and checking if
* the thread has been told to shut down.
*/
static bool
psmv_read_one_packet(struct psmv_device *psmv, uint8_t *buffer, size_t size)
{
os_thread_helper_lock(&psmv->oth);
while (os_thread_helper_is_running_locked(&psmv->oth)) {
os_thread_helper_unlock(&psmv->oth);
int ret = os_hid_read(psmv->hid, buffer, size, 1000);
if (ret == 0) {
fprintf(stderr, "%s\n", __func__);
// Must lock thread before check in while.
os_thread_helper_lock(&psmv->oth);
continue;
}
if (ret < 0) {
PSMV_ERROR(psmv, "Failed to read device '%i'!", ret);
return false;
}
return true;
}
return false;
}
static void *
psmv_run_thread(void *ptr)
{
struct psmv_device *psmv = (struct psmv_device *)ptr;
union {
uint8_t buffer[256];
struct psmv_input_zcm1 input;
} data;
struct psmv_parsed_input input = {0};
while (os_hid_read(psmv->hid, data.buffer, sizeof(data), 0) > 0) {
// Empty queue first
}
// Now wait for a package to sync up, it's discarded but that's okay.
if (!psmv_read_one_packet(psmv, data.buffer, sizeof(data))) {
return NULL;
}
timepoint_ns then_ns = os_monotonic_get_ns();
while (psmv_read_one_packet(psmv, data.buffer, sizeof(data))) {
timepoint_ns now_ns = os_monotonic_get_ns();
int num = psmv_parse_input(psmv, data.buffer, &input);
time_duration_ns delta_ns = now_ns - then_ns;
then_ns = now_ns;
// Lock last and the fusion.
os_mutex_lock(&psmv->lock);
// Make sure the leds stays on.
psmv_led_and_trigger_update_locked(psmv, now_ns);
// Copy to device.
psmv->last = input;
// Process the parsed data.
if (num == 2) {
// ZCM1
update_fusion(psmv, &input.samples[0],
now_ns - (delta_ns / 2.0),
(delta_ns / 2.0));
update_fusion(psmv, &input.samples[1], now_ns,
(delta_ns / 2.0));
} else if (num == 1) {
// ZCM2
update_fusion(psmv, &input.sample, now_ns, delta_ns);
} else {
assert(false);
}
// Now done.
os_mutex_unlock(&psmv->lock);
}
return NULL;
}
static void
psmv_get_fusion_pose(struct psmv_device *psmv,
enum xrt_input_name name,
timepoint_ns when,
struct xrt_space_relation *out_relation)
{
out_relation->pose.orientation = psmv->fusion.rot;
//! @todo assuming that orientation is actually currently tracked.
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
}
/*
*
* Device functions.
*
*/
static void
psmv_device_destroy(struct xrt_device *xdev)
{
struct psmv_device *psmv = psmv_device(xdev);
// Destroy the thread object.
os_thread_helper_destroy(&psmv->oth);
// Now that the thread is not running we can destroy the lock.
os_mutex_destroy(&psmv->lock);
// Destroy the IMU fusion.
imu_fusion_destroy(psmv->fusion.fusion);
// Remove the variable tracking.
u_var_remove_root(psmv);
// Includes null check, and sets to null.
xrt_tracked_psmv_destroy(&psmv->ball);
if (psmv->hid != NULL) {
psmv_send_led_control(psmv, 0x00, 0x00, 0x00, 0x00);
os_hid_destroy(psmv->hid);
psmv->hid = NULL;
}
free(psmv);
}
static void
psmv_device_update_inputs(struct xrt_device *xdev)
{
struct psmv_device *psmv = psmv_device(xdev);
int64_t now = os_monotonic_get_ns();
// Lock the data.
os_mutex_lock(&psmv->lock);
// clang-format off
psmv_update_input_click(psmv, PSMV_INDEX_PS_CLICK, now, PSMV_BUTTON_BIT_PS);
psmv_update_input_click(psmv, PSMV_INDEX_MOVE_CLICK, now, PSMV_BUTTON_BIT_MOVE_ANY);
psmv_update_input_click(psmv, PSMV_INDEX_START_CLICK, now, PSMV_BUTTON_BIT_START);
psmv_update_input_click(psmv, PSMV_INDEX_SELECT_CLICK, now, PSMV_BUTTON_BIT_SELECT);
psmv_update_input_click(psmv, PSMV_INDEX_SQUARE_CLICK, now, PSMV_BUTTON_BIT_SQUARE);
psmv_update_input_click(psmv, PSMV_INDEX_CROSS_CLICK, now, PSMV_BUTTON_BIT_CROSS);
psmv_update_input_click(psmv, PSMV_INDEX_CIRCLE_CLICK, now, PSMV_BUTTON_BIT_CIRCLE);
psmv_update_input_click(psmv, PSMV_INDEX_TRIANGLE_CLICK, now, PSMV_BUTTON_BIT_TRIANGLE);
psmv_update_trigger_value(psmv, PSMV_INDEX_TRIGGER_VALUE, now);
// Only report the ball as active if we can track it.
psmv->base.inputs[PSMV_INDEX_BALL_CENTER_POSE].active = psmv->ball != NULL;
// clang-format on
// Done now.
os_mutex_unlock(&psmv->lock);
}
static void
psmv_device_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t at_timestamp_ns,
uint64_t *out_relation_timestamp_ns,
struct xrt_space_relation *out_relation)
{
struct psmv_device *psmv = psmv_device(xdev);
//! @todo transform pose based on input.
// We have no tracking, don't return a position.
if (psmv->ball != NULL) {
xrt_tracked_psmv_get_tracked_pose(
psmv->ball, name, at_timestamp_ns, out_relation);
*out_relation_timestamp_ns = at_timestamp_ns;
} else {
uint64_t now = os_monotonic_get_ns();
psmv_get_fusion_pose(psmv, name, now, out_relation);
*out_relation_timestamp_ns = now;
}
}
static void
psmv_device_set_output(struct xrt_device *xdev,
enum xrt_output_name name,
union xrt_output_value *value)
{
struct psmv_device *psmv = psmv_device(xdev);
if (name != XRT_OUTPUT_NAME_PSMV_RUMBLE_VIBRATION) {
return;
}
psmv->wants.rumble =
psmv_clamp_zero_to_one_float_to_u8(value->vibration.amplitude);
// Resend if the rumble has been changed.
int64_t now = os_monotonic_get_ns();
psmv_led_and_trigger_update(psmv, now);
}
/*
*
* Prober functions.
*
*/
#define SET_INPUT(NAME) \
(psmv->base.inputs[PSMV_INDEX_##NAME].name = XRT_INPUT_PSMV_##NAME)
int
psmv_found(struct xrt_prober *xp,
struct xrt_prober_device **devices,
size_t num_devices,
size_t index,
cJSON *attached_data,
struct xrt_device **out_xdevs)
{
struct os_hid_device *hid = NULL;
int ret;
// We do not receive any sensor packages on USB.
if (devices[index]->bus != XRT_BUS_TYPE_BLUETOOTH) {
return 0;
}
// Sanity check for device type.
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;
}
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
struct psmv_device *psmv =
U_DEVICE_ALLOCATE(struct psmv_device, flags, 12, 1);
psmv->print_spew = debug_get_bool_option_psmv_spew();
psmv->print_debug = debug_get_bool_option_psmv_debug();
psmv->base.destroy = psmv_device_destroy;
psmv->base.update_inputs = psmv_device_update_inputs;
psmv->base.get_tracked_pose = psmv_device_get_tracked_pose;
psmv->base.set_output = psmv_device_set_output;
psmv->base.name = XRT_DEVICE_PSMV;
psmv->fusion.rot.w = 1.0f;
psmv->fusion.fusion = imu_fusion_create();
psmv->pid = devices[index]->product_id;
psmv->hid = hid;
snprintf(psmv->base.str, XRT_DEVICE_NAME_LEN, "%s",
"PS Move Controller");
m_imu_pre_filter_init(&psmv->calibration.prefilter, 1.f, 1.f);
// Default variance
switch (devices[index]->product_id) {
case PSMV_PID_ZCM1:
// Note that there is one axis "weird" in each - this model has
// 2-axis sensors.
psmv->fusion.variance.accel.x = 0.00046343013089f;
psmv->fusion.variance.accel.y = 0.000358375519793f;
psmv->fusion.variance.accel.z = 0.000358375519793f;
psmv->fusion.variance.gyro.x = 7.85920759635965E-05f;
psmv->fusion.variance.gyro.y = 7.85920759635965E-05f;
psmv->fusion.variance.gyro.z = 0.00051253981244f;
break;
case PSMV_PID_ZCM2:
//! @todo measure!
psmv->fusion.variance.accel.x = 0.00046343013089f;
psmv->fusion.variance.accel.y = 0.000358375519793f;
psmv->fusion.variance.accel.z = 0.000358375519793f;
psmv->fusion.variance.gyro.x = 7.85920759635965E-05f;
psmv->fusion.variance.gyro.y = 7.85920759635965E-05f;
psmv->fusion.variance.gyro.z = 0.00051253981244f;
break;
default:
//! @todo cleanup to not leak
return -1;
}
// Setup inputs.
SET_INPUT(PS_CLICK);
SET_INPUT(MOVE_CLICK);
SET_INPUT(START_CLICK);
SET_INPUT(SELECT_CLICK);
SET_INPUT(SQUARE_CLICK);
SET_INPUT(CROSS_CLICK);
SET_INPUT(CIRCLE_CLICK);
SET_INPUT(TRIANGLE_CLICK);
SET_INPUT(TRIGGER_VALUE);
SET_INPUT(BODY_CENTER_POSE);
SET_INPUT(BALL_CENTER_POSE);
SET_INPUT(BALL_TIP_POSE);
// We only have one output.
psmv->base.outputs[0].name = XRT_OUTPUT_NAME_PSMV_RUMBLE_VIBRATION;
// Mutex before thread.
ret = os_mutex_init(&psmv->lock);
if (ret != 0) {
PSMV_ERROR(psmv, "Failed to init mutex!");
psmv_device_destroy(&psmv->base);
return ret;
}
// Thread and other state.
ret = os_thread_helper_init(&psmv->oth);
if (ret != 0) {
PSMV_ERROR(psmv, "Failed to init threading!");
psmv_device_destroy(&psmv->base);
return ret;
}
// Get calibration data.
ret = psmv_get_calibration(psmv);
if (ret != 0) {
PSMV_ERROR(psmv, "Failed to get calibration data!");
psmv_device_destroy(&psmv->base);
return ret;
}
#if 1
// 45mm
float diameter = 0.045;
(void)diameter;
if (xp->tracking != NULL) {
xp->tracking->create_tracked_psmv(xp->tracking, &psmv->base,
&psmv->ball);
}
#endif
if (psmv->ball != NULL) {
// Use the new origin if we got a tracking system.
psmv->base.tracking_origin = psmv->ball->origin;
// We got a tracked ball, use it.
psmv->base.tracking_origin = psmv->ball->origin;
psmv->wants.led.r =
psmv_clamp_zero_to_one_float_to_u8(psmv->ball->colour.r);
psmv->wants.led.g =
psmv_clamp_zero_to_one_float_to_u8(psmv->ball->colour.g);
psmv->wants.led.b =
psmv_clamp_zero_to_one_float_to_u8(psmv->ball->colour.b);
} else {
// Failed to create a tracking ball.
static int hack = 0;
switch (hack++ % 3) {
case 0: psmv->wants.led.r = 0xff; break;
case 1:
psmv->wants.led.r = 0xff;
psmv->wants.led.b = 0xff;
break;
case 2: psmv->wants.led.b = 0xff; break;
}
}
// Send the first update package.
psmv_led_and_trigger_update(psmv, 1);
ret = os_thread_helper_start(&psmv->oth, psmv_run_thread, psmv);
if (ret != 0) {
PSMV_ERROR(psmv, "Failed to start thread!");
psmv_device_destroy(&psmv->base);
return ret;
}
// Start the variable tracking now that everything is in place.
// 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");
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.prefilter.accel.gain, "prefilter.accel.gain");
u_var_add_vec3_f32(psmv, &psmv->calibration.prefilter.accel.bias, "prefilter.accel.bias");
u_var_add_vec3_f32(psmv, &psmv->calibration.prefilter.gyro.gain, "prefilter.gyro.gain");
u_var_add_vec3_f32(psmv, &psmv->calibration.prefilter.gyro.bias, "prefilter.gyro.bias");
u_var_add_gui_header(psmv, &psmv->gui.last_frame, "Last data");
u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[0].accel, "last.samples[0].accel");
u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[1].accel, "last.samples[1].accel");
u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[0].gyro, "last.samples[0].gyro");
u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[1].gyro, "last.samples[1].gyro");
u_var_add_ro_vec3_f32(psmv, &psmv->read.accel, "read.accel");
u_var_add_ro_vec3_f32(psmv, &psmv->read.gyro, "read.gyro");
u_var_add_gui_header(psmv, &psmv->gui.fusion, "Fusion");
u_var_add_vec3_f32(psmv, &psmv->fusion.variance.accel, "fusion.variance.accel");
u_var_add_vec3_f32(psmv, &psmv->fusion.variance.gyro, "fusion.variance.gyro");
u_var_add_ro_quat_f32(psmv, &psmv->fusion.rot, "fusion.rot");
u_var_add_ro_vec3_f32(psmv, &psmv->fusion.rotvec, "fusion.rotvec");
u_var_add_gui_header(psmv, &psmv->gui.control, "Control");
u_var_add_rgb_u8(psmv, &psmv->wants.led, "Led");
u_var_add_u8(psmv, &psmv->wants.rumble, "Rumble");
u_var_add_bool(psmv, &psmv->print_debug, "Debug");
u_var_add_bool(psmv, &psmv->print_spew, "Spew");
// clang-format on
// And finally done
*out_xdevs = &psmv->base;
return 1;
}
/*
*
* Parsing functions
*
*/
static void
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)
{
// The cast is important, sign extend properly.
*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)
{
psmv_i32_from_i16_wire(&to->x, &from->x);
psmv_i32_from_i16_wire(&to->y, &from->y);
psmv_i32_from_i16_wire(&to->z, &from->z);
}
static void
psmv_f32_from_wire(float *to, const struct psmv_f32_wire *from)
{
union {
uint32_t wire;
float f32;
} safe_copy;
safe_copy.wire = (from->val[0] << 0) | (from->val[1] << 8) |
(from->val[2] << 16) | (from->val[3] << 24);
*to = safe_copy.f32;
}
static void
psmv_from_vec3_f32_wire(struct xrt_vec3 *to,
const struct psmv_vec3_f32_wire *from)
{
psmv_f32_from_wire(&to->x, &from->x);
psmv_f32_from_wire(&to->y, &from->y);
psmv_f32_from_wire(&to->z, &from->z);
}
/*
*
* Packet functions ZCM1
*
*/
static int
psmv_get_calibration_zcm1(struct psmv_device *psmv)
{
struct psmv_parsed_calibration_zcm1 *zcm1 = &psmv->calibration.zcm1;
struct psmv_calibration_zcm1 data;
uint8_t *dst = (uint8_t *)&data;
int ret = 0;
size_t src_offset, dst_offset;
for (int i = 0; i < 3; i++) {
struct psmv_calibration_part part = {0};
uint8_t *src = (uint8_t *)&part;
part.id = 0x10;
ret = os_hid_get_feature(psmv->hid, 0x10, src, sizeof(part));
if (ret < 0) {
PSMV_ERROR(psmv, "os_hid_get_feature returned %i", ret);
return ret;
}
if (ret != (int)sizeof(part)) {
PSMV_ERROR(psmv, "Size wrong: %i != %i", ret,
(int)sizeof(part));
return -1;
}
switch (part.which) {
case 0x00:
src_offset = 0;
dst_offset = 0;
break;
case 0x01:
src_offset = 2;
dst_offset = sizeof(part);
break;
case 0x82:
src_offset = 2;
dst_offset = sizeof(part) * 2 - 2;
break;
default:
PSMV_ERROR(psmv, "Unexpected part id! %i", part.which);
return -1;
}
memcpy(dst + dst_offset, src + src_offset,
sizeof(part) - src_offset);
}
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);
psmv_f32_from_wire(&zcm1->unknown_float_1, &data.unknown_float_1);
/*
* See the following reference for details on the computations below. We
* are currently pretending cross-gains are zero.
*
* Vitali, Andrea. “6-Point Tumble Sensor Calibration.” Design tip.
* STMicroelectronics, 2015.
* https://www.st.com/resource/en/design_tip/dm00253745-6point-tumble-sensor-calibration-stmicroelectronics.pdf.
*/
/*
* Acceleration
*/
psmv->calibration.prefilter.accel.gain.x =
MATH_GRAVITY_M_S2 /
((zcm1->accel_max_x.x - zcm1->accel_min_x.x) / 2.0);
psmv->calibration.prefilter.accel.gain.y =
MATH_GRAVITY_M_S2 /
((zcm1->accel_max_y.y - zcm1->accel_min_y.y) / 2.0);
psmv->calibration.prefilter.accel.gain.z =
MATH_GRAVITY_M_S2 /
((zcm1->accel_max_z.z - zcm1->accel_min_z.z) / 2.0);
psmv->calibration.prefilter.accel.bias.x =
(zcm1->accel_min_x.x + zcm1->accel_max_x.x + zcm1->accel_min_y.x +
zcm1->accel_max_y.x + zcm1->accel_min_z.x + zcm1->accel_max_z.x) /
6.0;
psmv->calibration.prefilter.accel.bias.y =
(zcm1->accel_min_x.y + zcm1->accel_max_x.y + zcm1->accel_min_y.y +
zcm1->accel_max_y.y + zcm1->accel_min_z.y + zcm1->accel_max_z.y) /
6.0;
psmv->calibration.prefilter.accel.bias.z =
(zcm1->accel_min_x.z + zcm1->accel_max_x.z + zcm1->accel_min_y.z +
zcm1->accel_max_y.z + zcm1->accel_min_z.z + zcm1->accel_max_z.z) /
6.0;
/*
* Gyro
*/
double gx =
(zcm1->gyro_rot_x.x - (zcm1->gyro_bias_0.x * zcm1->gyro_fact.x));
double gy =
(zcm1->gyro_rot_y.y - (zcm1->gyro_bias_0.y * zcm1->gyro_fact.y));
double gz =
(zcm1->gyro_rot_z.z - (zcm1->gyro_bias_0.z * zcm1->gyro_fact.z));
psmv->calibration.prefilter.gyro.gain.x =
(2.0 * M_PI * 80.0) / (60.0 * gx);
psmv->calibration.prefilter.gyro.gain.y =
(2.0 * M_PI * 80.0) / (60.0 * gy);
psmv->calibration.prefilter.gyro.gain.z =
(2.0 * M_PI * 80.0) / (60.0 * gz);
psmv->calibration.prefilter.gyro.bias.x = 0.0;
psmv->calibration.prefilter.gyro.bias.y = 0.0;
psmv->calibration.prefilter.gyro.bias.z = 0.0;
/*
* Print
*/
PSMV_DEBUG(
psmv,
"\n"
"\tCalibration:\n"
"\t\taccel_min_x: %6i %6i %6i\n"
"\t\taccel_max_x: %6i %6i %6i\n"
"\t\taccel_min_y: %6i %6i %6i\n"
"\t\taccel_max_y: %6i %6i %6i\n"
"\t\taccel_min_z: %6i %6i %6i\n"
"\t\taccel_max_z: %6i %6i %6i\n"
"\t\tgyro_rot_x: %6i %6i %6i\n"
"\t\tgyro_rot_y: %6i %6i %6i\n"
"\t\tgyro_rot_z: %6i %6i %6i\n"
"\t\tgyro_bias_0: %6i %6i %6i\n"
"\t\tgyro_bias_1: %6i %6i %6i\n"
"\t\tgyro_fact: %f %f %f\n"
"\t\tunknown_vec3: %f %f %f\n"
"\t\tunknown_float_0 %f\n"
"\t\tunknown_float_1 %f\n"
"\tCalculated:\n"
"\t\taccel.gain: %f %f %f\n"
"\t\taccel.bias: %f %f %f\n"
"\t\tgyro.gain: %f %f %f\n"
"\t\tgyro.bias: %f %f %f\n",
zcm1->accel_min_x.x, zcm1->accel_min_x.y, zcm1->accel_min_x.z,
zcm1->accel_max_x.x, zcm1->accel_max_x.y, zcm1->accel_max_x.z,
zcm1->accel_min_y.x, zcm1->accel_min_y.y, zcm1->accel_min_y.z,
zcm1->accel_max_y.x, zcm1->accel_max_y.y, zcm1->accel_max_y.z,
zcm1->accel_min_z.x, zcm1->accel_min_z.y, zcm1->accel_min_z.z,
zcm1->accel_max_z.x, zcm1->accel_max_z.y, zcm1->accel_max_z.z,
zcm1->gyro_rot_x.x, zcm1->gyro_rot_x.y, zcm1->gyro_rot_x.z,
zcm1->gyro_rot_y.x, zcm1->gyro_rot_y.y, zcm1->gyro_rot_y.z,
zcm1->gyro_rot_z.x, zcm1->gyro_rot_z.y, zcm1->gyro_rot_z.z,
zcm1->gyro_bias_0.x, zcm1->gyro_bias_0.y, zcm1->gyro_bias_0.z,
zcm1->gyro_bias_1.x, zcm1->gyro_bias_1.y, zcm1->gyro_bias_1.z,
zcm1->gyro_fact.x, zcm1->gyro_fact.y, zcm1->gyro_fact.z,
zcm1->unknown_vec3.x, zcm1->unknown_vec3.y, zcm1->unknown_vec3.z,
zcm1->unknown_float_0, zcm1->unknown_float_1,
psmv->calibration.prefilter.accel.gain.x,
psmv->calibration.prefilter.accel.gain.y,
psmv->calibration.prefilter.accel.gain.z,
psmv->calibration.prefilter.accel.bias.x,
psmv->calibration.prefilter.accel.bias.y,
psmv->calibration.prefilter.accel.bias.z,
psmv->calibration.prefilter.gyro.gain.x,
psmv->calibration.prefilter.gyro.gain.y,
psmv->calibration.prefilter.gyro.gain.z,
psmv->calibration.prefilter.gyro.bias.x,
psmv->calibration.prefilter.gyro.bias.y,
psmv->calibration.prefilter.gyro.bias.z);
return 0;
}
static int
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;
input->buttons = 0;
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 = 0;
input->timestamp |= (uint16_t)data->timestamp_low;
input->timestamp |= ((uint16_t)data->timestamp_high) << 8;
input->trigger_values[0] = data->trigger_f1;
input->trigger_values[1] = data->trigger_f2;
psmv_from_vec3_u16_wire(&input->samples[0].accel, &data->accel_f1);
psmv_from_vec3_u16_wire(&input->samples[0].gyro, &data->gyro_f1);
psmv_from_vec3_u16_wire(&input->samples[1].accel, &data->accel_f2);
psmv_from_vec3_u16_wire(&input->samples[1].gyro, &data->gyro_f2);
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"
"samples[0].accel: %6i %6i %6i\n\t"
"samples[1].accel: %6i %6i %6i\n\t"
"samples[0].gyro: %6i %6i %6i\n\t"
"samples[1].gyro: %6i %6i %6i\n\t"
"trigger_values[0]: %02x\n\t"
"trigger_values[1]: %02x\n\t"
"timestamp: %i\n\t"
"diff: %i\n\t"
"seq_no: %x\n",
missed ? "yes" : "no", input->buttons, input->battery,
input->samples[0].accel.x, input->samples[0].accel.y,
input->samples[0].accel.z, input->samples[1].accel.x,
input->samples[1].accel.y, input->samples[1].accel.z,
input->samples[0].gyro.x, input->samples[0].gyro.y,
input->samples[0].gyro.z, input->samples[1].gyro.x,
input->samples[1].gyro.y, input->samples[1].gyro.z,
input->trigger_values[0], input->trigger_values[1],
input->timestamp, diff, input->seq_no);
return 2;
}
/*
*
* 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;
part.id = 0x10;
ret = os_hid_get_feature(psmv->hid, 0x10, src, sizeof(part));
if (ret < 0) {
PSMV_ERROR(psmv, "os_hid_get_feature returned %i", ret);
return ret;
}
if (ret != (int)sizeof(part)) {
PSMV_ERROR(psmv, "Size wrong: %i != %i", 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:
PSMV_ERROR(psmv, "Unexpected part id! %i", part.which);
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);
psmv_from_vec3_i16_wire(&zcm2->gyro_bias, &data.gyro_bias);
/*
* Acceleration
*/
psmv->calibration.prefilter.accel.gain.x =
MATH_GRAVITY_M_S2 /
((zcm2->accel_max_x.x - zcm2->accel_min_x.x) / 2.0);
psmv->calibration.prefilter.accel.gain.y =
MATH_GRAVITY_M_S2 /
((zcm2->accel_max_y.y - zcm2->accel_min_y.y) / 2.0);
psmv->calibration.prefilter.accel.gain.z =
MATH_GRAVITY_M_S2 /
((zcm2->accel_max_z.z - zcm2->accel_min_z.z) / 2.0);
psmv->calibration.prefilter.accel.bias.x =
(zcm2->accel_min_x.x + zcm2->accel_max_x.x + zcm2->accel_min_y.x +
zcm2->accel_max_y.x + zcm2->accel_min_z.x + zcm2->accel_max_z.x) /
6.0;
psmv->calibration.prefilter.accel.bias.y =
(zcm2->accel_min_x.y + zcm2->accel_max_x.y + zcm2->accel_min_y.y +
zcm2->accel_max_y.y + zcm2->accel_min_z.y + zcm2->accel_max_z.y) /
6.0;
psmv->calibration.prefilter.accel.bias.z =
(zcm2->accel_min_x.z + zcm2->accel_max_x.z + zcm2->accel_min_y.z +
zcm2->accel_max_y.z + zcm2->accel_min_z.z + zcm2->accel_max_z.z) /
6.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.prefilter.gyro.gain.x =
(2.0 * M_PI * 90.0) / (60.0 * gx);
psmv->calibration.prefilter.gyro.gain.y =
(2.0 * M_PI * 90.0) / (60.0 * gy);
psmv->calibration.prefilter.gyro.gain.z =
(2.0 * M_PI * 90.0) / (60.0 * gz);
#if 0
psmv->calibration.prefilter.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.prefilter.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.prefilter.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;
#else
psmv->calibration.prefilter.gyro.bias.x = zcm2->gyro_bias.x;
psmv->calibration.prefilter.gyro.bias.y = zcm2->gyro_bias.y;
psmv->calibration.prefilter.gyro.bias.z = zcm2->gyro_bias.z;
#endif
/*
* Print
*/
PSMV_DEBUG(
psmv,
"\n"
"\tCalibration:\n"
"\t\taccel_min_x: %6i %6i %6i\n"
"\t\taccel_max_x: %6i %6i %6i\n"
"\t\taccel_min_y: %6i %6i %6i\n"
"\t\taccel_max_y: %6i %6i %6i\n"
"\t\taccel_min_z: %6i %6i %6i\n"
"\t\taccel_max_z: %6i %6i %6i\n"
"\t\tgyro_neg_x: %6i %6i %6i\n"
"\t\tgyro_pos_x: %6i %6i %6i\n"
"\t\tgyro_neg_y: %6i %6i %6i\n"
"\t\tgyro_pos_y: %6i %6i %6i\n"
"\t\tgyro_neg_z: %6i %6i %6i\n"
"\t\tgyro_pos_z: %6i %6i %6i\n"
"\t\tgyro_bias: %6i %6i %6i\n"
"\tCalculated:\n"
"\t\taccel.gain: %f %f %f\n"
"\t\taccel.bias: %f %f %f\n"
"\t\tgyro.gain: %f %f %f\n"
"\t\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,
zcm2->gyro_bias.x, zcm2->gyro_bias.y, zcm2->gyro_bias.z,
psmv->calibration.prefilter.accel.gain.x,
psmv->calibration.prefilter.accel.gain.y,
psmv->calibration.prefilter.accel.gain.z,
psmv->calibration.prefilter.accel.bias.x,
psmv->calibration.prefilter.accel.bias.y,
psmv->calibration.prefilter.accel.bias.z,
psmv->calibration.prefilter.gyro.gain.x,
psmv->calibration.prefilter.gyro.gain.y,
psmv->calibration.prefilter.gyro.gain.z,
psmv->calibration.prefilter.gyro.bias.x,
psmv->calibration.prefilter.gyro.bias.y,
psmv->calibration.prefilter.gyro.bias.z);
return 0;
}
static int
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 = 0;
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 = 0;
input->timestamp |= (uint16_t)data->timestamp_low;
input->timestamp |= ((uint16_t)data->timestamp_high) << 8;
input->timestamp_copy = 0;
input->timestamp_copy |= (uint16_t)data->timestamp_low_copy;
input->timestamp_copy |= ((uint16_t)data->timestamp_high_copy) << 8;
input->trigger_low_pass = data->trigger_low_pass;
input->trigger = data->trigger;
psmv_from_vec3_i16_wire(&input->sample.accel, &data->accel);
psmv_from_vec3_i16_wire(&input->sample.gyro, &data->gyro);
psmv_from_vec3_i16_wire(&input->sample_copy.accel, &data->accel_copy);
psmv_from_vec3_i16_wire(&input->sample_copy.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.accel: %6i %6i %6i\n\t"
"sample_copy.accel: %6i %6i %6i\n\t"
"sample.gyro: %6i %6i %6i\n\t"
"sample_copy.gyro: %6i %6i %6i\n\t"
"sample.trigger: %02x\n\t"
"sample.trigger_low_pass: %02x\n\t"
"timestamp: %04x\n\t"
"timestamp_copy: %04x\n\t"
"diff: %i\n\t"
"seq_no: %x\n",
missed ? "yes" : "no", input->buttons, input->battery,
input->samples[0].accel.x, input->samples[0].accel.y,
input->samples[0].accel.z, input->samples[1].accel.x,
input->samples[1].accel.y, input->samples[1].accel.z,
input->samples[0].gyro.x, input->samples[0].gyro.y,
input->samples[0].gyro.z, input->samples[1].gyro.x,
input->samples[1].gyro.y, input->samples[1].gyro.z,
input->trigger_low_pass, input->trigger, input->timestamp,
input->timestamp_copy, diff, input->seq_no);
return 1;
}
/*
*
* 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 int
psmv_parse_input(struct psmv_device *psmv,
void *data,
struct psmv_parsed_input *input)
{
U_ZERO(input);
switch (psmv->pid) {
case PSMV_PID_ZCM1:
return psmv_parse_input_zcm1(
psmv, (struct psmv_input_zcm1 *)data, input);
case PSMV_PID_ZCM2:
return psmv_parse_input_zcm2(
psmv, (struct psmv_input_zcm2 *)data, input);
default: return 0;
}
}
/*!
* @}
*/
Reference in a new issue View git blame Copy permalink