/* (hookshot) A controller's motion sensors as the Switch reports its six-axis state (host/pad_sdl.c fills it from SDL, host/nn_hid.c hands it to the game's GetSixAxisSensorStates). Header-only, no SDL or Windows, so tests/test_motion.py runs it as it is. This is Eden's (and yuzu's) motion, the emulators OoT aims right with: SDL's readings turned into the Switch's frame and units, and the state built as the console builds it, so the game gets what it gets on hardware and does its own aiming (it recenters on the camera each time it aims; how the pad is held, in bed or at a desk, does not matter): - acceleration in G and angular velocity in turns a second, in the Switch's frame (Eden's sdl_driver.cpp: x = -X, y = Z, z = -Y for the accelerometer, x = X, y = -Z, z = Y for the gyroscope, SDL's +X right, +Y out of the face buttons, +Z toward the player), so a pad lying face up reads (0, 0, -1) G; - the rotation: the angular velocity summed (turns); - the direction: the pad's orientation as a 3x3 matrix, from Mahony's filter (the gyroscope integrated as a quaternion, pulled toward the measured gravity: kp 0.3, ki 0.005), Eden's motion_input.cpp line for line. The gyroscope's resting rate (its bias) is learned while the pad is still on a table (both sensors steady) and taken off, and rates under 0.007 turns a second count as none, as the console's standard zero-drift mode does (the Motion deadzone row: Low and High are its tight 0.002 and loose 0.01). Motion sensitivity X/Y and Invert motion X/Y scale the angular velocity before it is used: its part about the world's up (the turn, X) and the rest (the tilt, Y). At 100% and not inverted the state is Eden's own. Earlier versions sent a partial state (a yaw rate and a tilt matrix, the rest neutral, spluton's and Link-22's findings in issue 112), and every way the pad could be held at the start needed its own case; the full state needs none. The filter, the frame and the orientation matrix follow Eden's src/hid_core/frontend/motion_input.cpp and src/input_common/drivers/sdl_driver.cpp (Copyright 2020 yuzu Emulator Project, GPL-2.0-or-later; Copyright 2026 Eden Emulator Project, GPL-3.0-or-later), as this project is GPL-3.0. */ #pragma once #include #include #include #define HOS_MOTION_G 9.80665f #define HOS_MOTION_TAU 6.28318531f typedef struct { float accel[3]; /* G, the Switch's frame */ float gyro[3]; /* turns a second */ float rotation[3]; /* turns */ float direction[9]; /* rows: the orientation */ } HosMotionState; /* one reading's state as GetSixAxisSensorStates hands it out: n counts the readings, dt_ns is the time since the one before (host/pad_sdl.c keeps the last HOS_MOTION_HISTORY) */ typedef struct { HosMotionState st; int64_t n, dt_ns; } HosMotionSample; #define HOS_MOTION_HISTORY 32 typedef struct { float quat[4]; /* x, y, z, w (Eden's order): the real orientation, Eden's filter on the real rates */ float quat_out[4]; /* the orientation handed out: integrated from the rates as Motion sensitivity and Invert motion scale them, with the real filter's drift correction (the same as quat at 100%) */ float real_error[3], integral_error[3]; float bias[3]; /* turns a second, the Switch's frame */ float learned_t; /* seconds of stillness learned from (fast at first, then slowly) */ float still_t; /* seconds still */ float gmean[3], gvar, avec[3], avar, amean; /* the short window's means and variances (stillness) */ int stats_ready, only_accelerometer, reset_counter; float last_accel[3], last_gyro[3], pending_dt; float threshold; /* turns a second, the deadzone (0: HOS_MOTION_THRESHOLD) */ float up[3]; /* the world's up in the Switch's frame (from the accelerometer, smoothed: for the split of the angular velocity into turn and tilt) */ HosMotionState out; } HosMotion; #define HOS_MOTION_STILL_GYRO 0.015f /* rad/s, summed over three axes (about 0.009 on each) */ #define HOS_MOTION_STILL_ACCEL 0.0173f /* G, the same way (0.01 on each) */ #define HOS_MOTION_STILL_S 0.5f #define HOS_MOTION_WINDOW_S 0.2f #define HOS_MOTION_DT_MAX 0.1f /* (Eden ignores a longer gap) */ #define HOS_MOTION_THRESHOLD 0.007f /* turns a second: less is none (the console's standard zero-drift mode) */ #define HOS_MOTION_THRESHOLD_TIGHT 0.002f /* (its tight and loose modes: the Motion deadzone row's Low and High) */ #define HOS_MOTION_THRESHOLD_LOOSE 0.01f static inline void hos_motion_rest(HosMotionState *o) { memset(o, 0, sizeof *o); o->accel[2] = -1; o->direction[0] = o->direction[4] = o->direction[8] = 1; } static inline void hos_motion_reset(HosMotion *m) { memset(m, 0, sizeof *m); m->quat[2] = -1; /* (Eden's ResetQuaternion: {0, 0, -1, 0}) */ m->quat_out[2] = -1; m->only_accelerometer = 1; m->up[2] = 1; hos_motion_rest(&m->out); } static inline float hos_motion_len(const float v[3]) { return sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); } /* a quaternion (Eden's x, y, z, w) turned at Eden's filter rate rg for period seconds */ static inline void hos_motion_integrate(float quat[4], const float rg[3], float period) { float q1 = quat[3], q2 = quat[0], q3 = quat[1], q4 = quat[2]; const float gx = rg[1], gy = rg[0], gz = rg[2]; const float pa = q2, pb = q3, pc = q4; q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * period); q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * period); q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * period); q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * period); const float n = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); if (n > 0) quat[3] = q1 / n, quat[0] = q2 / n, quat[1] = q3 / n, quat[2] = q4 / n; } static inline void hos_motion_rate(const HosMotion *m, const float gyro[3], float rg[3]) { /* (Eden's swap, rad/s) */ rg[0] = gyro[1] * HOS_MOTION_TAU, rg[1] = -gyro[0] * HOS_MOTION_TAU, rg[2] = -gyro[2] * HOS_MOTION_TAU; if (m->only_accelerometer) rg[0] = rg[1] = rg[2] = 0; } /* Mahony's filter on the real quaternion, Eden's UpdateOrientation (period: seconds; gain 10: the settling of SetOrientationFromAccelerometer). Returns the feedback it added to the rate, in *corr (rad/s, the filter's frame), so the handed-out orientation can lose the same drift. */ static inline void hos_motion_mahony(HosMotion *m, const float accel[3], const float gyro[3], float period, float gain, float corr[3]) { const float q1 = m->quat[3], q2 = m->quat[0], q3 = m->quat[1], q4 = m->quat[2]; const float alen = hos_motion_len(accel); float rg[3]; hos_motion_rate(m, gyro, rg); if (gain > 1) rg[0] = rg[1] = rg[2] = 0; corr[0] = corr[1] = corr[2] = 0; if ((alen >= 0.75f && alen <= 1.25f) || gain > 1) { const float ax = -accel[0] / alen, ay = accel[1] / alen, az = -accel[2] / alen; const float vx = 2.0f * (q2 * q4 - q1 * q3); const float vy = 2.0f * (q1 * q2 + q3 * q4); const float vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; const float e[3] = {az * vx - ax * vz, ay * vz - az * vy, ax * vy - ay * vx}; memcpy(m->real_error, e, sizeof e); if (gain <= 1 && hos_motion_len(e) >= 0.05f) for (int i = 0; i < 3; i++) m->integral_error[i] += e[i]; else if (gain <= 1) memset(m->integral_error, 0, sizeof m->integral_error); const float kp = 0.3f, ki = 0.005f; const float wp = gain > 1 ? 10 : m->only_accelerometer ? 35 : 1, wi = gain > 1 ? 5 : m->only_accelerometer ? 10 : 1; for (int i = 0; i < 3; i++) corr[i] = wp * kp * e[i] + wi * ki * m->integral_error[i]; } for (int i = 0; i < 3; i++) rg[i] += corr[i]; hos_motion_integrate(m->quat, rg, period); } /* the orientation matrix, Eden's GetOrientation (its quaternion's matrix, with its sign changes) */ static inline void hos_motion_direction(const float quat[4], float d[9]) { const float x = -quat[1], y = -quat[0], z = -quat[3], w = -quat[2]; const float x2 = x * x, y2 = y * y, z2 = z * z, xy = x * y, wz = w * z, xz = x * z, wy = w * y, yz = y * z, wx = w * x; const float mm[9] = {1 - 2 * (y2 + z2), 2 * (xy + wz), 2 * (xz - wy), 2 * (xy - wz), 1 - 2 * (x2 + z2), 2 * (yz + wx), 2 * (xz + wy), 2 * (yz - wx), 1 - 2 * (x2 + y2)}; d[0] = mm[0], d[1] = mm[1], d[2] = -mm[2]; d[3] = mm[3], d[4] = mm[4], d[5] = -mm[5]; d[6] = -mm[6], d[7] = -mm[7], d[8] = mm[8]; } /* One reading: SDL's accelerometer (m/s^2) and gyroscope (rad/s), dt seconds after the last one. sens_h and sens_v scale the turn and the tilt (1: as the pad turns); inv_h and inv_v turn them the other way. Returns 0 for a repeat of the last reading (no new reading: its time goes into the next), 1 otherwise. */ static inline int hos_motion_update(HosMotion *m, const float accel_sdl[3], const float gyro_sdl[3], float dt, float sens_h, float sens_v, int inv_h, int inv_v) { if (!(dt >= 0)) dt = 0; if (m->stats_ready && !memcmp(accel_sdl, m->last_accel, sizeof m->last_accel) && !memcmp(gyro_sdl, m->last_gyro, sizeof m->last_gyro)) { m->pending_dt += dt; return 0; } memcpy(m->last_accel, accel_sdl, sizeof m->last_accel), memcpy(m->last_gyro, gyro_sdl, sizeof m->last_gyro); dt += m->pending_dt, m->pending_dt = 0; /* the Switch's frame and units (Eden's sdl_driver.cpp), clamped as Eden clamps them */ float a[3] = {-accel_sdl[0] / HOS_MOTION_G, accel_sdl[2] / HOS_MOTION_G, -accel_sdl[1] / HOS_MOTION_G}; float g[3] = {gyro_sdl[0] / HOS_MOTION_TAU, -gyro_sdl[2] / HOS_MOTION_TAU, gyro_sdl[1] / HOS_MOTION_TAU}; for (int i = 0; i < 3; i++) { a[i] = a[i] < -7 ? -7 : a[i] > 7 ? 7 : a[i]; } const float alen = hos_motion_len(a); /* stillness: the short window's spread of both sensors (rad/s and G, summed over the axes), about 1 G */ if (!m->stats_ready) { memcpy(m->gmean, g, sizeof m->gmean), memcpy(m->avec, a, sizeof m->avec); m->gvar = 0.01f, m->avar = 0.01f, m->amean = alen, m->stats_ready = 1; } else if (dt > 0) { const float k = dt / (HOS_MOTION_WINDOW_S + dt); float gv = 0, av = 0; for (int i = 0; i < 3; i++) { const float dg = (g[i] - m->gmean[i]) * HOS_MOTION_TAU, da = a[i] - m->avec[i]; m->gmean[i] += (g[i] - m->gmean[i]) * k, m->avec[i] += da * k; gv += dg * dg, av += da * da; } m->gvar += (gv - m->gvar) * k, m->avar += (av - m->avar) * k, m->amean += (alen - m->amean) * k; } const int still = m->gvar < HOS_MOTION_STILL_GYRO * HOS_MOTION_STILL_GYRO && m->avar < HOS_MOTION_STILL_ACCEL * HOS_MOTION_STILL_ACCEL && m->amean > 0.9f && m->amean < 1.1f; m->still_t = still ? m->still_t + dt : 0; if (m->still_t >= HOS_MOTION_STILL_S && dt > 0) { const float k = dt / ((m->learned_t < 2 ? 0.25f : 3.0f) + dt); for (int i = 0; i < 3; i++) m->bias[i] += (m->gmean[i] - m->bias[i]) * k; m->learned_t += dt; } for (int i = 0; i < 3; i++) { g[i] -= m->bias[i]; g[i] = g[i] < -5 ? -5 : g[i] > 5 ? 5 : g[i]; } if (hos_motion_len(g) < (m->threshold > 0 ? m->threshold : HOS_MOTION_THRESHOLD)) g[0] = g[1] = g[2] = 0; else m->only_accelerometer = 0; /* the turn and the tilt: the angular velocity's part about the world's up, and the rest, scaled and turned around as the menu says (the up from the accelerometer, smoothed) */ if (alen > 0.65f && alen < 1.35f) { const float k = dt / (0.1f + dt); for (int i = 0; i < 3; i++) m->up[i] += (-a[i] / alen - m->up[i]) * k; const float n = hos_motion_len(m->up); if (n > 0.001f) for (int i = 0; i < 3; i++) m->up[i] /= n; } float gs[3]; /* (the scaled rates: what the game is handed; g stays real for the filter) */ { const float sh = sens_h * (inv_h ? -1.0f : 1.0f), sv = sens_v * (inv_v ? -1.0f : 1.0f); const float t = g[0] * m->up[0] + g[1] * m->up[1] + g[2] * m->up[2]; for (int i = 0; i < 3; i++) gs[i] = sh * t * m->up[i] + sv * (g[i] - t * m->up[i]); } if (dt > HOS_MOTION_DT_MAX) dt = 0; /* (Eden ignores a stall: no rotation, no orientation step) */ for (int i = 0; i < 3; i++) m->out.rotation[i] += gs[i] * dt; /* Eden's ResetOrientation: a filter that lost track (its error over 0.1) is set again from the accelerometer once the pad has lain face up and still for 900 readings */ if (hos_motion_len(m->real_error) >= 0.1f && !m->only_accelerometer) { if (hos_motion_len(g) < 0.05f && alen > 0.9f && alen < 1.1f && a[2] <= -0.9f) { if (++m->reset_counter > 900) { m->quat[0] = m->quat[1] = m->quat[3] = 0, m->quat[2] = -1; memset(m->integral_error, 0, sizeof m->integral_error); float c[3]; for (int it = 0; it < 100 && hos_motion_len(m->real_error) >= 0.01f; it++) hos_motion_mahony(m, a, g, 0.015f, 10, c); memcpy(m->quat_out, m->quat, sizeof m->quat); /* (set again from gravity: the handed-out one too) */ m->reset_counter = 0; } } else { m->reset_counter = 0; } } if (dt > 0) { /* the real filter, then the handed-out orientation: the scaled rates plus the real filter's correction, so it loses the gyro's drift but keeps a scaled or inverted move (the gravity pull acts on the real one only; hckrman101). While only the accelerometer has been seen the two settle together. */ const int settling = m->only_accelerometer; float c[3], rs[3]; hos_motion_mahony(m, a, g, dt, 1, c); if (settling) { memcpy(m->quat_out, m->quat, sizeof m->quat); } else { /* the scaling in the world's frame (hckrman101): the real rate taken to the world by the real orientation, its vertical part (the turn) scaled by sh and the rest (the tilt) by sv, and brought back to the pad by the handed-out one, so a scaled turn while tilted turns quat_out about its own vertical and does not tip it. Both matrices map the world to the pad (their third column is the world's up seen from the pad). */ float dr[9], dq[9], w[3], go[3]; hos_motion_direction(m->quat, dr); hos_motion_direction(m->quat_out, dq); { const float sh = sens_h * (inv_h ? -1.0f : 1.0f), sv = sens_v * (inv_v ? -1.0f : 1.0f); for (int i = 0; i < 3; i++) w[i] = dr[i] * g[0] + dr[3 + i] * g[1] + dr[6 + i] * g[2]; /* (dr^T g) */ w[0] *= sv, w[1] *= sv, w[2] *= sh; for (int i = 0; i < 3; i++) go[i] = dq[3 * i] * w[0] + dq[3 * i + 1] * w[1] + dq[3 * i + 2] * w[2]; } hos_motion_rate(m, go, rs); for (int i = 0; i < 3; i++) rs[i] += c[i]; hos_motion_integrate(m->quat_out, rs, dt); } } memcpy(m->out.accel, a, sizeof a); memcpy(m->out.gyro, gs, sizeof gs); hos_motion_direction(m->quat_out, m->out.direction); return 1; }