d/wmr: Factor out parsing of controller messages and minor cleanup.

This commit is contained in:
Nima01 2021-11-15 02:05:19 +01:00 committed by Jakob Bornecrantz
parent c4db3dfccc
commit e536a02b33
7 changed files with 218 additions and 101 deletions

View file

@ -308,6 +308,8 @@ if(XRT_BUILD_DRIVER_WMR)
wmr/wmr_prober.c
wmr/wmr_protocol.c
wmr/wmr_protocol.h
wmr/wmr_controller_protocol.c
wmr/wmr_controller_protocol.h
)
target_link_libraries(drv_wmr PRIVATE xrt-interfaces aux_util aux_math xrt-external-cjson)
list(APPEND ENABLED_HEADSET_DRIVERS wmr)

View file

@ -303,6 +303,8 @@ lib_drv_wmr = static_library(
'wmr/wmr_prober.c',
'wmr/wmr_protocol.c',
'wmr/wmr_protocol.h',
'wmr/wmr_controller_protocol.c',
'wmr/wmr_controller_protocol.h',
),
include_directories: [
xrt_include,

View file

@ -8,9 +8,6 @@
* @ingroup drv_wmr
*/
#include "xrt/xrt_config_os.h"
#include "xrt/xrt_device.h"
#include "os/os_time.h"
#include "os/os_hid.h"
@ -25,9 +22,8 @@
#include "util/u_debug.h"
#include "util/u_device.h"
#include "wmr_bt_controller.h"
#include "wmr_common.h"
#include "wmr_protocol.h"
#include "wmr_bt_controller.h"
#include <stdio.h>
#include <stdlib.h>
@ -51,91 +47,34 @@ wmr_bt_controller(struct xrt_device *p)
}
static bool
control_read_packets(struct wmr_bt_controller *d)
read_packets(struct wmr_bt_controller *d)
{
unsigned char buffer[WMR_FEATURE_BUFFER_SIZE];
unsigned char buffer[WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE];
// Do not block
int size = os_hid_read(d->controller_hid, buffer, sizeof(buffer), 0);
if (size < 0) {
WMR_ERROR(d, "Error reading from controller device");
WMR_ERROR(d, "WMR Controller (Bluetooth): Error reading from device");
return false;
} else if (size == 0) {
WMR_TRACE(d, "No more data to read from controller device");
WMR_TRACE(d, "WMR Controller (Bluetooth): No data to read from device");
return true; // No more messages, return.
} else {
WMR_DEBUG(d, "Read %u bytes from controller device", size);
WMR_DEBUG(d, "WMR Controller (Bluetooth): Read %u bytes from device", size);
}
switch (buffer[0]) {
case WMR_MS_HOLOLENS_MSG_SENSORS: //
if (size != 45) {
WMR_ERROR(d, "WMR Controller unexpected message size: %d", size);
case WMR_BT_MOTION_CONTROLLER_MSG:
// Note: skipping msg type byte
if (!wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->controller_message, d->ll)) {
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i",
buffer[0], size);
return false;
}
WMR_DEBUG(d,
"%02x | " // msg type
"%02x %02x %02x %02x %02x %02x %02x %02x | " // buttons and inputs, battery
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // accel
"%02x %02x | " // temp
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // gyro
"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | " // timestamp and more?
"%02x %02x %02x %02x %02x %02x", // device run state, status and more?
buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7],
buffer[8], buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15],
buffer[16], buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22],
buffer[23], buffer[24], buffer[25], buffer[26], buffer[27], buffer[28], buffer[29],
buffer[30], buffer[31], buffer[32], buffer[33], buffer[34], buffer[35], buffer[36],
buffer[37], buffer[38], buffer[39], buffer[40], buffer[41], buffer[42], buffer[43],
buffer[44]);
const unsigned char *p = (unsigned char *)&buffer[1];
// HP Reverb G1 button mask:
// Stick_pressed: 0x01
// Windows button: 0x02
// Menu button: 0x04
// Side button: 0x08
// Touch-pad pressed: 0x10
// BT pairing button: 0x20
// Touch-pad touched: 0x40
uint8_t buttons = read8(&p);
// Todo: interpret analog stick data
uint8_t stick_1 = read8(&p);
uint8_t stick_2 = read8(&p);
uint8_t stick_3 = read8(&p);
uint8_t trigger = read8(&p); // pressure: 0x00 - 0xFF
// Touchpad coords range: 0x00 - 0x64. Both are 0xFF when untouched.
uint8_t pad_x = read8(&p);
uint8_t pad_y = read8(&p);
uint8_t battery = read8(&p);
int32_t accel_x = read24(&p);
int32_t accel_y = read24(&p);
int32_t accel_z = read24(&p);
int32_t temp = read16(&p);
int32_t gyro_x = read24(&p);
int32_t gyro_y = read24(&p);
int32_t gyro_z = read24(&p);
uint64_t timestamp = read32(&p); // Maybe only part of timestamp.
read16(&p); // Unknown. Seems to depend on controller orientation.
read32(&p); // Unknown.
read16(&p); // Unknown. Device state, etc.
read16(&p);
read16(&p);
WMR_DEBUG(d, "timestamp %lu\ttemp %d\taccel x: %f\ty: %f\tz: %f\t\tgyro x: %f\tgyro y: %f\tgyro z: %f",
timestamp, temp, accel_x * 0.001f, accel_y * 0.001f, accel_z * 0.001f, gyro_x * 2e-6,
gyro_y * 2e-6, gyro_z * 2e-6);
break;
default: //
WMR_DEBUG(d, "Unknown message type: %02x, size: %i from controller device", buffer[0], size);
default:
WMR_DEBUG(d, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], size);
break;
}
@ -178,12 +117,12 @@ wmr_bt_controller_run_thread(void *ptr)
os_thread_helper_unlock(&d->controller_thread);
// Does not block.
if (!control_read_packets(d)) {
if (!read_packets(d)) {
break;
}
}
WMR_DEBUG(d, "Exiting reading thread.");
WMR_DEBUG(d, "WMR Controller (Bluetooth): Exiting reading thread.");
return NULL;
}
@ -199,10 +138,6 @@ wmr_bt_controller_destroy(struct xrt_device *xdev)
if (d->controller_hid != NULL) {
/* Do any deinit if we have a deinit function */
// if (d->hmd_desc && d->hmd_desc->deinit_func) {
// d->hmd_desc->deinit_func(d);
// }
os_hid_destroy(d->controller_hid);
d->controller_hid = NULL;
}
@ -246,12 +181,12 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
int ret = 0;
// Todo: Read config file from controller
// Todo: Read config file from controller if possible.
// Thread and other state.
ret = os_thread_helper_init(&d->controller_thread);
if (ret != 0) {
WMR_ERROR(d, "Failed to init WMR controller threading!");
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to init controller threading!");
wmr_bt_controller_destroy(&d->base);
d = NULL;
return NULL;
@ -260,7 +195,7 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
// Hand over controller device to reading thread.
ret = os_thread_helper_start(&d->controller_thread, wmr_bt_controller_run_thread, d);
if (ret != 0) {
WMR_ERROR(d, "Failed to start WMR controller thread!");
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to start controller thread!");
wmr_bt_controller_destroy(&d->base);
d = NULL;
return NULL;

View file

@ -12,15 +12,11 @@
#include "os/os_threading.h"
#include "xrt/xrt_prober.h"
#include "math/m_imu_3dof.h"
#include "util/u_logging.h"
#include "xrt/xrt_device.h"
#include "xrt/xrt_prober.h"
#include "wmr_protocol.h"
#include "wmr_config.h"
#include "wmr_controller_protocol.h"
#ifdef __cplusplus
extern "C" {
@ -41,12 +37,7 @@ struct wmr_bt_controller
struct os_thread_helper controller_thread;
struct os_mutex lock;
struct
{
uint64_t time_ns;
uint32_t last_sample_time_raw;
timepoint_ns ts_received_ns;
} imu;
struct wmr_controller_message controller_message;
struct m_imu_3dof fusion;
@ -61,12 +52,6 @@ struct wmr_bt_controller
enum u_logging_level ll;
uint32_t last_ticks;
// firmware configuration block, with device names etc
// struct wmr_config_header config_hdr;
// Config data parsed from the firmware JSON
// wmr_bt_controller_config config;
};

View file

@ -0,0 +1,88 @@
// Copyright 2020-2021, N Madsen.
// Copyright 2020-2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief WMR Motion Controller protocol helpers implementation.
* @author Nis Madsen <nima_zero_one@protonmail.com>
* @ingroup drv_wmr
*/
#include "util/u_misc.h"
#include "util/u_debug.h"
#include "util/u_logging.h"
#include "wmr_controller_protocol.h"
/*
*
* WMR Motion Controller protocol helpers
*
*/
bool
wmr_controller_packet_parse(const unsigned char *buffer,
size_t len,
struct wmr_controller_message *out_message,
enum u_logging_level ll)
{
if (len != 44) {
U_LOG_IFL_E(ll, "WMR Controller: unexpected message length: %zd", len);
return false;
}
U_LOG_IFL_D(ll,
"%02x %02x %02x %02x %02x %02x %02x %02x | " // buttons and inputs, battery
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // accel
"%02x %02x | " // temp
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // gyro
"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | " // timestamp and more?
"%02x %02x %02x %02x %02x %02x", // device run state, status and more?
buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7], buffer[8],
buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15], buffer[16],
buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22], buffer[23], buffer[24],
buffer[25], buffer[26], buffer[27], buffer[28], buffer[29], buffer[30], buffer[31], buffer[32],
buffer[33], buffer[34], buffer[35], buffer[36], buffer[37], buffer[38], buffer[39], buffer[40],
buffer[41], buffer[42], buffer[43]);
const unsigned char *p = buffer;
out_message->buttons = read8(&p);
// Todo: interpret analog stick data
out_message->stick_1 = read8(&p);
out_message->stick_2 = read8(&p);
out_message->stick_3 = read8(&p);
out_message->trigger = read8(&p); // pressure: 0x00 - 0xFF
// Touchpad coords range: 0x00 - 0x64. Both are 0xFF when untouched.
out_message->pad_x = read8(&p);
out_message->pad_y = read8(&p);
out_message->battery = read8(&p);
out_message->accel_x = read24(&p);
out_message->accel_y = read24(&p);
out_message->accel_z = read24(&p);
out_message->temp = read16(&p);
out_message->gyro_x = read24(&p);
out_message->gyro_y = read24(&p);
out_message->gyro_z = read24(&p);
out_message->timestamp = read32(&p); // Maybe only part of timestamp.
read16(&p); // Unknown. Seems to depend on controller orientation.
read32(&p); // Unknown.
read16(&p); // Unknown. Device state, etc.
read16(&p);
read16(&p);
U_LOG_IFL_D(ll, "buttons: %02x, trigger: %02x, pad_x: %02x, pad_y: %02x", out_message->buttons,
out_message->trigger, out_message->pad_x, out_message->pad_y);
/*
U_LOG_IFL_D(ll, "timestamp %lu\ttemp %d\taccel x: %f\ty: %f\tz: %f\t\tgyro x: %f\tgyro y: %f\tgyro z:
%f", timestamp, temp, accel_x * 0.001f, accel_y * 0.001f, accel_z * 0.001f, gyro_x * 2e-6, gyro_y * 2e-6,
gyro_z * 2e-6);
*/
return true;
}

