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

d/arduino: Add new flexible arduino based input device

Browse source
This commit is contained in:
Jakob Bornecrantz 2020-03-14 23:33:49 +00:00 committed by Jakob Bornecrantz
parent bc53be8562
commit a8a4d8c3dd
10 changed files with 886 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
.gitignore vendored
View file

@ -37,8 +37,13 @@ build*/
# Ignore patches generated by scripts # Ignore patches generated by scripts
patches/ patches/
# Imgui settings
imgui.ini imgui.ini
# files from package building # files from package building
obj-*/ obj-*/
.pc/ .pc/
# Arduino toolchain files
src/xrt/drivers/arduino/device/*.elf
src/xrt/drivers/arduino/device/*.bin

8
CMakeLists.txt
View file

@ -92,6 +92,7 @@ cmake_dependent_option(BUILD_WITH_VIVE "Enable Vive driver" ON "ZLIB_FOUND" OFF)
cmake_dependent_option(BUILD_WITH_OPENHMD "Enable OpenHMD driver" ON "OPENHMD_FOUND" OFF) cmake_dependent_option(BUILD_WITH_OPENHMD "Enable OpenHMD driver" ON "OPENHMD_FOUND" OFF)
cmake_dependent_option(BUILD_WITH_SDL2 "Enable SDL2 based test application" ON "SDL2_FOUND" OFF) cmake_dependent_option(BUILD_WITH_SDL2 "Enable SDL2 based test application" ON "SDL2_FOUND" OFF)
cmake_dependent_option(BUILD_WITH_DAYDREAM "Enable Bluetooth LE via DBUS" ON "BUILD_WITH_DBUS" OFF) cmake_dependent_option(BUILD_WITH_DAYDREAM "Enable Bluetooth LE via DBUS" ON "BUILD_WITH_DBUS" OFF)
cmake_dependent_option(BUILD_WITH_ARDUINO "Enable Arduino input device with BLE via DBUS" ON "BUILD_WITH_DBUS" OFF)
# These all use the Monado internal hid wrapper which is assumed to be available. # These all use the Monado internal hid wrapper which is assumed to be available.
option(BUILD_WITH_HDK "Enable HDK driver" ON) option(BUILD_WITH_HDK "Enable HDK driver" ON)
@ -100,7 +101,7 @@ option(BUILD_WITH_HYDRA "Enable Hydra driver" ON)
option(BUILD_WITH_NS "Enable North Star driver" ON) option(BUILD_WITH_NS "Enable North Star driver" ON)
# You can set this from a superproject to add a driver # You can set this from a superproject to add a driver
list(APPEND AVAILABLE_DRIVERS DUMMY HDK HYDRA NS OHMD PSMV PSVR RS V4L2 VIVE DAYDREAM) list(APPEND AVAILABLE_DRIVERS ARDUINO DUMMY HDK HYDRA NS OHMD PSMV PSVR RS V4L2 VIVE DAYDREAM)
### ###
# Flags # Flags
@ -153,6 +154,11 @@ if(BUILD_WITH_SDL2)
set(BUILD_TARGET_GUI TRUE) set(BUILD_TARGET_GUI TRUE)
endif() endif()
if(BUILD_WITH_ARDUINO)
set(BUILD_DRIVER_ARDUINO TRUE)
endif()
if(BUILD_WITH_DUMMY) if(BUILD_WITH_DUMMY)
set(BUILD_DRIVER_DUMMY TRUE) set(BUILD_DRIVER_DUMMY TRUE)
endif() endif()

1
doc/changes/drivers/mr.251.md Normal file
View file

@ -0,0 +1 @@
Added a Arduino based flexible input device driver, along with Arduino C++ code for it.

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

@ -5,6 +5,19 @@
set(ENABLED_HEADSET_DRIVERS) set(ENABLED_HEADSET_DRIVERS)
set(ENABLED_DRIVERS) set(ENABLED_DRIVERS)
if(BUILD_DRIVER_ARDUINO)
set(ARDUINO_SOURCE_FILES
arduino/arduino_device.c
arduino/arduino_interface.h
arduino/arduino_prober.c
)
add_library(drv_arduino STATIC ${ARDUINO_SOURCE_FILES})
target_link_libraries(drv_arduino PRIVATE xrt-interfaces aux_util aux_os)
list(APPEND ENABLED_DRIVERS arduino)
endif()
if(BUILD_DRIVER_DAYDREAM) if(BUILD_DRIVER_DAYDREAM)
set(DAYDREAM_SOURCE_FILES set(DAYDREAM_SOURCE_FILES
daydream/daydream_device.c daydream/daydream_device.c

436
src/xrt/drivers/arduino/arduino_device.c Normal file
View file

@ -0,0 +1,436 @@
// Copyright 2019-2020, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Arduino felxable input device code.
* @author Pete Black <pete.black@collabora.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_arduino
*/
#include "xrt/xrt_device.h"
#include "xrt/xrt_prober.h"
#include "xrt/xrt_tracking.h"
#include "os/os_ble.h"
#include "os/os_time.h"
#include "os/os_threading.h"
#include "math/m_api.h"
#include "math/m_imu_pre.h"
#include "math/m_imu_3dof.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 "util/u_bitwise.h"
#include "arduino_interface.h"
#include <stdio.h>
#include <math.h>
#include <assert.h>
/*
*
* Structs.
*
*/
/*!
* A parsed sample of accel and gyro.
*/
struct arduino_parsed_sample
{
uint32_t time;
uint32_t delta;
struct xrt_vec3_i32 accel;
struct xrt_vec3_i32 gyro;
};
struct arduino_parsed_input
{
uint32_t timestamp;
struct arduino_parsed_sample sample;
};
struct arduino_device
{
struct xrt_device base;
struct os_ble_device *ble;
struct os_thread_helper oth;
struct
{
//! Device time.
uint64_t device_time;
//! Lock for last and fusion.
struct os_mutex lock;
uint64_t last_time;
//! Pre filter for the IMU.
struct m_imu_pre_filter pre_filter;
struct m_imu_3dof fusion;
};
struct
{
bool last;
} gui;
bool print_spew;
bool print_debug;
};
/*
*
* Smaller helper functions.
*
*/
#define ARDUINO_SPEW(c, ...) \
do { \
if (c->print_spew) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define ARDUINO_DEBUG(c, ...) \
do { \
if (c->print_debug) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define ARDUINO_ERROR(c, ...) \
do { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} while (false)
static inline struct arduino_device *
arduino_device(struct xrt_device *xdev)
{
return (struct arduino_device *)xdev;
}
static uint32_t
calc_delta_and_handle_rollover(uint32_t next, uint32_t last)
{
uint32_t tick_delta = next - last;
// The 24-bit tick counter has rolled over,
// adjust the "negative" value to be positive.
if (tick_delta > 0xffffff) {
tick_delta += 0x1000000;
}
return tick_delta;
}
static int16_t
read_i16(const uint8_t *buffer, size_t offset)
{
return (buffer[offset] << 8) | buffer[offset + 1];
}
/*
*
* Internal functions.
*
*/
static void
update_fusion(struct arduino_device *ad,
struct arduino_parsed_sample *sample,
timepoint_ns timestamp_ns,
time_duration_ns delta_ns)
{
struct xrt_vec3 accel, gyro;
m_imu_pre_filter_data(&ad->pre_filter, &sample->accel, &sample->gyro,
&accel, &gyro);
ad->device_time += (uint64_t)sample->delta * 1000;
m_imu_3dof_update(&ad->fusion, ad->device_time, &accel, &gyro);
double delta_device_ms = (double)sample->delta / 1000.0;
double delta_host_ms = (double)delta_ns / (1000.0 * 1000.0);
ARDUINO_DEBUG(ad, "%+fms %+fms", delta_host_ms, delta_device_ms);
ARDUINO_DEBUG(
ad, "fusion sample %u (ax %d ay %d az %d) (gx %d gy %d gz %d)",
sample->time, sample->accel.x, sample->accel.y, sample->accel.z,
sample->gyro.x, sample->gyro.y, sample->gyro.z);
ARDUINO_DEBUG(ad, "\n");
}
static void
arduino_parse_input(struct arduino_device *ad,
void *data,
struct arduino_parsed_input *input)
{
U_ZERO(input);
unsigned char *b = (unsigned char *)data;
ARDUINO_SPEW(
ad,
"raw input: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x "
"%02x %02x %02x %02x %02x %02x %02x %02x %02x",
b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7], b[8], b[9], b[10],
b[11], b[12], b[13], b[14], b[15], b[16], b[17], b[18], b[19]);
uint32_t time = b[5] | b[4] << 8 | b[3] << 16;
input->sample.time = time;
input->sample.delta =
calc_delta_and_handle_rollover(time, ad->last_time);
ad->last_time = time;
input->sample.accel.x = read_i16(b, 6);
input->sample.accel.y = read_i16(b, 8);
input->sample.accel.z = read_i16(b, 10);
input->sample.gyro.x = read_i16(b, 12);
input->sample.gyro.y = read_i16(b, 14);
input->sample.gyro.z = read_i16(b, 16);
}
/*!
* Reads one packet from the device,handles locking and checking if
* the thread has been told to shut down.
*/
static bool
arduino_read_one_packet(struct arduino_device *ad, uint8_t *buffer, size_t size)
{
os_thread_helper_lock(&ad->oth);
while (os_thread_helper_is_running_locked(&ad->oth)) {
int retries = 5;
int ret = -1;
os_thread_helper_unlock(&ad->oth);
while (retries > 0) {
ret = os_ble_read(ad->ble, buffer, size, 500);
if (ret == (int)size) {
break;
}
retries--;
}
if (ret == 0) {
fprintf(stderr, "%s\n", __func__);
// Must lock thread before check in while.
os_thread_helper_lock(&ad->oth);
continue;
}
if (ret < 0) {
ARDUINO_ERROR(arduino, "Failed to read device '%i'!",
ret);
return false;
}
return true;
}
return false;
}
static void *
arduino_run_thread(void *ptr)
{
struct arduino_device *ad = (struct arduino_device *)ptr;
uint8_t buffer[20];
timepoint_ns then_ns, now_ns;
struct arduino_parsed_input input; // = {0};
// wait for a package to sync up, it's discarded but that's okay.
if (!arduino_read_one_packet(ad, buffer, 20)) {
return NULL;
}
then_ns = os_monotonic_get_ns();
while (arduino_read_one_packet(ad, buffer, 20)) {
// As close to when we get a packet.
now_ns = os_monotonic_get_ns();
// Parse the data we got.
arduino_parse_input(ad, buffer, &input);
time_duration_ns delta_ns = now_ns - then_ns;
then_ns = now_ns;
// Lock last and the fusion.
os_mutex_lock(&ad->lock);
// Process the parsed data.
update_fusion(ad, &input.sample, now_ns, delta_ns);
// Now done.
os_mutex_unlock(&ad->lock);
}
return NULL;
}
/*
*
* Device functions.
*
*/
static void
arduino_get_fusion_pose(struct arduino_device *ad,
enum xrt_input_name name,
timepoint_ns when,
struct xrt_space_relation *out_relation)
{
out_relation->pose.orientation = ad->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);
}
static void
arduino_device_destroy(struct xrt_device *xdev)
{
struct arduino_device *ad = arduino_device(xdev);
// Destroy the thread object.
os_thread_helper_destroy(&ad->oth);
// Now that the thread is not running we can destroy the lock.
os_mutex_destroy(&ad->lock);
// Remove the variable tracking.
u_var_remove_root(ad);
// Destroy the fusion.
m_imu_3dof_close(&ad->fusion);
// Does null checking and zeros.
os_ble_destroy(&ad->ble);
free(ad);
}
static void
arduino_device_update_inputs(struct xrt_device *xdev,
struct time_state *timekeeping)
{
struct arduino_device *ad = arduino_device(xdev);
int64_t now = time_state_get_now(timekeeping);
// Lock the data.
os_mutex_lock(&ad->lock);
ad->base.inputs[0].timestamp = now;
ad->base.inputs[1].timestamp = now;
ad->base.inputs[2].timestamp = now;
ad->base.inputs[3].timestamp = now;
ad->base.inputs[4].timestamp = now;
ad->base.inputs[5].timestamp = now;
ad->base.inputs[6].timestamp = now;
ad->base.inputs[7].timestamp = now;
// Done now.
os_mutex_unlock(&ad->lock);
}
static void
arduino_device_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
struct time_state *timekeeping,
int64_t *out_timestamp,
struct xrt_space_relation *out_relation)
{
struct arduino_device *ad = arduino_device(xdev);
timepoint_ns now = time_state_get_now(timekeeping);
arduino_get_fusion_pose(ad, name, now, out_relation);
}
/*
*
* Prober functions.
*
*/
struct xrt_device *
arduino_device_create(struct os_ble_device *ble,
bool print_spew,
bool print_debug)
{
enum u_device_alloc_flags flags =
(enum u_device_alloc_flags)(U_DEVICE_ALLOC_TRACKING_NONE);
struct arduino_device *ad =
U_DEVICE_ALLOCATE(struct arduino_device, flags, 8, 0);
ad->base.name = XRT_DEVICE_DAYDREAM;
ad->base.destroy = arduino_device_destroy;
ad->base.update_inputs = arduino_device_update_inputs;
ad->base.get_tracked_pose = arduino_device_get_tracked_pose;
ad->base.inputs[0].name = XRT_INPUT_DAYDREAM_POSE;
ad->base.inputs[1].name = XRT_INPUT_DAYDREAM_TOUCHPAD_CLICK;
ad->base.inputs[2].name = XRT_INPUT_DAYDREAM_BAR_CLICK;
ad->base.inputs[3].name = XRT_INPUT_DAYDREAM_CIRCLE_CLICK;
ad->base.inputs[4].name = XRT_INPUT_DAYDREAM_VOLDN_CLICK;
ad->base.inputs[5].name = XRT_INPUT_DAYDREAM_VOLUP_CLICK;
ad->base.inputs[6].name = XRT_INPUT_DAYDREAM_TOUCHPAD_VALUE_X;
ad->base.inputs[7].name = XRT_INPUT_DAYDREAM_TOUCHPAD_VALUE_Y;
ad->ble = ble;
ad->print_spew = print_spew;
ad->print_debug = print_debug;
m_imu_3dof_init(&ad->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_300MS);
#define DEG_TO_RAD ((double)M_PI / 180.0)
float accel_ticks_to_float = (4.0 * MATH_GRAVITY_M_S2) / INT16_MAX;
float gyro_ticks_to_float = (2000.0 * DEG_TO_RAD) / INT16_MAX;
m_imu_pre_filter_init(&ad->pre_filter, accel_ticks_to_float,
gyro_ticks_to_float);
m_imu_pre_filter_set_switch_x_and_y(&ad->pre_filter);
#if 0
ad->pre_filter.gyro.bias.x = 10 * gyro_ticks_to_float;
ad->pre_filter.gyro.bias.y = 10 * gyro_ticks_to_float;
#endif
// Everything done, finally start the thread.
int ret = os_thread_helper_start(&ad->oth, arduino_run_thread, ad);
if (ret != 0) {
ARDUINO_ERROR(dd, "Failed to start thread!");
arduino_device_destroy(&ad->base);
return NULL;
}
u_var_add_root(ad, "Arduino flexible input device", true);
u_var_add_gui_header(ad, &ad->gui.last, "Last");
u_var_add_ro_vec3_f32(ad, &ad->fusion.last.accel, "last.accel");
u_var_add_ro_vec3_f32(ad, &ad->fusion.last.gyro, "last.gyro");
ARDUINO_DEBUG(ad, "Created device!");
return &ad->base;
}

