mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-01 12:46:12 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/wmr: Add wmr_source with camera streams support

Browse source
This commit is contained in:
Mateo de Mayo 2021-12-06 08:54:15 -03:00 committed by Jakob Bornecrantz
parent 92c1e55c37
commit 41f42e0b3a
9 changed files with 426 additions and 27 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
src/xrt/drivers/CMakeLists.txt
View file

@ -328,8 +328,10 @@ if(XRT_BUILD_DRIVER_WMR)
wmr/wmr_protocol.h
wmr/wmr_controller_protocol.c
wmr/wmr_controller_protocol.h
wmr/wmr_source.c
wmr/wmr_source.h
)
target_link_libraries(drv_wmr PRIVATE xrt-interfaces aux_util aux_math xrt-external-cjson)
target_link_libraries(drv_wmr PRIVATE xrt-interfaces aux_util aux_math aux_tracking xrt-external-cjson)
list(APPEND ENABLED_HEADSET_DRIVERS wmr)
# Can only build camera support with libusb

2
src/xrt/drivers/meson.build
View file

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

45
src/xrt/drivers/wmr/wmr_camera.c
View file

@ -96,6 +96,10 @@ struct wmr_camera
struct u_sink_debug debug_sinks[2];
bool submit_frames; //!< Whether to push frames to left/right sinks
struct xrt_frame_sink *left_sink; //!< Downstream sinks to push left tracking frames to
struct xrt_frame_sink *right_sink; //!< Downstream sinks to push right tracking frames to
enum u_logging_level log_level;
};
@ -244,7 +248,6 @@ set_active(struct wmr_camera *cam, bool active)
return send_buffer_to_device(cam, (uint8_t *)&cmd, sizeof(cmd));
}
static void LIBUSB_CALL
img_xfer_cb(struct libusb_transfer *xfer)
{
@ -311,7 +314,7 @@ img_xfer_cb(struct libusb_transfer *xfer)
uint16_t unknown16 = read16(&src);
uint16_t unknown16_2 = read16(&src);
WMR_CAM_DEBUG(
WMR_CAM_TRACE(
cam, "Frame start TS %" PRIu64 " (%" PRIi64 " since last) end %" PRIu64 " dt %" PRIi64 " unknown %u %u",
frame_start_ts, frame_start_ts - cam->last_frame_ts, frame_end_ts, delta, unknown16, unknown16_2);
@ -326,19 +329,40 @@ img_xfer_cb(struct libusb_transfer *xfer)
cam->frame_sequence, exposure);
xf->source_sequence = cam->frame_sequence;
xf->timestamp = xf->source_timestamp = frame_start_ts;
xf->timestamp = frame_start_ts + delta / 2;
xf->source_timestamp = frame_start_ts;
cam->last_frame_ts = frame_start_ts;
cam->last_seq = seq;
/* Exposure of 0 is a dark frame for controller tracking */
/* Exposure of 0 is a dark frame for controller tracking (usually ~60fps) */
int sink_index = (exposure == 0) ? 1 : 0;
if (u_sink_debug_is_active(&cam->debug_sinks[sink_index])) {
u_sink_debug_push_frame(&cam->debug_sinks[sink_index], xf);
}
/* TODO: Push frame for tracking */
// Push to sinks
bool tracking_frame = sink_index == 0;
if (tracking_frame && cam->submit_frames) {
// Tracking frames usually come at ~30fps
struct xrt_frame *xf_left = NULL;
struct xrt_frame *xf_right = NULL;
u_frame_create_roi(xf, cam->configs[0].roi, &xf_left);
u_frame_create_roi(xf, cam->configs[1].roi, &xf_right);
if (cam->left_sink != NULL) {
xrt_sink_push_frame(cam->left_sink, xf_left);
}
if (cam->right_sink != NULL) {
xrt_sink_push_frame(cam->right_sink, xf_right);
}
xrt_frame_reference(&xf_left, NULL);
xrt_frame_reference(&xf_right, NULL);
}
xrt_frame_reference(&xf, NULL);
if (cam->last_exposure != cam->debug_exposure || cam->last_gain != cam->debug_gain) {
@ -366,7 +390,10 @@ out:
*/
struct wmr_camera *
wmr_camera_open(struct xrt_prober_device *dev_holo, enum u_logging_level log_level)
wmr_camera_open(struct xrt_prober_device *dev_holo,
struct xrt_frame_sink *left_sink,
struct xrt_frame_sink *right_sink,
enum u_logging_level log_level)
{
struct wmr_camera *cam = calloc(1, sizeof(struct wmr_camera));
int res, i;
@ -416,6 +443,12 @@ wmr_camera_open(struct xrt_prober_device *dev_holo, enum u_logging_level log_lev
u_var_add_log_level(cam, &cam->log_level, "Log level");
u_var_add_u8(cam, &cam->debug_gain, "Gain");
u_var_add_u8(cam, &cam->debug_exposure, "Exposure * 200");
u_var_add_bool(cam, &cam->submit_frames, "Submit frames");
u_var_add_sink_debug(cam, &cam->debug_sinks[0], "Tracking Streams");
u_var_add_sink_debug(cam, &cam->debug_sinks[1], "Controller Streams");
cam->left_sink = left_sink;
cam->right_sink = right_sink;
return cam;

8
src/xrt/drivers/wmr/wmr_camera.h
View file

@ -12,6 +12,7 @@
#include "xrt/xrt_config_have.h"
#include "util/u_logging.h"
#include "xrt/xrt_prober.h"
#include "xrt/xrt_frame.h"
#include "wmr_config.h"
@ -23,7 +24,10 @@ struct wmr_camera;
#ifdef XRT_HAVE_LIBUSB
struct wmr_camera *
wmr_camera_open(struct xrt_prober_device *dev_holo, enum u_logging_level log_level);
wmr_camera_open(struct xrt_prober_device *dev_holo,
struct xrt_frame_sink *left_sink,
struct xrt_frame_sink *right_sink,
enum u_logging_level log_level);
void
wmr_camera_free(struct wmr_camera *cam);
@ -51,7 +55,7 @@ wmr_camera_set_exposure_gain(struct wmr_camera *cam, uint8_t camera_id, uint16_t
#else
/* Stubs to disable camera functions without libusb */
#define wmr_camera_open(dev_holo, ll) NULL
#define wmr_camera_open(dev_holo, left_sink, right_sink, log_level) NULL
#define wmr_camera_free(cam)
#define wmr_camera_start(cam, cam_configs, n_configs) false
#define wmr_camera_stop(cam) false

35
src/xrt/drivers/wmr/wmr_hmd.c
View file

@ -34,6 +34,7 @@
#include "wmr_common.h"
#include "wmr_config_key.h"
#include "wmr_protocol.h"
#include "wmr_source.h"
#include <stdio.h>
#include <stdlib.h>
@ -843,16 +844,15 @@ wmr_hmd_destroy(struct xrt_device *xdev)
wh->hid_control_dev = NULL;
}
if (wh->camera != NULL) {
wmr_camera_free(wh->camera);
}
// Destroy SLAM source and tracker
xrt_frame_context_destroy_nodes(&wh->slam.xfctx);
// Destroy the fusion.
m_imu_3dof_close(&wh->fusion.i3dof);
os_mutex_destroy(&wh->fusion.mutex);
free(wh);
u_device_free(&wh->base);
}
static bool
@ -1012,7 +1012,7 @@ struct xrt_device *
wmr_hmd_create(enum wmr_headset_type hmd_type,
struct os_hid_device *hid_holo,
struct os_hid_device *hid_ctrl,
struct wmr_camera *cam,
struct xrt_prober_device *dev_holo,
enum u_logging_level log_level)
{
enum u_device_alloc_flags flags =
@ -1067,12 +1067,6 @@ wmr_hmd_create(enum wmr_headset_type hmd_type,
return NULL;
}
wh->slam.enabled = slam_enabled;
wh->use_slam_tracker = slam_enabled;
wh->pose = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
wh->offset = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
// Setup input.
wh->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
@ -1084,6 +1078,14 @@ wmr_hmd_create(enum wmr_headset_type hmd_type,
return NULL;
}
// Setup data source
wh->slam.source = wmr_source_create(&wh->slam.xfctx, dev_holo, wh->config);
wh->slam.enabled = slam_enabled;
wh->use_slam_tracker = slam_enabled;
wh->pose = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
wh->offset = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
/* Now that we have the config loaded, iterate the map of known headsets and see if we have
* an entry for this specific headset (otherwise the generic entry will be used)
*/
@ -1223,8 +1225,17 @@ wmr_hmd_create(enum wmr_headset_type hmd_type,
hololens_sensors_enable_imu(wh);
//! @todo Initialize SLAM tracker
struct xrt_slam_sinks *sinks = NULL;
//! @todo Stream data source into sinks (if populated)
// Stream data source into sinks (if populated)
bool stream_started = xrt_fs_slam_stream_start(wh->slam.source, sinks);
if (!stream_started) {
//! @todo Could reach this due to !XRT_HAVE_LIBUSB but the HMD should keep working
WMR_WARN(wh, "Failed to start WMR source");
wmr_hmd_destroy(&wh->base);
wh = NULL;
return NULL;
}
// Hand over hololens sensor device to reading thread.
ret = os_thread_helper_start(&wh->oth, wmr_run_thread, wh);

11
src/xrt/drivers/wmr/wmr_hmd.h
View file

@ -14,6 +14,7 @@
#pragma once
#include "xrt/xrt_device.h"
#include "xrt/xrt_frame.h"
#include "xrt/xrt_prober.h"
#include "os/os_threading.h"
#include "math/m_imu_3dof.h"
@ -82,8 +83,6 @@ struct wmr_hmd
//! Packet reading thread.
struct os_thread_helper oth;
struct wmr_camera *camera;
enum u_logging_level log_level;
/*!
@ -131,6 +130,12 @@ struct wmr_hmd
struct
{
//! Source of video/IMU data for SLAM
struct xrt_fs *source;
//! Context for @ref source
struct xrt_frame_context xfctx;
//! Set at start. Whether the tracker was initialized.
bool enabled;
} slam;
@ -160,7 +165,7 @@ struct xrt_device *
wmr_hmd_create(enum wmr_headset_type hmd_type,
struct os_hid_device *hid_holo,
struct os_hid_device *hid_ctrl,
struct wmr_camera *cam,
struct xrt_prober_device *dev_holo,
enum u_logging_level log_level);
#define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)

4
src/xrt/drivers/wmr/wmr_prober.c
View file

@ -196,9 +196,7 @@ wmr_found(struct xrt_prober *xp,
return -1;
}
struct wmr_camera *cam = wmr_camera_open(dev_holo, log_level);
struct xrt_device *p = wmr_hmd_create(hmd_type, hid_holo, hid_companion, cam, log_level);
struct xrt_device *p = wmr_hmd_create(hmd_type, hid_holo, hid_companion, dev_holo, log_level);
if (!p) {
U_LOG_IFL_E(log_level, "Failed to create WMR HMD device.");
return -1;

306
src/xrt/drivers/wmr/wmr_source.c Normal file
View file

@ -0,0 +1,306 @@
// Copyright 2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief WMR camera and IMU data source.
* @author Mateo de Mayo <mateo.demayo@collabora.com>
* @ingroup drv_wmr
*/
#include "wmr_source.h"
#include "wmr_camera.h"
#include "wmr_config.h"
#include "wmr_protocol.h"
#include "math/m_api.h"
#include "math/m_filter_fifo.h"
#include "util/u_debug.h"
#include "util/u_sink.h"
#include "util/u_var.h"
#include "xrt/xrt_tracking.h"
#include "xrt/xrt_frameserver.h"
#include <assert.h>
#include <stdio.h>
#define WMR_SOURCE_STR "WMR Source"
#define WMR_TRACE(w, ...) U_LOG_IFL_T(w->log_level, __VA_ARGS__)
#define WMR_DEBUG(w, ...) U_LOG_IFL_D(w->log_level, __VA_ARGS__)
#define WMR_INFO(w, ...) U_LOG_IFL_I(w->log_level, __VA_ARGS__)
#define WMR_WARN(w, ...) U_LOG_IFL_W(w->log_level, __VA_ARGS__)
#define WMR_ERROR(w, ...) U_LOG_IFL_E(w->log_level, __VA_ARGS__)
#define WMR_ASSERT(predicate, ...) \
do { \
bool p = predicate; \
if (!p) { \
U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
assert(false && "WMR_ASSERT failed: " #predicate); \
exit(EXIT_FAILURE); \
} \
} while (false);
#define WMR_ASSERT_(predicate) WMR_ASSERT(predicate, "Assertion failed " #predicate)
DEBUG_GET_ONCE_LOG_OPTION(wmr_log, "WMR_LOG", U_LOGGING_INFO)
/*!
* Handles all the data sources from the WMR driver
*
* @todo Currently only properly handling tracking cameras, move IMU and other sources here
* @implements xrt_fs
* @implements xrt_frame_node
*/
struct wmr_source
{
struct xrt_fs xfs;
struct xrt_frame_node node;
enum u_logging_level log_level; //!< Log level
struct wmr_hmd_config config;
struct wmr_camera *camera;
// Sinks
struct xrt_frame_sink left_sink; //!< Intermediate sink for left camera frames
struct xrt_frame_sink right_sink; //!< Intermediate sink for right camera frames
struct xrt_imu_sink imu_sink; //!< Intermediate sink for IMU samples
struct xrt_slam_sinks in_sinks; //!< Pointers to intermediate sinks
struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks
// UI Sinks
struct u_sink_debug ui_left_sink; //!< Sink to display left frames in UI
struct u_sink_debug ui_right_sink; //!< Sink to display right frames in UI
struct m_ff_vec3_f32 *gyro_ff; //!< Queue of gyroscope data to display in UI
struct m_ff_vec3_f32 *accel_ff; //!< Queue of accelerometer data to display in UI
bool is_running; //!< Whether the device is streaming
bool average_imus; //!< Average 4 IMU samples before sending them to the sinks
};
/*
*
* Sinks functionality
*
*/
static void
receive_left_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
{
struct wmr_source *ws = container_of(sink, struct wmr_source, left_sink);
WMR_TRACE(ws, "left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
u_sink_debug_push_frame(&ws->ui_left_sink, xf);
if (ws->out_sinks.left) {
xrt_sink_push_frame(ws->out_sinks.left, xf);
}
}
static void
receive_right_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
{
struct wmr_source *ws = container_of(sink, struct wmr_source, right_sink);
WMR_TRACE(ws, "right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
u_sink_debug_push_frame(&ws->ui_right_sink, xf);
if (ws->out_sinks.right) {
xrt_sink_push_frame(ws->out_sinks.right, xf);
}
}
static void
receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
{
struct wmr_source *ws = container_of(sink, struct wmr_source, imu_sink);
timepoint_ns ts = s->timestamp_ns;
struct xrt_vec3_f64 a = s->accel_m_s2;
struct xrt_vec3_f64 w = s->gyro_rad_secs;
WMR_TRACE(ws, "imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z);
// Push to debug UI
struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z};
struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z};
m_ff_vec3_f32_push(ws->gyro_ff, &gyro, ts);
m_ff_vec3_f32_push(ws->accel_ff, &accel, ts);
if (ws->out_sinks.imu) {
xrt_sink_push_imu(ws->out_sinks.imu, s);
}
}
/*
*
* Frameserver functionality
*
*/
static inline struct wmr_source *
wmr_source_from_xfs(struct xrt_fs *xfs)
{
struct wmr_source *ws = container_of(xfs, struct wmr_source, xfs);
return ws;
}
static bool
wmr_source_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count)
{
WMR_ASSERT(false, "Not implemented");
return false;
}
static bool
wmr_source_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp)
{
WMR_ASSERT(false, "Not implemented");
return false;
}
static bool
wmr_source_stream_stop(struct xrt_fs *xfs)
{
struct wmr_source *ws = wmr_source_from_xfs(xfs);
bool stopped = wmr_camera_stop(ws->camera);
if (!stopped) {
WMR_ERROR(ws, "Unable to stop WMR cameras");
WMR_ASSERT_(false);
}
return stopped;
}
static bool
wmr_source_is_running(struct xrt_fs *xfs)
{
struct wmr_source *ws = wmr_source_from_xfs(xfs);
return ws->is_running;
}
static bool
wmr_source_stream_start(struct xrt_fs *xfs,
struct xrt_frame_sink *xs,
enum xrt_fs_capture_type capture_type,
uint32_t descriptor_index)
{
struct wmr_source *ws = wmr_source_from_xfs(xfs);
if (xs == NULL && capture_type == XRT_FS_CAPTURE_TYPE_TRACKING) {
WMR_INFO(ws, "Starting WMR stream in tracking mode");
} else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
WMR_INFO(ws, "Starting WMR stream in calibration mode, will stream only left frames");
ws->out_sinks.left = xs;
} else {
WMR_ASSERT(false, "Unsupported stream configuration xs=%p capture_type=%d", (void *)xs, capture_type);
return false;
}
bool started = wmr_camera_start(ws->camera, ws->config.cameras, ws->config.n_cameras);
if (!started) {
WMR_ERROR(ws, "Unable to start WMR cameras");
WMR_ASSERT_(false);
}
ws->is_running = started;
return ws->is_running;
}
static bool
wmr_source_slam_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks)
{
struct wmr_source *ws = wmr_source_from_xfs(xfs);
if (sinks != NULL) {
ws->out_sinks = *sinks;
}
return wmr_source_stream_start(xfs, NULL, XRT_FS_CAPTURE_TYPE_TRACKING, 0);
}
/*
*
* Frame node functionality
*
*/
static void
wmr_source_node_break_apart(struct xrt_frame_node *node)
{
struct wmr_source *ws = container_of(node, struct wmr_source, node);
wmr_source_stream_stop(&ws->xfs);
}
static void
wmr_source_node_destroy(struct xrt_frame_node *node)
{
struct wmr_source *ws = container_of(node, struct wmr_source, node);
WMR_DEBUG(ws, "Destroying WMR source");
u_sink_debug_destroy(&ws->ui_left_sink);
u_sink_debug_destroy(&ws->ui_right_sink);
m_ff_vec3_f32_free(&ws->gyro_ff);
m_ff_vec3_f32_free(&ws->accel_ff);
u_var_remove_root(ws);
if (ws->camera != NULL) { // It could be null if XRT_HAVE_LIBUSB is not defined
wmr_camera_free(ws->camera);
}
free(ws);
}
/*
*
* Exported functions
*
*/
//! Create and open the frame server for IMU/camera streaming.
struct xrt_fs *
wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev_holo, struct wmr_hmd_config cfg)
{
struct wmr_source *ws = U_TYPED_CALLOC(struct wmr_source);
ws->log_level = debug_get_log_option_wmr_log();
ws->average_imus = true;
// Setup xrt_fs
struct xrt_fs *xfs = &ws->xfs;
xfs->enumerate_modes = wmr_source_enumerate_modes;
xfs->configure_capture = wmr_source_configure_capture;
xfs->stream_start = wmr_source_stream_start;
xfs->slam_stream_start = wmr_source_slam_stream_start;
xfs->stream_stop = wmr_source_stream_stop;
xfs->is_running = wmr_source_is_running;
snprintf(xfs->name, sizeof(xfs->name), WMR_SOURCE_STR);
snprintf(xfs->product, sizeof(xfs->product), WMR_SOURCE_STR " Product");
snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), WMR_SOURCE_STR " Manufacturer");
snprintf(xfs->serial, sizeof(xfs->serial), WMR_SOURCE_STR " Serial");
xfs->source_id = 0x574d522d53524300; // WMR_SRC\0 in hex
// Setup sinks
ws->left_sink.push_frame = receive_left_frame;
ws->right_sink.push_frame = receive_right_frame;
ws->imu_sink.push_imu = receive_imu_sample;
ws->in_sinks.left = &ws->left_sink;
ws->in_sinks.right = &ws->right_sink;
ws->in_sinks.imu = &ws->imu_sink;
ws->camera = wmr_camera_open(dev_holo, ws->in_sinks.left, ws->in_sinks.right, ws->log_level);
ws->config = cfg;
// Setup UI
u_sink_debug_init(&ws->ui_left_sink);
u_sink_debug_init(&ws->ui_right_sink);
m_ff_vec3_f32_alloc(&ws->gyro_ff, 1000);
m_ff_vec3_f32_alloc(&ws->accel_ff, 1000);
u_var_add_root(ws, WMR_SOURCE_STR, false);
u_var_add_log_level(ws, &ws->log_level, "Log Level");
u_var_add_bool(ws, &ws->average_imus, "Average IMU samples");
u_var_add_ro_ff_vec3_f32(ws, ws->gyro_ff, "Gyroscope");
u_var_add_ro_ff_vec3_f32(ws, ws->accel_ff, "Accelerometer");
u_var_add_sink_debug(ws, &ws->ui_left_sink, "Left Camera");
u_var_add_sink_debug(ws, &ws->ui_right_sink, "Right Camera");
// Setup node
struct xrt_frame_node *xfn = &ws->node;
xfn->break_apart = wmr_source_node_break_apart;
xfn->destroy = wmr_source_node_destroy;
xrt_frame_context_add(xfctx, &ws->node);
WMR_DEBUG(ws, "WMR Source created");
return xfs;
}

38
src/xrt/drivers/wmr/wmr_source.h Normal file
View file

@ -0,0 +1,38 @@
// Copyright 2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Interface for WMR data sources
* @author Mateo de Mayo <mateo.demayo@collabora.com>
* @ingroup drv_wmr
*/
#pragma once
#include "wmr_config.h"
#include "xrt/xrt_frameserver.h"
#include "xrt/xrt_prober.h"
/*!
* WMR video/IMU data sources
*
* @addtogroup drv_wmr
* @{
*/
#ifdef __cplusplus
extern "C" {
#endif
//! Create and return the data source as a @ref xrt_fs ready for data streaming.
struct xrt_fs *
wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev_holo, struct wmr_hmd_config cfg);
/*!
* @}
*/
#ifdef __cplusplus
}
#endif