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

d/vive/controller: Use u_json and move config to vive_config.

Browse source 
To be consistent with the headset implementation, move all config
related code to vive_config and use u_json consistently with static
string sizes.
This commit is contained in:
Lubosz Sarnecki 2020-07-02 18:33:33 +02:00
parent 53619fa64e
commit 260cfd8d26
4 changed files with 111 additions and 135 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

74
src/xrt/drivers/vive/vive_config.c
View file

@ -15,6 +15,7 @@
#include "math/m_api.h"
#include "vive_device.h"
#include "vive_controller.h"
#define JSON_INT(a, b, c) u_json_get_int(u_json_get(a, b), c)
#define JSON_FLOAT(a, b, c) u_json_get_float(u_json_get(a, b), c)
@ -332,3 +333,76 @@ vive_config_parse(struct vive_device *d, char *json_string)
return true;
}
bool
vive_config_parse_controller(struct vive_controller_device *d,
char *json_string)
{
VIVE_CONTROLLER_DEBUG(d, "JSON config:\n%s\n", json_string);
cJSON *json = cJSON_Parse(json_string);
if (!cJSON_IsObject(json)) {
VIVE_CONTROLLER_ERROR(d, "Could not parse JSON data.");
return false;
}
JSON_STRING(json, "model_number", d->firmware.model_number);
if (strcmp(d->firmware.model_number, "Vive. Controller MV") == 0) {
d->variant = CONTROLLER_VIVE_WAND;
VIVE_CONTROLLER_DEBUG(d, "Found Vive Wand controller");
} else if (strcmp(d->firmware.model_number, "Knuckles Right") == 0) {
d->variant = CONTROLLER_INDEX_RIGHT;
VIVE_CONTROLLER_DEBUG(d, "Found Knuckles Right controller");
} else if (strcmp(d->firmware.model_number, "Knuckles Left") == 0) {
d->variant = CONTROLLER_INDEX_LEFT;
VIVE_CONTROLLER_DEBUG(d, "Found Knuckles Left controller");
} else {
VIVE_CONTROLLER_ERROR(d, "Failed to parse controller variant");
}
switch (d->variant) {
case CONTROLLER_VIVE_WAND: {
JSON_VEC3(json, "acc_bias", &d->imu.acc_bias);
JSON_VEC3(json, "acc_scale", &d->imu.acc_scale);
JSON_VEC3(json, "gyro_bias", &d->imu.gyro_bias);
JSON_VEC3(json, "gyro_scale", &d->imu.gyro_scale);
JSON_STRING(json, "mb_serial_number",
d->firmware.mb_serial_number);
} break;
case CONTROLLER_INDEX_LEFT:
case CONTROLLER_INDEX_RIGHT: {
const cJSON *imu = u_json_get(json, "imu");
_get_pose_from_pos_x_z(imu, &d->imu.trackref);
JSON_VEC3(imu, "acc_bias", &d->imu.acc_bias);
JSON_VEC3(imu, "acc_scale", &d->imu.acc_scale);
JSON_VEC3(imu, "gyro_bias", &d->imu.gyro_bias);
} break;
default:
VIVE_CONTROLLER_ERROR(d, "Unknown Vive watchman variant.\n");
return false;
}
JSON_STRING(json, "device_serial_number",
d->firmware.device_serial_number);
cJSON_Delete(json);
// clang-format off
VIVE_CONTROLLER_DEBUG(d, "= Vive controller configuration =");
VIVE_CONTROLLER_DEBUG(d, "model_number: %s", d->firmware.model_number);
VIVE_CONTROLLER_DEBUG(d, "mb_serial_number: %s", d->firmware.mb_serial_number);
VIVE_CONTROLLER_DEBUG(d, "device_serial_number: %s", d->firmware.device_serial_number);
if (d->print_debug) {
_print_vec3("acc_bias", &d->imu.acc_bias);
_print_vec3("acc_scale", &d->imu.acc_scale);
_print_vec3("gyro_bias", &d->imu.gyro_bias);
_print_vec3("gyro_scale", &d->imu.gyro_scale);
}
// clang-format on
return true;
}

7
src/xrt/drivers/vive/vive_config.h
View file

@ -15,3 +15,10 @@ struct vive_device;
bool
vive_config_parse(struct vive_device *d, char *json_string);
struct vive_controller_device;
bool
vive_config_parse_controller(struct vive_controller_device *d,
char *json_string);

134
src/xrt/drivers/vive/vive_controller.c
View file

