raphiaura/KytyPS5
1
0
Fork 0
mirror of https://github.com/KytyPS5/KytyPS5.git synced 2026-10-20 10:56:58 +02:00
Code Issues Packages Projects Releases Wiki Activity

input: correct gyro orientation drift with the accelerometer (#899)

Browse Source 
* input: correct gyro orientation drift with the accelerometer

* input: simplify gyro drift correction and reset gravity on reconnect

---------

Co-authored-by: nmzik <Nmzik@mail.ru>
This commit is contained in:
Ewen trividic
2026-09-30 20:40:53 +02:00
committed by GitHub
parent bc193f5272
commit b7a1fac898
1 changed files with 60 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/libs/controller.cpp
+60 -13
View File
@@ -133,6 +133,9 @@ private:
int m_connected_count = 0;
bool m_motion_enabled = true;
uint64_t m_gyro_time = 0;
// Accelerometer gravity direction in the reset frame.
std::array<float, 3> m_up_reference {0.0f, 1.0f, 0.0f};
bool m_up_reference_valid = false;
ControllerState m_state;
ControllerState m_states[STATES_MAX];
bool m_obtained[STATES_MAX] {};
@@ -348,6 +351,7 @@ void GameController::CheckActive() {
m_connected = new_connected;
m_state = {};
m_gyro_time = 0;
m_up_reference_valid = false;
m_states_num = 0;
m_first_state = 0;
m_next_touch_id = 1;
@@ -439,6 +443,29 @@ void GameController::TouchPad(int id, int finger, bool down, float x, float y) {
}
}
namespace {
using Vec3 = std::array<float, 3>;
using Quat = std::array<float, 4>; // x, y, z, w
Vec3 QuatRotate(const Quat& q, const Vec3& v) {
const Vec3 t {2.0f * (q[1] * v[2] - q[2] * v[1]), 2.0f * (q[2] * v[0] - q[0] * v[2]),
2.0f * (q[0] * v[1] - q[1] * v[0])};
return {v[0] + q[3] * t[0] + (q[1] * t[2] - q[2] * t[1]),
v[1] + q[3] * t[1] + (q[2] * t[0] - q[0] * t[2]),
v[2] + q[3] * t[2] + (q[0] * t[1] - q[1] * t[0])};
}
Quat QuatConjugate(const Quat& q) {
return {-q[0], -q[1], -q[2], q[3]};
}
float Length(const Vec3& v) {
return std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}
} // namespace
void GameController::Motion(int id, Sensor sensor, const float* data, uint64_t time_us) {
Common::LockGuard lock(m_mutex);
if (id != m_active_id || !m_motion_enabled) {
@@ -450,20 +477,38 @@ void GameController::Motion(int id, Sensor sensor, const float* data, uint64_t t
for (int i = 0; i < 3; i++) {
m_state.accel[i] = data[i] / SDL_STANDARD_GRAVITY;
}
// Capture gravity near 1 G to limit interference from linear acceleration.
const float magnitude = Length(m_state.accel);
if (!m_up_reference_valid && magnitude > 0.9f && magnitude < 1.1f) {
const Vec3 up {m_state.accel[0] / magnitude, m_state.accel[1] / magnitude,
m_state.accel[2] / magnitude};
m_up_reference = QuatRotate(m_state.orientation, up);
m_up_reference_valid = true;
}
} else {
std::copy_n(data, 3, m_state.gyro.begin());
// Mahony tilt correction; heading is unobservable and reported gyro values stay raw.
auto rate = m_state.gyro;
const float magnitude = Length(m_state.accel);
if (m_up_reference_valid && magnitude > 0.8f && magnitude < 1.2f) {
const Vec3 measured {m_state.accel[0] / magnitude, m_state.accel[1] / magnitude,
m_state.accel[2] / magnitude};
const Vec3 predicted = QuatRotate(QuatConjugate(m_state.orientation), m_up_reference);
rate[0] += measured[1] * predicted[2] - measured[2] * predicted[1];
rate[1] += measured[2] * predicted[0] - measured[0] * predicted[2];
rate[2] += measured[0] * predicted[1] - measured[1] * predicted[0];
}
// Do not extrapolate a single sample across lost reports (e.g. loss of window focus).
constexpr uint64_t max_gyro_interval_us = 100000;
if (m_gyro_time != 0 && time_us > m_gyro_time &&
time_us - m_gyro_time <= max_gyro_interval_us) {
const float dt = static_cast<float>(time_us - m_gyro_time) * 0.000001f;
const float speed =
std::sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]);
const float dt = static_cast<float>(time_us - m_gyro_time) * 0.000001f;
const float speed = Length(rate);
const float half_angle = speed * dt * 0.5f;
const float scale = speed > 0.0f ? std::sin(half_angle) / speed : 0.0f;
const float x = data[0] * scale;
const float y = data[1] * scale;
const float z = data[2] * scale;
const float x = rate[0] * scale;
const float y = rate[1] * scale;
const float z = rate[2] * scale;
const float w = std::cos(half_angle);
const auto q = m_state.orientation;
// Accumulate body-local rotation relative to connection / orientation reset.
@@ -489,19 +534,21 @@ void GameController::Motion(int id, Sensor sensor, const float* data, uint64_t t
void GameController::SetMotionSensorState(bool enable) {
Common::LockGuard lock(m_mutex);
if (m_motion_enabled != enable) {
m_motion_enabled = enable;
m_gyro_time = 0;
m_state.accel = {0.0f, 1.0f, 0.0f};
m_state.gyro = {};
m_state.time = LibKernel::KernelGetProcessTime();
m_motion_enabled = enable;
m_gyro_time = 0;
m_state.accel = {0.0f, 1.0f, 0.0f};
m_state.gyro = {};
m_up_reference_valid = false;
m_state.time = LibKernel::KernelGetProcessTime();
AddState();
}
}
void GameController::ResetOrientation() {
Common::LockGuard lock(m_mutex);
m_state.orientation = {0.0f, 0.0f, 0.0f, 1.0f};
m_gyro_time = 0;
m_state.orientation = {0.0f, 0.0f, 0.0f, 1.0f};
m_gyro_time = 0;
m_up_reference_valid = false;
m_state.time = LibKernel::KernelGetProcessTime();
AddState();
}