From cb827a525a1076b66371510e12e8605e48660bb4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Florian=20M=C3=A4rkl?= Date: Wed, 6 Jan 2021 21:42:06 +0100 Subject: [PATCH] Add Motion to Feedback --- gui/src/streamsession.cpp | 18 ++++---- lib/include/chiaki/controller.h | 29 ++++--------- lib/include/chiaki/feedback.h | 3 ++ lib/include/chiaki/orientation.h | 10 ++++- lib/src/controller.c | 43 +++++++++++++++++++ lib/src/feedback.c | 73 ++++++++++++++++++++++++-------- lib/src/feedbacksender.c | 28 +++++++++++- lib/src/orientation.c | 28 +++++++++++- 8 files changed, 182 insertions(+), 50 deletions(-) diff --git a/gui/src/streamsession.cpp b/gui/src/streamsession.cpp index dcec36d..7d25136 100644 --- a/gui/src/streamsession.cpp +++ b/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 - // and SendFeedbackState(); + chiaki_orientation_tracker_apply_to_controller_state(&orient_tracker, &setsu_state); + 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; diff --git a/lib/include/chiaki/controller.h b/lib/include/chiaki/controller.h index e8f2e7c..24e86a9 100644 --- a/lib/include/chiaki/controller.h +++ b/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) -{ - 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; itouches[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; -} +CHIAKI_EXPORT bool chiaki_controller_state_equals(ChiakiControllerState *a, ChiakiControllerState *b); +/** + * 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 diff --git a/lib/include/chiaki/feedback.h b/lib/include/chiaki/feedback.h index 264af5c..be66384 100644 --- a/lib/include/chiaki/feedback.h +++ b/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; diff --git a/lib/include/chiaki/orientation.h b/lib/include/chiaki/orientation.h index 4a29c2e..2322d55 100644 --- a/lib/include/chiaki/orientation.h +++ b/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 } diff --git a/lib/src/controller.c b/lib/src/controller.c index 0e3a522..21b9971 100644 --- a/lib/src/controller.c +++ b/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; itouches[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)) diff --git a/lib/src/feedback.c b/lib/src/feedback.c index 4c52b72..2ce0b98 100644 --- a/lib/src/feedback.c +++ b/lib/src/feedback.c @@ -9,26 +9,65 @@ #include #endif #include +#include + +#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[0x1] = 0xff; // TODO - buf[0x2] = 0x7f; // TODO - buf[0x3] = 0xff; // TODO - buf[0x4] = 0x7f; // TODO - buf[0x5] = 0xff; // TODO - buf[0x6] = 0x7f; // TODO - buf[0x7] = 0xff; // TODO - buf[0x8] = 0x7f; // TODO - buf[0x9] = 0x99; // TODO - buf[0xa] = 0x99; // TODO - buf[0xb] = 0xff; // TODO - buf[0xc] = 0x7f; // TODO - buf[0xd] = 0xfe; // TODO - buf[0xe] = 0xf7; // TODO - buf[0xf] = 0xef; // TODO - buf[0x10] = 0x1f; // TODO + buf[0x0] = 0xa0; + uint16_t v = (uint16_t)(0xffff * ((float)state->gyro_x - GYRO_MIN) / (GYRO_MAX - GYRO_MIN)); + buf[0x1] = v; + buf[0x2] = v >> 8; + v = (uint16_t)(0xffff * ((float)state->gyro_y - GYRO_MIN) / (GYRO_MAX - GYRO_MIN)); + buf[0x3] = v; + buf[0x4] = v >> 8; + v = (uint16_t)(0xffff * ((float)state->gyro_z - GYRO_MIN) / (GYRO_MAX - GYRO_MIN)); + buf[0x5] = v; + buf[0x6] = v >> 8; + v = (uint16_t)(0xffff * ((float)state->accel_x - ACCEL_MIN) / (ACCEL_MAX - ACCEL_MIN)); + buf[0x7] = v; + buf[0x8] = v >> 8; + v = (uint16_t)(0xffff * ((float)state->accel_y - ACCEL_MIN) / (ACCEL_MAX - ACCEL_MIN)); + buf[0x9] = v; + buf[0xa] = v >> 8; + 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); diff --git a/lib/src/feedbacksender.c b/lib/src/feedbacksender.c index dc67bf6..771796f 100644 --- a/lib/src/feedbacksender.c +++ b/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"); diff --git a/lib/src/orientation.c b/lib/src/orientation.c index 90e991f..bb0849e 100644 --- a/lib/src/orientation.c +++ b/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; +}