mirror of
https://git.sr.ht/~thestr4ng3r/chiaki
synced 2025-08-21 05:53:12 -07:00
Add Motion to Feedback
This commit is contained in:
Florian Märkl
parent
170dcd4d65
commit
cb827a525a
8 changed files with 182 additions and 50 deletions
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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; 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;
|
||||
}
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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[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);
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue