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

* 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>

src/libs/controller.cpp

@@ -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 =
+			const float speed      = Length(rate);
-			    std::sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]);
 			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 x          = rate[0] * scale;
-			const float y          = data[1] * scale;
+			const float y          = rate[1] * scale;
-			const float z          = data[2] * 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.
@@ -493,6 +538,7 @@ void GameController::SetMotionSensorState(bool 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();
 	}
@@ -502,6 +548,7 @@ void GameController::ResetOrientation() {
 	Common::LockGuard lock(m_mutex);
 	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();
 }