Add Motion to Feedback

gui/src/streamsession.cpp

@@ -155,7 +155,7 @@ StreamSession::StreamSession(const StreamSessionConnectInfo &connect_info, QObje
 #if CHIAKI_GUI_ENABLE_SETSU
 	setsu_motion_device = nullptr;
 	chiaki_controller_state_set_idle(&setsu_state);
-	orient_dirty = false;
+	orient_dirty = true;
 	chiaki_orientation_tracker_init(&orient_tracker);
 	setsu = setsu_new();
 	auto timer = new QTimer(this);
@@ -163,8 +163,8 @@ StreamSession::StreamSession(const StreamSessionConnectInfo &connect_info, QObje
 		setsu_poll(setsu, SessionSetsuCb, this);
 		if(orient_dirty)
 		{
-			// TODO: put orient/gyro/acc into setsu_state
+			chiaki_orientation_tracker_apply_to_controller_state(&orient_tracker, &setsu_state);
-			// and SendFeedbackState();
+			SendFeedbackState();
 			orient_dirty = false;
 		}
 	});
@@ -330,16 +330,17 @@ void StreamSession::SendFeedbackState()
 	ChiakiControllerState state;
 	chiaki_controller_state_set_idle(&state);
 
+#if CHIAKI_GUI_ENABLE_SETSU
+	// setsu is the one that potentially has gyro/accel/orient so copy that directly first
+	state = setsu_state;
+#endif
+
 	for(auto controller : controllers)
 	{
 		auto controller_state = controller->GetState();
 		chiaki_controller_state_or(&state, &state, &controller_state);
 	}
 
-#if CHIAKI_GUI_ENABLE_SETSU
-	chiaki_controller_state_or(&state, &state, &setsu_state);
-#endif
-
 	chiaki_controller_state_or(&state, &state, &keyboard_state);
 	chiaki_session_set_controller_state(&session, &state);
 }
@@ -465,7 +466,8 @@ void StreamSession::HandleSetsuEvent(SetsuEvent *event)
 						break;
 					CHIAKI_LOGI(GetChiakiLog(), "Setsu Motion Device %s disconnected", event->path);
 					setsu_motion_device = nullptr;
-					SendFeedbackState();
+					chiaki_orientation_tracker_init(&orient_tracker);
+					orient_dirty = true;
 					break;
 			}
 			break;

lib/include/chiaki/controller.h

@@ -66,6 +66,10 @@ typedef struct chiaki_controller_state_t
 
 	uint8_t touch_id_next;
 	ChiakiControllerTouch touches[CHIAKI_CONTROLLER_TOUCHES_MAX];
+
+	float gyro_x, gyro_y, gyro_z;
+	float accel_x, accel_y, accel_z;
+	float orient_x, orient_y, orient_z, orient_w;
 } ChiakiControllerState;
 
 CHIAKI_EXPORT void chiaki_controller_state_set_idle(ChiakiControllerState *state);