View file

@ -0,0 +1,103 @@
// Copyright 2020-2021, N Madsen.
// Copyright 2020-2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief WMR Motion Controller protocol constants, structures and helpers
* @author Nis Madsen <nima_zero_one@protonmail.com>
* @ingroup drv_wmr
*/
#pragma once
#include "wmr_protocol.h"
#ifdef __cplusplus
extern "C" {
#endif
/*!
* WMR Motion Controller protocol constant and structures
*
* @ingroup drv_wmr
* @{
*/
// Todo: Is this enough?
#define WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE 256
// Messages types specific to Bluetooth connected WMR motion controllers
#define WMR_BT_MOTION_CONTROLLER_MSG 0x01
struct wmr_controller_message
{
// Very much still work in progress!
// HP Reverb G1 button map:
// Stick_pressed: 0x01
// Home button pressed: 0x02
// Menu button pressed: 0x04
// Grip button pressed: 0x08
// Touch-pad pressed: 0x10
// BT pairing button pressed: 0x20
// Touch-pad touched: 0x40
uint8_t buttons;
// Todo: interpret analog stick data
uint8_t stick_1;
uint8_t stick_2;
uint8_t stick_3;
uint8_t trigger; // pressure: 0x00 - 0xFF
// Touchpad coords range: 0x00 - 0x64. Both are 0xFF when untouched.
uint8_t pad_x;
uint8_t pad_y;
uint8_t battery;
int32_t accel_x;
int32_t accel_y;
int32_t accel_z;
int32_t temp;
int32_t gyro_x;
int32_t gyro_y;
int32_t gyro_z;
uint64_t timestamp;
};
/*!
* @}
*/
/*!
* WMR Motion Controller protocol helpers
*
* @ingroup drv_wmr
* @{
*/
bool
wmr_controller_packet_parse(const unsigned char *buffer,
size_t len,
struct wmr_controller_message *out_message,
enum u_logging_level ll);
/*!
* @}
*/
#ifdef __cplusplus
}
#endif

View file

@ -30,14 +30,16 @@ extern "C" {
#define WMR_FEATURE_BUFFER_SIZE 497
#define WMR_MS_HOLOLENS_NS_PER_TICK 100
// Messages types specific to WMR Hololens Sensors devices
#define WMR_MS_HOLOLENS_MSG_SENSORS 0x01
#define WMR_MS_HOLOLENS_MSG_CONTROL 0x02
#define WMR_MS_HOLOLENS_MSG_CONTROL 0x02 // Integrated motion controller messages?
#define WMR_MS_HOLOLENS_MSG_DEBUG 0x03
#define WMR_MS_HOLOLENS_MSG_UNKNOWN_05 0x05
#define WMR_MS_HOLOLENS_MSG_UNKNOWN_06 0x06
#define WMR_MS_HOLOLENS_MSG_UNKNOWN_0E 0x0E
#define WMR_MS_HOLOLENS_MSG_UNKNOWN_17 0x17
// Messages types specific to WMR Hololens Sensors' companion devices
#define WMR_CONTROL_MSG_IPD_VALUE 0x01
#define WMR_CONTROL_MSG_UNKNOWN_05 0x05