From a8a4d8c3dd48be7a1a4bdc36d5dae7cc27fa0dc9 Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Sat, 14 Mar 2020 23:33:49 +0000 Subject: [PATCH] d/arduino: Add new flexible arduino based input device --- .gitignore | 5 + CMakeLists.txt | 8 +- doc/changes/drivers/mr.251.md | 1 + src/xrt/drivers/CMakeLists.txt | 13 + src/xrt/drivers/arduino/arduino_device.c | 436 ++++++++++++++++++++ src/xrt/drivers/arduino/arduino_interface.h | 54 +++ src/xrt/drivers/arduino/arduino_prober.c | 109 +++++ src/xrt/drivers/arduino/device/device.ino | 248 +++++++++++ src/xrt/targets/common/CMakeLists.txt | 4 + src/xrt/targets/common/target_lists.c | 9 + 10 files changed, 886 insertions(+), 1 deletion(-) create mode 100644 doc/changes/drivers/mr.251.md create mode 100644 src/xrt/drivers/arduino/arduino_device.c create mode 100644 src/xrt/drivers/arduino/arduino_interface.h create mode 100644 src/xrt/drivers/arduino/arduino_prober.c create mode 100644 src/xrt/drivers/arduino/device/device.ino diff --git a/.gitignore b/.gitignore index 34e5e822f..e0487e6e0 100644 --- a/.gitignore +++ b/.gitignore @@ -37,8 +37,13 @@ build*/ # Ignore patches generated by scripts patches/ +# Imgui settings imgui.ini # files from package building obj-*/ .pc/ + +# Arduino toolchain files +src/xrt/drivers/arduino/device/*.elf +src/xrt/drivers/arduino/device/*.bin diff --git a/CMakeLists.txt b/CMakeLists.txt index 8151752f7..ada44a51e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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_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_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. 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) # 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 @@ -153,6 +154,11 @@ if(BUILD_WITH_SDL2) set(BUILD_TARGET_GUI TRUE) endif() + +if(BUILD_WITH_ARDUINO) + set(BUILD_DRIVER_ARDUINO TRUE) +endif() + if(BUILD_WITH_DUMMY) set(BUILD_DRIVER_DUMMY TRUE) endif() diff --git a/doc/changes/drivers/mr.251.md b/doc/changes/drivers/mr.251.md new file mode 100644 index 000000000..791516f60 --- /dev/null +++ b/doc/changes/drivers/mr.251.md @@ -0,0 +1 @@ +Added a Arduino based flexible input device driver, along with Arduino C++ code for it. diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 4dd379913..b8a15c3bc 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -5,6 +5,19 @@ set(ENABLED_HEADSET_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) set(DAYDREAM_SOURCE_FILES daydream/daydream_device.c diff --git a/src/xrt/drivers/arduino/arduino_device.c b/src/xrt/drivers/arduino/arduino_device.c new file mode 100644 index 000000000..42d82a8e4 --- /dev/null +++ b/src/xrt/drivers/arduino/arduino_device.c @@ -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 + * @author Jakob Bornecrantz + * @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 +#include +#include + + +/* + * + * 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; +} diff --git a/src/xrt/drivers/arduino/arduino_interface.h b/src/xrt/drivers/arduino/arduino_interface.h new file mode 100644 index 000000000..0bae414a8 --- /dev/null +++ b/src/xrt/drivers/arduino/arduino_interface.h @@ -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 + * @author Jakob Bornecrantz + * @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 diff --git a/src/xrt/drivers/arduino/arduino_prober.c b/src/xrt/drivers/arduino/arduino_prober.c new file mode 100644 index 000000000..525862d1a --- /dev/null +++ b/src/xrt/drivers/arduino/arduino_prober.c @@ -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 + * @ingroup drv_arduino + */ + +#include +#include +#include + +#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; +} diff --git a/src/xrt/drivers/arduino/device/device.ino b/src/xrt/drivers/arduino/device/device.ino new file mode 100644 index 000000000..fb973e427 --- /dev/null +++ b/src/xrt/drivers/arduino/device/device.ino @@ -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 + * @author Jakob Bornecrantz + * @ingroup drv_arduino + */ + +#include "mbed.h" +#include +#include +#include + + +#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 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); +} diff --git a/src/xrt/targets/common/CMakeLists.txt b/src/xrt/targets/common/CMakeLists.txt index d02389b9f..bb5b621ca 100644 --- a/src/xrt/targets/common/CMakeLists.txt +++ b/src/xrt/targets/common/CMakeLists.txt @@ -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 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +if(BUILD_DRIVER_ARDUINO) + target_link_libraries(target_lists PRIVATE drv_arduino) +endif() + if(BUILD_DRIVER_DAYDREAM) target_link_libraries(target_lists PRIVATE drv_daydream) endif() diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index c1527cc95..3b6f591d3 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -10,6 +10,10 @@ #include "target_lists.h" +#ifdef XRT_BUILD_DRIVER_ARDUINO +#include "arduino/arduino_interface.h" +#endif + #ifdef XRT_BUILD_DRIVER_DUMMY #include "dummy/dummy_interface.h" #endif @@ -96,6 +100,11 @@ xrt_auto_prober_creator target_auto_list[] = { psvr_create_auto_prober, #endif +#ifdef XRT_BUILD_DRIVER_ARDUINO + // Before OpenHMD + arduino_create_auto_prober, +#endif + #ifdef XRT_BUILD_DRIVER_DAYDREAM // Before OpenHMD daydream_create_auto_prober,