54
src/xrt/drivers/arduino/arduino_interface.h Normal file
View file

@ -0,0 +1,54 @@
// Copyright 2019-2020, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Interer face @ref drv_arduino.
* @author Pete Black <pete.black@collabora.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_arduino
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
struct os_ble_device;
/*!
* @defgroup drv_arduino Arduino flexible input device driver
* @ingroup drv
*
* @brief Driver for the Monado Arduino based flexible input device.
*/
/*!
* Probing function for the Arduino based flexible input device driver.
*
* @ingroup drv_arduino
*/
struct xrt_auto_prober *
arduino_create_auto_prober();
/*!
* Create a arduino device from a ble notify.
*
* @ingroup drv_arduino
*/
struct xrt_device *
arduino_device_create(struct os_ble_device *ble,
bool print_spew,
bool print_debug);
/*!
* @dir drivers/arduino
*
* @brief @ref drv_arduino files.
*/
#ifdef __cplusplus
}
#endif

109
src/xrt/drivers/arduino/arduino_prober.c Normal file
View file

@ -0,0 +1,109 @@
// Copyright 2019-2020, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Arduino felxable input device prober code.
* @author Pete Black <pete.black@collabora.com>
* @ingroup drv_arduino
*/
#include <stdio.h>
#include <stdlib.h>
#include <wchar.h>
#include "os/os_ble.h"
#include "xrt/xrt_prober.h"
#include "util/u_misc.h"
#include "util/u_debug.h"
#include "arduino_interface.h"
/*
*
* Defines & structs.
*
*/
DEBUG_GET_ONCE_BOOL_OPTION(arduino_enable, "ARDUINO_ENABLE", true)
DEBUG_GET_ONCE_BOOL_OPTION(arduino_spew, "ARDUINO_PRINT_SPEW", false)
DEBUG_GET_ONCE_BOOL_OPTION(arduino_debug, "ARDUINO_PRINT_DEBUG", false)
/*!
* Arduino prober struct.
*
* @ingroup drv_arduino
*/
struct arduino_prober
{
struct xrt_auto_prober base;
bool print_spew;
bool print_debug;
bool enabled;
};
/*
*
* Static functions.
*
*/
static inline struct arduino_prober *
arduino_prober(struct xrt_auto_prober *p)
{
return (struct arduino_prober *)p;
}
static void
arduino_prober_destroy(struct xrt_auto_prober *p)
{
struct arduino_prober *ap = arduino_prober(p);
free(ap);
}
static struct xrt_device *
arduino_prober_autoprobe(struct xrt_auto_prober *xap,
bool no_hmds,
struct xrt_prober *xp)
{
struct arduino_prober *ap = arduino_prober(xap);
if (!ap->enabled) {
return NULL;
}
const char *dev_uuid = "00004242-0000-1000-8000-004242424242";
const char *char_uuid = "00000001-1000-1000-8000-004242424242";
struct os_ble_device *ble = NULL;
os_ble_notify_open(dev_uuid, char_uuid, &ble);
if (ble == NULL) {
return NULL;
}
return arduino_device_create(ble, ap->print_spew, ap->print_debug);
}
/*
*
* Exported functions.
*
*/
struct xrt_auto_prober *
arduino_create_auto_prober()
{
struct arduino_prober *ap = U_TYPED_CALLOC(struct arduino_prober);
ap->base.destroy = arduino_prober_destroy;
ap->base.lelo_dallas_autoprobe = arduino_prober_autoprobe;
ap->enabled = debug_get_bool_option_arduino_enable();
ap->print_spew = debug_get_bool_option_arduino_spew();
ap->print_debug = debug_get_bool_option_arduino_debug();
return &ap->base;
}

