d/arduino: Add new flexible arduino based input device

.gitignore

···
 # 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

CMakeLists.txt

···
 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)
···
 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
···
 
 	# SDL2 based gui
 	set(BUILD_TARGET_GUI TRUE)
+endif()
+
+
+if(BUILD_WITH_ARDUINO)
+	set(BUILD_DRIVER_ARDUINO TRUE)
 endif()
 
 if(BUILD_WITH_DUMMY)

doc/changes/drivers/mr.251.md

···
+Added a Arduino based flexible input device driver, along with Arduino C++ code for it.

src/xrt/drivers/CMakeLists.txt

···
 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

src/xrt/drivers/arduino/arduino_device.c

···
+// Copyright 2019-2020, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Arduino felxable input device code.
+ * @author Pete Black <[e-mail]>
+ * @author Jakob Bornecrantz <[e-mail]>
+ * @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;
+}

src/xrt/drivers/arduino/arduino_interface.h

···
+// Copyright 2019-2020, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Interer face @ref drv_arduino.
+ * @author Pete Black <[e-mail]>
+ * @author Jakob Bornecrantz <[e-mail]>
+ * @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

src/xrt/drivers/arduino/arduino_prober.c

···
+// Copyright 2019-2020, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Arduino felxable input device prober code.
+ * @author Pete Black <[e-mail]>
+ * @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;
+}

src/xrt/drivers/arduino/device/device.ino

···
+// Copyright 2019-2020, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Arduino based felxable input device code.
+ * @author Pete Black <[e-mail]>
+ * @author Jakob Bornecrantz <[e-mail]>
+ * @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);
+}

src/xrt/targets/common/CMakeLists.txt

···
 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()

src/xrt/targets/common/target_lists.c

···
 
 #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
···
 xrt_auto_prober_creator target_auto_list[] = {
 #ifdef XRT_BUILD_DRIVER_PSVR
     psvr_create_auto_prober,
+#endif
+
+#ifdef XRT_BUILD_DRIVER_ARDUINO
+    // Before OpenHMD
+    arduino_create_auto_prober,
 #endif
 
 #ifdef XRT_BUILD_DRIVER_DAYDREAM