d/wmr: Hook up motion controller gyro and accel

Basic IMU setup with fusion. Reading and applying config callibration data from controller is still work in progress

src/xrt/drivers/wmr/wmr_bt_controller.c

@@ -62,18 +62,26 @@ read_packets(struct wmr_bt_controller *d)
 	} else if (size == 0) {
 		WMR_TRACE(d, "WMR Controller (Bluetooth): No data to read from device");
 		return true; // No more messages, return.
-	} else {
-		WMR_DEBUG(d, "WMR Controller (Bluetooth): Read %u bytes from device", size);
 	}
 
+	WMR_TRACE(d, "WMR Controller (Bluetooth): Read %u bytes from device", size);
+
+
 	switch (buffer[0]) {
 	case WMR_BT_MOTION_CONTROLLER_MSG:
+		os_mutex_lock(&d->lock);
 		// Note: skipping msg type byte
-		if (!wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level)) {
+		bool b = wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level);
+		if (b) {
+			m_imu_3dof_update(&d->fusion, d->input.imu.timestamp_ticks, &d->input.imu.gyro,
+			                  &d->input.imu.acc);
+		} else {
 			WMR_ERROR(d, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i",
 			          buffer[0], size);
+			os_mutex_unlock(&d->lock);
 			return false;
 		}
+		os_mutex_unlock(&d->lock);
 		break;
 	default:
 		WMR_DEBUG(d, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], size);