@@ -79,27 +83,12 @@ CHIAKI_EXPORT void chiaki_controller_state_stop_touch(ChiakiControllerState *sta
 
 CHIAKI_EXPORT void chiaki_controller_state_set_touch_pos(ChiakiControllerState *state, uint8_t id, uint16_t x, uint16_t y);
 
-static inline bool chiaki_controller_state_equals(ChiakiControllerState *a, ChiakiControllerState *b)
+CHIAKI_EXPORT bool chiaki_controller_state_equals(ChiakiControllerState *a, ChiakiControllerState *b);
-{
-	if(!(a->buttons == b->buttons
-		&& a->l2_state == b->l2_state
-		&& a->r2_state == b->r2_state
-		&& a->left_x == b->left_x
-		&& a->left_y == b->left_y
-		&& a->right_x == b->right_x
-		&& a->right_y == b->right_y))
-		return false;
-
-	for(size_t i=0; i<CHIAKI_CONTROLLER_TOUCHES_MAX; i++)
-	{
-		if(a->touches[i].id != b->touches[i].id)
-			return false;
-		if(a->touches[i].id >= 0 && (a->touches[i].x != b->touches[i].x || a->touches[i].y != b->touches[i].y))
-			return false;
-	}
-	return true;
-}
 
+/**
+ * Union of two controller states.
+ * Ignores gyro, accel and orient because it makes no sense there.
+ */
 CHIAKI_EXPORT void chiaki_controller_state_or(ChiakiControllerState *out, ChiakiControllerState *a, ChiakiControllerState *b);
 
 #ifdef __cplusplus

lib/include/chiaki/feedback.h

@@ -15,6 +15,9 @@ extern "C" {
 
 typedef struct chiaki_feedback_state_t
 {
+	float gyro_x, gyro_y, gyro_z;
+	float accel_x, accel_y, accel_z;
+	float orient_x, orient_y, orient_z, orient_w;
 	int16_t left_x;
 	int16_t left_y;
 	int16_t right_x;

lib/include/chiaki/orientation.h

@@ -4,6 +4,7 @@
 #define CHIAKI_ORIENTATION_H
 
 #include "common.h"
+#include "controller.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -20,13 +21,16 @@ typedef struct chiaki_orientation_t
 } ChiakiOrientation;
 
 CHIAKI_EXPORT void chiaki_orientation_init(ChiakiOrientation *orient);
-CHIAKI_EXPORT void chiaki_orientation_update(ChiakiOrientation *orient, float gx, float gy, float gz, float ax, float ay, float az, float time_step_sec);
+CHIAKI_EXPORT void chiaki_orientation_update(ChiakiOrientation *orient,
+		float gx, float gy, float gz, float ax, float ay, float az, float time_step_sec);
 
 /**
- * Extension of ChiakiOrientation, also tracking an absolute timestamp
+ * Extension of ChiakiOrientation, also tracking an absolute timestamp and the current gyro/accel state
  */
 typedef struct chiaki_orientation_tracker_t
 {
+	float gyro_x, gyro_y, gyro_z;
+	float accel_x, accel_y, accel_z;
 	ChiakiOrientation orient;
 	uint32_t timestamp;
 	bool first_sample;
@@ -35,6 +39,8 @@ typedef struct chiaki_orientation_tracker_t
 CHIAKI_EXPORT void chiaki_orientation_tracker_init(ChiakiOrientationTracker *tracker);
 CHIAKI_EXPORT void chiaki_orientation_tracker_update(ChiakiOrientationTracker *tracker,
 		float gx, float gy, float gz, float ax, float ay, float az, uint32_t timestamp_us);
+CHIAKI_EXPORT void chiaki_orientation_tracker_apply_to_controller_state(ChiakiOrientationTracker *tracker,
+		ChiakiControllerState *state);
 
 #ifdef __cplusplus
 }

lib/src/controller.c

@@ -20,6 +20,14 @@ CHIAKI_EXPORT void chiaki_controller_state_set_idle(ChiakiControllerState *state
 		state->touches[i].x = 0;
 		state->touches[i].y = 0;
 	}
+	state->gyro_x = state->gyro_y = state->gyro_z = 0.0f;
+	state->accel_x = 0.0f;
+	state->accel_y = 1.0f;
+	state->accel_z = 0.0f;
+	state->orient_x = 0.0f;
+	state->orient_y = 0.0f;
+	state->orient_z = 0.0f;
+	state->orient_w = 1.0f;
 }
 
 CHIAKI_EXPORT int8_t chiaki_controller_state_start_touch(ChiakiControllerState *state, uint16_t x, uint16_t y)
@@ -64,6 +72,41 @@ CHIAKI_EXPORT void chiaki_controller_state_set_touch_pos(ChiakiControllerState *
 	}
 }
 