248
src/xrt/drivers/arduino/device/device.ino Normal file
View file

@ -0,0 +1,248 @@
// Copyright 2019-2020, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Arduino based felxable input device code.
* @author Pete Black <pete.black@collabora.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_arduino
*/
#include "mbed.h"
#include <stdint.h>
#include <ArduinoBLE.h>
#include <Arduino_LSM9DS1.h>
#define USE_SERIAL
#ifdef USE_SERIAL
#define LOG(...) Serial.print(__VA_ARGS__)
#define LOG_LN(...) Serial.println(__VA_ARGS__)
#else
#define LOG(...) (void)NULL
#define LOG_LN(...) (void)NULL
#endif
#define SET(o, v) \
do { \
uint32_t val = v; \
buffer[o + 0] = val >> 8; \
buffer[o + 1] = val; \
} while (false)
struct Sample
{
int32_t acc_x = 0;
int32_t acc_y = 0;
int32_t acc_z = 0;
int32_t gyr_x = 0;
int32_t gyr_y = 0;
int32_t gyr_z = 0;
uint32_t time = 0;
};
/*
*
* Global variables
*
*/
volatile bool isConnected;
BLEService service("00004242-0000-1000-8000-004242424242"); // create service
// create switch characteristic and allow remote device to read and write
BLECharacteristic notify("00000001-1000-1000-8000-004242424242",
BLERead | BLENotify,
20,
true);
rtos::Mutex mutex;
rtos::Thread thread;
rtos::Mail<Sample, 100> mail;
/*
*
* IMU functions.
*
*/
void
imu_loop()
{
if (isConnected == false) {
return;
}
if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable()) {
uint32_t time = micros();
float gyr_x, gyr_y, gyr_z;
float acc_x, acc_y, acc_z;
IMU.readGyroscope(gyr_x, gyr_y, gyr_z);
IMU.readAcceleration(acc_x, acc_y, acc_z);
Sample *sample = mail.alloc();
sample->time = time;
sample->acc_x = acc_x * (32768.0 / 4.0);
sample->acc_y = acc_y * (32768.0 / 4.0);
sample->acc_z = acc_z * (32768.0 / 4.0);
sample->gyr_x = gyr_x * (32768.0 / 2000.0);
sample->gyr_y = gyr_y * (32768.0 / 2000.0);
sample->gyr_z = gyr_z * (32768.0 / 2000.0);
mail.put(sample);
}
}
void
imu_thread()
{
while (true) {
imu_loop();
delay(1);
}
}
/*
*
* BLE functions.
*
*/
void
loop()
{
bool got_mail = false;
int32_t acc_x = 0, acc_y = 0, acc_z = 0;
int32_t gyr_x = 0, gyr_y = 0, gyr_z = 0;
int32_t count = 0;
uint32_t time;
uint8_t buffer[20];
static int countPacket = 0;
BLE.poll();
if (isConnected == false) {
return;
}
while (true) {
osEvent evt = mail.get(0);
if (evt.status != osEventMail) {
break;
}
Sample *sample = (Sample *)evt.value.p;
acc_x += sample->acc_x;
acc_y += sample->acc_y;
acc_z += sample->acc_z;
gyr_x += sample->gyr_x;
gyr_y += sample->gyr_y;
gyr_z += sample->gyr_z;
time = sample->time;
mail.free(sample);
count++;
got_mail = true;
}
if (!got_mail) {
delay(1);
return;
}
buffer[0] = countPacket++;
buffer[1] = count;
buffer[2] = 0;
buffer[3] = time >> 16;
buffer[4] = time >> 8;
buffer[5] = time;
SET(6, (acc_x / count));
SET(8, (acc_y / count));
SET(10, (acc_z / count));
SET(12, (gyr_x / count));
SET(14, (gyr_y / count));
SET(16, (gyr_z / count));
buffer[18] = 0;
buffer[19] = 0;
notify.writeValue(buffer, 20);
}
void
ble_peripheral_connect_handler(BLEDevice peer)
{
LOG("Connected event, central: ");
LOG_LN(peer.address());
isConnected = true;
BLE.poll();
}
void
ble_peripheral_disconnect_handler(BLEDevice peer)
{
LOG("Disconnected event, central: ");
LOG_LN(peer.address());
isConnected = false;
BLE.poll();
}
/*
*
* Main functions.
*
*/
void
block()
{
while (true) {
LOG_LN("BLOCKED");
delay(1000);
}
}
void
setup()
{
#ifdef USE_SERIAL
Serial.begin(9600);
while (!Serial) {
}
#endif
if (!BLE.begin()) {
LOG_LN("BLE initialisation failed!");
block();
}
if (!IMU.begin()) {
LOG_LN("IMU initialisation failed!");
block();
}
BLE.setLocalName("Monado Flexible Controller");
BLE.setAdvertisedService(service);
service.addCharacteristic(notify);
BLE.addService(service);
// Assign event handlers for connected, disconnected to peripheral.
BLE.setEventHandler(BLEConnected, ble_peripheral_connect_handler);
BLE.setEventHandler(BLEDisconnected, ble_peripheral_disconnect_handler);
notify.setValue(0);
// Start advertising.
BLE.advertise();
isConnected = false;
// IMU has it's own thread.
thread.start(imu_thread);
}