@@ -97,21 +105,45 @@ wmr_bt_controller_get_tracked_pose(struct xrt_device *xdev,
                                    uint64_t at_timestamp_ns,
                                    struct xrt_space_relation *out_relation)
 {
-	// struct wmr_bt_controller *d = wmr_bt_controller(xdev);
+	struct wmr_bt_controller *d = wmr_bt_controller(xdev);
-	// Todo: implement
+
+	// Variables needed for prediction.
+	uint64_t last_imu_timestamp_ns = 0;
+	struct xrt_space_relation relation = {0};
+	relation.relation_flags = (enum xrt_space_relation_flags)(
+	    XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
+	    XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
+
+
 	struct xrt_pose pose = {{0, 0, 0, 1}, {0, 1.2, -0.5}};
 	if (xdev->device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
 		pose.position.x = -0.2;
 	} else {
 		pose.position.x = 0.2;
 	}
+	relation.pose = pose;
 
-	out_relation->pose = pose;
+	// Copy data while holding the lock.
-	out_relation->relation_flags = (enum xrt_space_relation_flags)(
+	os_mutex_lock(&d->lock);
-	    XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
+	relation.pose.orientation = d->fusion.rot;
-	    XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
+	relation.angular_velocity = d->last_angular_velocity;
+	last_imu_timestamp_ns = d->last_imu_timestamp_ns * WMR_MOTION_CONTROLLER_NS_PER_TICK;
+	os_mutex_unlock(&d->lock);
+
+	// No prediction needed.
+	if (at_timestamp_ns < last_imu_timestamp_ns) {
+		*out_relation = relation;
+		return;
+	}
+
+	uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns;
+	double prediction_s = time_ns_to_s(prediction_ns);
+
+	m_predict_relation(&relation, prediction_s, out_relation);
 }
 
+
+
 static void
 wmr_bt_controller_update_inputs(struct xrt_device *xdev)
 {
@@ -119,7 +151,7 @@ wmr_bt_controller_update_inputs(struct xrt_device *xdev)
 
 	struct xrt_input *inputs = d->base.inputs;
 
-	//! @todo Mutex protect the input struct.
+	os_mutex_lock(&d->lock);
 
 	inputs[WMR_INDEX_MENU_CLICK].value.boolean = d->input.menu;
 	inputs[WMR_INDEX_SQUEEZE_CLICK].value.boolean = d->input.squeeze;
@@ -129,6 +161,8 @@ wmr_bt_controller_update_inputs(struct xrt_device *xdev)
 	inputs[WMR_INDEX_TRACKPAD_CLICK].value.boolean = d->input.trackpad.click;
 	inputs[WMR_INDEX_TRACKPAD_TOUCH].value.boolean = d->input.trackpad.touch;
 	inputs[WMR_INDEX_TRACKPAD].value.vec2 = d->input.trackpad.values;
+
+	os_mutex_unlock(&d->lock);
 }
 
 static void *
@@ -136,7 +170,6 @@ wmr_bt_controller_run_thread(void *ptr)
 {
 	struct wmr_bt_controller *d = wmr_bt_controller(ptr);
 
-
 	os_thread_helper_lock(&d->controller_thread);
 	while (os_thread_helper_is_running_locked(&d->controller_thread)) {
 		os_thread_helper_unlock(&d->controller_thread);
@@ -164,12 +197,13 @@ wmr_bt_controller_destroy(struct xrt_device *xdev)
 	// Destroy the thread object.
 	os_thread_helper_destroy(&d->controller_thread);
 
-
 	if (d->controller_hid != NULL) {
 		os_hid_destroy(d->controller_hid);
 		d->controller_hid = NULL;
 	}
 
+	os_mutex_destroy(&d->lock);
+
 	// Destroy the fusion.
 	m_imu_3dof_close(&d->fusion);
 
@@ -260,6 +294,8 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
 	d->base.position_tracking_supported = false;
 	d->base.hand_tracking_supported = true;
 
+
+	d->input.imu.timestamp_ticks = 0;
 	m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
 
 
@@ -268,6 +304,13 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
 
 	// Todo: Read config file from controller if possible.
 
+	ret = os_mutex_init(&d->lock);
+	if (ret != 0) {
+		WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to init mutex!");
+		wmr_bt_controller_destroy(&d->base);
+		return NULL;
+	}
+
 	// Thread and other state.
 	ret = os_thread_helper_init(&d->controller_thread);
 	if (ret != 0) {
@@ -289,8 +332,11 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
 
 	u_var_add_root(d, d->base.str, true);
 	u_var_add_bool(d, &d->input.menu, "input.menu");
+	u_var_add_bool(d, &d->input.home, "input.home");
+	u_var_add_bool(d, &d->input.bt_pairing, "input.bt_pairing");
 	u_var_add_bool(d, &d->input.squeeze, "input.squeeze");
 	u_var_add_f32(d, &d->input.trigger, "input.trigger");
+	u_var_add_u8(d, &d->input.battery, "input.battery");
 	u_var_add_bool(d, &d->input.thumbstick.click, "input.thumbstick.click");
 	u_var_add_f32(d, &d->input.thumbstick.values.x, "input.thumbstick.values.y");
 	u_var_add_f32(d, &d->input.thumbstick.values.y, "input.thumbstick.values.x");
@@ -298,6 +344,10 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
 	u_var_add_bool(d, &d->input.trackpad.touch, "input.trackpad.touch");
 	u_var_add_f32(d, &d->input.trackpad.values.x, "input.trackpad.values.x");
 	u_var_add_f32(d, &d->input.trackpad.values.y, "input.trackpad.values.y");
+	u_var_add_ro_vec3_f32(d, &d->input.imu.acc, "imu.acc");
+	u_var_add_ro_vec3_f32(d, &d->input.imu.gyro, "imu.gyro");
+	u_var_add_i32(d, &d->input.imu.temperature, "imu.temperature");
+
 
 	return &d->base;
 }

src/xrt/drivers/wmr/wmr_bt_controller.h

@@ -24,7 +24,7 @@ extern "C" {
 
 
 /*!
- * Indices where each input is in the input list.
+ * Indices in input list of each input.
  */
 enum wmr_bt_input_index
 {
@@ -52,23 +52,20 @@ struct wmr_bt_controller
 
 	struct os_hid_device *controller_hid;
 	struct os_thread_helper controller_thread;
+
+
 	struct os_mutex lock;
 
+	//! The last decoded package of IMU and button data
+	struct wmr_controller_input input;
+	//! Time of last IMU sample, in CPU time.
+	uint64_t last_imu_timestamp_ns;
+	//! Main fusion calculator.
 	struct m_imu_3dof fusion;
-
+	//! The last angular velocity from the IMU, for prediction.
-	struct
+	struct xrt_vec3 last_angular_velocity;
-	{
-		struct xrt_vec3 acc;
-		struct xrt_vec3 gyro;
-	} last;
-
-	struct xrt_quat rot_filtered;
 
 	enum u_logging_level log_level;
-
-	uint32_t last_ticks;
-
-	struct wmr_controller_input input;
 };
 
 

src/xrt/drivers/wmr/wmr_controller_protocol.c

@@ -19,6 +19,29 @@
  *
  */
 
+static inline void
+vec3_from_wmr_controller_accel(int32_t sample[3], struct xrt_vec3 *out_vec)
+{
+
+	// Reverb G1: 1g approximately equivalent to 490,000
+	// float g = sqrtf(sample[0]*sample[0] + sample[1]*sample[1] + sample[2]*sample[2]);
+	// U_LOG_IFL_D(log_level, "g: %f", g);
+
+	out_vec->x = (float)sample[0] * 0.001f * 1.0f;
+	out_vec->y = (float)sample[1] * 0.001f * 1.0f;
+	out_vec->z = (float)sample[2] * 0.001f * 1.0f;
+}
+
+
+static inline void
+vec3_from_wmr_controller_gyro(int32_t sample[3], struct xrt_vec3 *out_vec)
+{
+	out_vec->x = (float)sample[0] * 0.001f * 1.0f;
+	out_vec->y = (float)sample[1] * 0.001f * 1.0f;
+	out_vec->z = (float)sample[2] * 0.001f * 1.0f;
+}
+
+
 bool
 wmr_controller_packet_parse(const unsigned char *buffer,
                             size_t len,
@@ -30,39 +53,24 @@ wmr_controller_packet_parse(const unsigned char *buffer,
 		return false;
 	}
 
-	/*
-	        U_LOG_IFL_D(log_level,
-	                    "%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;
 
 	// Read buttons
-	unsigned char buttons = read8(&p);
+	uint8_t buttons = read8(&p);
 	decoded_input->thumbstick.click = buttons & 0x01;
-	// decoded_input->home = buttons & 0x02;
+	decoded_input->home = buttons & 0x02;
 	decoded_input->menu = buttons & 0x04;
 	decoded_input->squeeze = buttons & 0x08; // squeeze-click
 	decoded_input->trackpad.click = buttons & 0x10;
-	// decoded_input->bt_pairing = buttons & 0x20;
+	decoded_input->bt_pairing = buttons & 0x20;
 	decoded_input->trackpad.touch = buttons & 0x40;
 
 
 	// Read thumbstick coordinates (12 bit resolution)
-	signed int stick_x = read8(&p);
+	int16_t stick_x = read8(&p);
-	unsigned char nipples = read8(&p);
+	uint8_t nibbles = read8(&p);
-	stick_x += ((nipples & 0x0F) << 8);
+	stick_x += ((nibbles & 0x0F) << 8);
-	signed int stick_y = (nipples >> 4);
+	int16_t stick_y = (nibbles >> 4);
 	stick_y += (read8(&p) << 4);
 
 	decoded_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF;
@@ -75,39 +83,50 @@ wmr_controller_packet_parse(const unsigned char *buffer,
 		decoded_input->thumbstick.values.y = 1.0f;
 	}
 
-
 	// Read trigger value (0x00 - 0xFF)
 	decoded_input->trigger = (float)read8(&p) / 0xFF;
 
-	U_LOG_IFL_D(log_level, "thumbstick: x %f, y %f, trigger: %f", decoded_input->thumbstick.values.x,
-	            decoded_input->thumbstick.values.y, decoded_input->trigger);
-
-
 	// Read trackpad coordinates (0x00 - 0x64. Both are 0xFF when untouched)
-	unsigned char trackpad_x = read8(&p);
+	uint8_t trackpad_x = read8(&p);
-	unsigned char trackpad_y = read8(&p);
+	uint8_t trackpad_y = read8(&p);
 	decoded_input->trackpad.values.x = (trackpad_x == 0xFF) ? 0.0f : (float)(trackpad_x - 0x32) / 0x32;
 	decoded_input->trackpad.values.y = (trackpad_y == 0xFF) ? 0.0f : (float)(trackpad_y - 0x32) / 0x32;
 
-	U_LOG_IFL_D(log_level, "touchpad: x %f, y %f", decoded_input->trackpad.values.x,
-	            decoded_input->trackpad.values.y);
 
+	decoded_input->battery = read8(&p);
+
+
+	int32_t acc[3];
+	acc[0] = read24(&p); // x
+	acc[1] = read24(&p); // y
+	acc[2] = read24(&p); // z
+	vec3_from_wmr_controller_accel(acc, &decoded_input->imu.acc);
+
+
+	decoded_input->imu.temperature = read16(&p);
+
+
+	int32_t gyro[3];
+	gyro[0] = read24(&p);
+	gyro[1] = read24(&p);
+	gyro[2] = read24(&p);
+	vec3_from_wmr_controller_accel(gyro, &decoded_input->imu.gyro);
+
+
+	uint32_t prev_ticks = decoded_input->imu.timestamp_ticks & 0xFFFFFFFFUL;
+
+	// Write the new ticks value into the lower half of timestamp_ticks
+	decoded_input->imu.timestamp_ticks &= (0xFFFFFFFFUL << 32);
+	decoded_input->imu.timestamp_ticks += (uint32_t)read32(&p);
+
+	if ((decoded_input->imu.timestamp_ticks & 0xFFFFFFFFUL) < prev_ticks) {
+		// Timer overflow, so increment the upper half of timestamp_ticks
+		decoded_input->imu.timestamp_ticks += (0x1UL << 32);
+	}
 
 	/* Todo: More decoding here
-
+	    read16(&p); // Unknown. Seems to depend on controller orientation.
-	    unsigned char battery = read8(&p);
+	    read32(&p); // Unknown.
-	    unsigned int accel_x = read24(&p);
-	    unsigned int accel_y = read24(&p);
-	    unsigned int accel_z = read24(&p);
-	    unsigned int temp = read16(&p);
-	    unsigned int gyro_x = read24(&p);
-	    unsigned int gyro_y = read24(&p);
-	    unsigned int gyro_z = read24(&p);
-
-	    unsigned int 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);

src/xrt/drivers/wmr/wmr_controller_protocol.h

@@ -27,6 +27,7 @@ extern "C" {
 
 // Todo: Is this enough?
 #define WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE 256
+#define WMR_MOTION_CONTROLLER_NS_PER_TICK 100
 
 
 // Messages types specific to Bluetooth connected WMR motion controllers
@@ -35,8 +36,12 @@ extern "C" {
 
 struct wmr_controller_input
 {
+	// buttons clicked
 	bool menu;
+	bool home;
+	bool bt_pairing;
 	bool squeeze; // Actually a "squeeze" click
+
 	float trigger;
 
 	struct
@@ -50,8 +55,17 @@ struct wmr_controller_input
 		bool touch;
 		struct xrt_vec2 values;
 	} trackpad;
-};
 
+	uint8_t battery;
+
+	struct
+	{
+		uint64_t timestamp_ticks;
+		struct xrt_vec3 acc;
+		struct xrt_vec3 gyro;
+		int32_t temperature;
+	} imu;
+};
 
 /*!
  * @}