+CHIAKI_EXPORT bool chiaki_controller_state_equals(ChiakiControllerState *a, ChiakiControllerState *b)
+{
+	if(!(a->buttons == b->buttons
+		&& a->l2_state == b->l2_state
+		&& a->r2_state == b->r2_state
+		&& a->left_x == b->left_x
+		&& a->left_y == b->left_y
+		&& a->right_x == b->right_x
+		&& a->right_y == b->right_y))
+		return false;
+
+	for(size_t i=0; i<CHIAKI_CONTROLLER_TOUCHES_MAX; i++)
+	{
+		if(a->touches[i].id != b->touches[i].id)
+			return false;
+		if(a->touches[i].id >= 0 && (a->touches[i].x != b->touches[i].x || a->touches[i].y != b->touches[i].y))
+			return false;
+	}
+
+#define CHECKF(n) if(a->n < b->n - 0.0000001f || a->n > b->n + 0.0000001f) return false
+	CHECKF(gyro_x);
+	CHECKF(gyro_y);
+	CHECKF(gyro_z);
+	CHECKF(accel_x);
+	CHECKF(accel_y);
+	CHECKF(accel_z);
+	CHECKF(orient_x);
+	CHECKF(orient_y);
+	CHECKF(orient_z);
+	CHECKF(orient_w);
+#undef CHECKF
+
+	return true;
+}
+
 #define MAX(a, b)	  ((a) > (b) ? (a) : (b))
 #define ABS(a)		  ((a) > 0 ? (a) : -(a))
 #define MAX_ABS(a, b) (ABS(a) > ABS(b) ? (a) : (b))

lib/src/feedback.c

@@ -9,26 +9,65 @@
 #include <arpa/inet.h>
 #endif
 #include <string.h>