@ -33,6 +33,7 @@
#include "vive_protocol.h"
#include "vive_controller.h"
#include "vive_config.h"
#ifdef XRT_OS_LINUX
#include <unistd.h>
@ -45,31 +46,6 @@
*
*/
#define VIVE_CONTROLLER_SPEW(p, ...) \
do { \
if (p->print_spew) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define VIVE_CONTROLLER_DEBUG(p, ...) \
do { \
if (p->print_debug) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define VIVE_CONTROLLER_ERROR(p, ...) \
do { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} while (false)
DEBUG_GET_ONCE_BOOL_OPTION(vive_controller_spew,
"VIVE_CONTROLLER_PRINT_SPEW",
false)
@ -154,7 +130,6 @@ vive_controller_device_update_wand_inputs(struct xrt_device *xdev)
printf("\n");
*/
uint64_t now = os_monotonic_get_ns();
/* d->state.buttons is bitmask of currently pressed buttons.
@ -1025,111 +1000,6 @@ vive_controller_run_thread(void *ptr)
return NULL;
}
static char *
_json_get_string(const cJSON *json, const char *name)
{
const cJSON *item = cJSON_GetObjectItemCaseSensitive(json, name);
return strdup(item->valuestring);
}
static void
print_vec3(const char *title, struct xrt_vec3 *vec)
{
printf("%s = %f %f %f\n", title, (double)vec->x, (double)vec->y,
(double)vec->z);
}
static void
_get_pose_from_pos_x_z(const cJSON *obj, struct xrt_pose *pose)
{
struct xrt_vec3 plus_x, plus_z;
u_json_get_vec3_array(u_json_get(obj, "plus_x"), &plus_x);
u_json_get_vec3_array(u_json_get(obj, "plus_z"), &plus_z);
u_json_get_vec3_array(u_json_get(obj, "position"), &pose->position);
math_quat_from_plus_x_z(&plus_x, &plus_z, &pose->orientation);
}
static bool
vive_controller_parse_config(struct vive_controller_device *d,
char *json_string)
{
VIVE_CONTROLLER_DEBUG(d, "JSON config:\n%s\n", json_string);
cJSON *json = cJSON_Parse(json_string);
if (!cJSON_IsObject(json)) {
VIVE_CONTROLLER_ERROR(d, "Could not parse JSON data.");
return false;
}
d->firmware.model_number = _json_get_string(json, "model_number");
if (strcmp(d->firmware.model_number, "Vive. Controller MV") == 0) {
d->variant = CONTROLLER_VIVE_WAND;
VIVE_CONTROLLER_DEBUG(d, "Found Vive Wand controller");
} else if (strcmp(d->firmware.model_number, "Knuckles Right") == 0) {
d->variant = CONTROLLER_INDEX_RIGHT;
VIVE_CONTROLLER_DEBUG(d, "Found Knuckles Right controller");
} else if (strcmp(d->firmware.model_number, "Knuckles Left") == 0) {
d->variant = CONTROLLER_INDEX_LEFT;
VIVE_CONTROLLER_DEBUG(d, "Found Knuckles Left controller");
} else {
VIVE_CONTROLLER_ERROR(d, "Failed to parse controller variant");
}
switch (d->variant) {
case CONTROLLER_VIVE_WAND: {
u_json_get_vec3_array(u_json_get(json, "acc_bias"),
&d->imu.acc_bias);
u_json_get_vec3_array(u_json_get(json, "acc_scale"),
&d->imu.acc_scale);
u_json_get_vec3_array(u_json_get(json, "gyro_bias"),
&d->imu.gyro_bias);
u_json_get_vec3_array(u_json_get(json, "gyro_scale"),
&d->imu.gyro_scale);
d->firmware.mb_serial_number =
_json_get_string(json, "mb_serial_number");
} break;
case CONTROLLER_INDEX_LEFT:
case CONTROLLER_INDEX_RIGHT: {
const cJSON *imu = u_json_get(json, "imu");
_get_pose_from_pos_x_z(imu, &d->imu.trackref);
u_json_get_vec3_array(u_json_get(imu, "acc_bias"),
&d->imu.acc_bias);
u_json_get_vec3_array(u_json_get(imu, "acc_scale"),
&d->imu.acc_scale);
u_json_get_vec3_array(u_json_get(imu, "gyro_bias"),
&d->imu.gyro_bias);
} break;
default:
VIVE_CONTROLLER_ERROR(d, "Unknown Vive watchman variant.\n");
return false;
}
d->firmware.device_serial_number =
_json_get_string(json, "device_serial_number");
cJSON_Delete(json);
// clang-format off
VIVE_CONTROLLER_DEBUG(d, "= Vive controller configuration =");
VIVE_CONTROLLER_DEBUG(d, "model_number: %s", d->firmware.model_number);
VIVE_CONTROLLER_DEBUG(d, "mb_serial_number: %s", d->firmware.mb_serial_number);
VIVE_CONTROLLER_DEBUG(d, "device_serial_number: %s", d->firmware.device_serial_number);
if (d->print_debug) {
print_vec3("acc_bias", &d->imu.acc_bias);
print_vec3("acc_scale", &d->imu.acc_scale);
print_vec3("gyro_bias", &d->imu.gyro_bias);
print_vec3("gyro_scale", &d->imu.gyro_scale);
}
// clang-format on
return true;
}
#define SET_WAND_INPUT(NAME, NAME2) \
do { \
(d->base.inputs[VIVE_CONTROLLER_INDEX_##NAME].name = \
@ -1207,7 +1077,7 @@ vive_controller_create(struct os_hid_device *controller_hid,
// successful config parsing determines d->variant
char *config = vive_read_config(d->controller_hid);
if (config != NULL) {
vive_controller_parse_config(d, config);
vive_config_parse_controller(d, config);
free(config);
} else {
VIVE_CONTROLLER_ERROR(d,

31
src/xrt/drivers/vive/vive_controller.h
View file

@ -116,9 +116,9 @@ struct vive_controller_device
uint8_t hardware_version_micro;
uint8_t hardware_version_minor;
uint8_t hardware_version_major;
char *mb_serial_number;
char *model_number;
char *device_serial_number;
char mb_serial_number[32];
char model_number[32];
char device_serial_number[32];
} firmware;
enum watchman_gen watchman_gen;
@ -133,3 +133,28 @@ vive_controller_create(struct os_hid_device *controller_hid,
#ifdef __cplusplus
}
#endif
#define VIVE_CONTROLLER_SPEW(p, ...) \
do { \
if (p->print_spew) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define VIVE_CONTROLLER_DEBUG(p, ...) \
do { \
if (p->print_debug) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define VIVE_CONTROLLER_ERROR(p, ...) \
do { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} while (false)