4
src/xrt/targets/common/CMakeLists.txt
View file

@ -10,6 +10,10 @@ target_link_libraries(target_lists PRIVATE xrt-interfaces)
target_include_directories(target_lists PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../../drivers) target_include_directories(target_lists PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../../drivers)
target_include_directories(target_lists PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_include_directories(target_lists PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
if(BUILD_DRIVER_ARDUINO)
target_link_libraries(target_lists PRIVATE drv_arduino)
endif()
if(BUILD_DRIVER_DAYDREAM) if(BUILD_DRIVER_DAYDREAM)
target_link_libraries(target_lists PRIVATE drv_daydream) target_link_libraries(target_lists PRIVATE drv_daydream)
endif() endif()

9
src/xrt/targets/common/target_lists.c
View file

@ -10,6 +10,10 @@
#include "target_lists.h" #include "target_lists.h"
#ifdef XRT_BUILD_DRIVER_ARDUINO
#include "arduino/arduino_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_DUMMY #ifdef XRT_BUILD_DRIVER_DUMMY
#include "dummy/dummy_interface.h" #include "dummy/dummy_interface.h"
#endif #endif
@ -96,6 +100,11 @@ xrt_auto_prober_creator target_auto_list[] = {
psvr_create_auto_prober, psvr_create_auto_prober,
#endif #endif
#ifdef XRT_BUILD_DRIVER_ARDUINO
// Before OpenHMD
arduino_create_auto_prober,
#endif
#ifdef XRT_BUILD_DRIVER_DAYDREAM #ifdef XRT_BUILD_DRIVER_DAYDREAM
// Before OpenHMD // Before OpenHMD
daydream_create_auto_prober, daydream_create_auto_prober,