+#include <math.h>
+
+#define GYRO_MIN -30.0f
+#define GYRO_MAX 30.0f
+#define ACCEL_MIN -5.0f
+#define ACCEL_MAX 5.0f
+
+static uint32_t compress_quat(float *q)
+{
+	// very similar idea as https://github.com/jpreiss/quatcompress
+	size_t largest_i = 0;
+	for(size_t i = 1; i < 4; i++)
+	{
+		if(fabs(q[i]) > fabs(q[largest_i]))
+			largest_i = i;
+	}
+	uint32_t r = (q[largest_i] < 0.0 ? 1 : 0) | (largest_i << 1);
+	for(size_t i = 0; i < 3; i++)
+	{
+		size_t qi = i < largest_i ? i : i + 1;
+		float v = q[qi];
+		if(v < -M_SQRT1_2)
+			v = -M_SQRT1_2;
+		if(v > M_SQRT1_2)
+			v = M_SQRT1_2;
+		v += M_SQRT1_2;
+		v *= (float)0x1ff / (2.0f * M_SQRT1_2);
+		r |= (uint32_t)v << (3 + i * 9);
+	}
+	return r;
+}
 
 CHIAKI_EXPORT void chiaki_feedback_state_format_v9(uint8_t *buf, ChiakiFeedbackState *state)
 {
-	buf[0x0] = 0xa0; // TODO
+	buf[0x0] = 0xa0;
-	buf[0x1] = 0xff; // TODO
+	uint16_t v = (uint16_t)(0xffff * ((float)state->gyro_x - GYRO_MIN) / (GYRO_MAX - GYRO_MIN));
-	buf[0x2] = 0x7f; // TODO
+	buf[0x1] = v;
-	buf[0x3] = 0xff; // TODO
+	buf[0x2] = v >> 8;
-	buf[0x4] = 0x7f; // TODO
+	v = (uint16_t)(0xffff * ((float)state->gyro_y - GYRO_MIN) / (GYRO_MAX - GYRO_MIN));
-	buf[0x5] = 0xff; // TODO
+	buf[0x3] = v;
-	buf[0x6] = 0x7f; // TODO
+	buf[0x4] = v >> 8;
-	buf[0x7] = 0xff; // TODO
+	v = (uint16_t)(0xffff * ((float)state->gyro_z - GYRO_MIN) / (GYRO_MAX - GYRO_MIN));
-	buf[0x8] = 0x7f; // TODO
+	buf[0x5] = v;
-	buf[0x9] = 0x99; // TODO
+	buf[0x6] = v >> 8;
-	buf[0xa] = 0x99; // TODO
+	v = (uint16_t)(0xffff * ((float)state->accel_x - ACCEL_MIN) / (ACCEL_MAX - ACCEL_MIN));
-	buf[0xb] = 0xff; // TODO
+	buf[0x7] = v;
-	buf[0xc] = 0x7f; // TODO
+	buf[0x8] = v >> 8;
-	buf[0xd] = 0xfe; // TODO
+	v = (uint16_t)(0xffff * ((float)state->accel_y - ACCEL_MIN) / (ACCEL_MAX - ACCEL_MIN));
-	buf[0xe] = 0xf7; // TODO
+	buf[0x9] = v;
-	buf[0xf] = 0xef; // TODO
+	buf[0xa] = v >> 8;
-	buf[0x10] = 0x1f; // TODO
+	v = (uint16_t)(0xffff * ((float)state->accel_z - ACCEL_MIN) / (ACCEL_MAX - ACCEL_MIN));
+	buf[0xb] = v;
+	buf[0xc] = v >> 8;
+	float q[4] = { state->orient_x, state->orient_y, state->orient_z, state->orient_w };
+	uint32_t qc = compress_quat(q);
+	buf[0xd] = qc;
+	buf[0xe] = qc >> 0x8;
+	buf[0xf] = qc >> 0x10;
+	buf[0x10] = qc >> 0x18;
 	*((chiaki_unaligned_uint16_t *)(buf + 0x11)) = htons((uint16_t)state->left_x);
 	*((chiaki_unaligned_uint16_t *)(buf + 0x13)) = htons((uint16_t)state->left_y);
 	*((chiaki_unaligned_uint16_t *)(buf + 0x15)) = htons((uint16_t)state->right_x);

lib/src/feedbacksender.c

@@ -83,10 +83,24 @@ CHIAKI_EXPORT ChiakiErrorCode chiaki_feedback_sender_set_controller_state(Chiaki
 
 static bool controller_state_equals_for_feedback_state(ChiakiControllerState *a, ChiakiControllerState *b)
 {
-	return a->left_x == b->left_x
+	if(!(a->left_x == b->left_x
 		&& a->left_y == b->left_y
 		&& a->right_x == b->right_x
-		&& a->right_y == b->right_y;
+		&& a->right_y == b->right_y))
+		return false;
+#define CHECKF(n) if(a->n < b->n - 0.0000001f || a->n > b->n + 0.0000001f) return false
+	CHECKF(gyro_x);
+	CHECKF(gyro_y);
+	CHECKF(gyro_z);
+	CHECKF(accel_x);
+	CHECKF(accel_y);
+	CHECKF(accel_z);
+	CHECKF(orient_x);
+	CHECKF(orient_y);
+	CHECKF(orient_z);
+	CHECKF(orient_w);
+#undef CHECKF
+	return true;
 }
 
 static void feedback_sender_send_state(ChiakiFeedbackSender *feedback_sender)
@@ -96,6 +110,16 @@ static void feedback_sender_send_state(ChiakiFeedbackSender *feedback_sender)
 	state.left_y = feedback_sender->controller_state.left_y;
 	state.right_x = feedback_sender->controller_state.right_x;
 	state.right_y = feedback_sender->controller_state.right_y;
+	state.gyro_x = feedback_sender->controller_state.gyro_x;
+	state.gyro_y = feedback_sender->controller_state.gyro_y;
+	state.gyro_z = feedback_sender->controller_state.gyro_z;
+	state.accel_x = feedback_sender->controller_state.accel_x;
+	state.accel_y = feedback_sender->controller_state.accel_y;
+	state.accel_z = feedback_sender->controller_state.accel_z;
+	state.orient_x = feedback_sender->controller_state.orient_x;
+	state.orient_y = feedback_sender->controller_state.orient_y;
+	state.orient_z = feedback_sender->controller_state.orient_z;
+	state.orient_w = feedback_sender->controller_state.orient_w;
 	ChiakiErrorCode err = chiaki_takion_send_feedback_state(feedback_sender->takion, feedback_sender->state_seq_num++, &state);
 	if(err != CHIAKI_ERR_SUCCESS)
 		CHIAKI_LOGE(feedback_sender->log, "FeedbackSender failed to send Feedback State");

lib/src/orientation.c

@@ -11,7 +11,8 @@ CHIAKI_EXPORT void chiaki_orientation_init(ChiakiOrientation *orient)
 #define BETA 0.1f		// 2 * proportional gain
 static float inv_sqrt(float x);
 
-CHIAKI_EXPORT void chiaki_orientation_update(ChiakiOrientation *orient, float gx, float gy, float gz, float ax, float ay, float az, float time_step_sec)
+CHIAKI_EXPORT void chiaki_orientation_update(ChiakiOrientation *orient,
+		float gx, float gy, float gz, float ax, float ay, float az, float time_step_sec)
 {
 	float q0 = orient->w, q1 = orient->x, q2 = orient->y, q3 = orient->z;
 	// Madgwick's IMU algorithm.
@@ -103,6 +104,10 @@ static float inv_sqrt(float x)
 
 CHIAKI_EXPORT void chiaki_orientation_tracker_init(ChiakiOrientationTracker *tracker)
 {
+	tracker->accel_x = 0.0f;
+	tracker->accel_y = 1.0f;
+	tracker->accel_z = 0.0f;
+	tracker->gyro_x = tracker->gyro_y = tracker->gyro_z = 0.0f;
 	chiaki_orientation_init(&tracker->orient);
 	tracker->timestamp = 0;
 	tracker->first_sample = true;
@@ -111,6 +116,12 @@ CHIAKI_EXPORT void chiaki_orientation_tracker_init(ChiakiOrientationTracker *tra
 CHIAKI_EXPORT void chiaki_orientation_tracker_update(ChiakiOrientationTracker *tracker,
 		float gx, float gy, float gz, float ax, float ay, float az, uint32_t timestamp_us)
 {
+	tracker->gyro_x = gx;
+	tracker->gyro_y = gy;
+	tracker->gyro_z = gz;
+	tracker->accel_x = ax;
+	tracker->accel_y = ay;
+	tracker->accel_z = az;
 	if(tracker->first_sample)
 	{
 		tracker->first_sample = false;
@@ -124,3 +135,18 @@ CHIAKI_EXPORT void chiaki_orientation_tracker_update(ChiakiOrientationTracker *t
 	tracker->timestamp = timestamp_us;
 	chiaki_orientation_update(&tracker->orient, gx, gy, gz, ax, ay, az, (float)delta_us / 1000000.0f);
 }
+
+CHIAKI_EXPORT void chiaki_orientation_tracker_apply_to_controller_state(ChiakiOrientationTracker *tracker,
+		ChiakiControllerState *state)
+{
+	state->gyro_x = tracker->gyro_x;
+	state->gyro_y = tracker->gyro_y;
+	state->gyro_z = tracker->gyro_z;
+	state->accel_x = tracker->accel_x;
+	state->accel_y = tracker->accel_y;
+	state->accel_z = tracker->accel_z;
+	state->orient_x = tracker->orient.x;
+	state->orient_y = tracker->orient.y;
+	state->orient_z = tracker->orient.z;
+	state->orient_w = tracker->orient.w;
+}