From b350a6801b7169eac5433c8d398ce9e7fe0f77c3 Mon Sep 17 00:00:00 2001 From: WujekFoliarz Date: Thu, 29 May 2025 19:20:33 +0200 Subject: [PATCH] Change touchpad handling and orientation calculation --- CMakeSettings.json | 4 +- src/RCa10748 | Bin 0 -> 2716 bytes src/core/libraries/pad/pad.cpp | 129 ++++++++++++++++++++++++++---- src/input/controller.cpp | 140 ++++++++++++++++++++------------- src/input/controller.h | 26 +++++- 5 files changed, 226 insertions(+), 73 deletions(-) create mode 100644 src/RCa10748 diff --git a/CMakeSettings.json b/CMakeSettings.json index bb522fcfc..ccc33157a 100644 --- a/CMakeSettings.json +++ b/CMakeSettings.json @@ -6,7 +6,7 @@ "configurationType": "Release", "buildRoot": "${projectDir}\\Build\\${name}", "installRoot": "${projectDir}\\Install\\${name}", - "cmakeCommandArgs": "", + "cmakeCommandArgs": "-DENABLE_QT_GUI=ON -DCMAKE_PREFIX_PATH=C:\\Qt\\6.9.0\\msvc2022_64", "buildCommandArgs": "", "ctestCommandArgs": "", "inheritEnvironments": [ "clang_cl_x64_x64" ], @@ -18,7 +18,7 @@ "configurationType": "Debug", "buildRoot": "${projectDir}\\Build\\${name}", "installRoot": "${projectDir}\\Install\\${name}", - "cmakeCommandArgs": "", + "cmakeCommandArgs": "-DENABLE_QT_GUI=ON -DCMAKE_PREFIX_PATH=C:\\Qt\\6.9.0\\msvc2022_64", "buildCommandArgs": "", "ctestCommandArgs": "", "inheritEnvironments": [ "clang_cl_x64_x64" ], diff --git a/src/RCa10748 b/src/RCa10748 new file mode 100644 index 0000000000000000000000000000000000000000..b3a493078d1966f825eeaed50dc984e0f3a21e49 GIT binary patch literal 2716 zcmds&T~8W86o$`rlm3UzdebIV7fYM!ou3qoKom%uG%*$xX%Glts{g*vJHxQH4T_nVv@Et?* zvQxXU3%j(YomqJi{ zinyxWRi_#BYnD(`Bo%Dc;BRrygs<>hpZk(b{?_F=McL>gcjxRzmq6&=lY69arm6xqccP64d1s8f%N1f$jWD>*U-wxx$P)EJ9Q9jga4`Ra@UBpYjtie zov?31t00$h&yjho_YPh{20s;il|WCnytgXZZJ;Ykm8z)PIm^>Z*a&-J)+e(U)Q2$J zWciv5b*BuDp1`m~m8AYUM0rXDRu^77oX#yvR}6Kkvq1z|PN2);5mGhmWhEkX^gFZ@ za1>%K?3_N~;{@KZ;|Z-*#@z`{;@WI_v^S6)x zA@njUKbQaHy@PkXRo;g8noho#RH|lV%!G~kzRW`Cx4*TJc3`lOEJa;)FmrbvlBYo- zTUor9p;yB~&*>Lm&5*ew=ehS(FR|)ona75!kDjY)#T}|gw|HD_+4wiGde2&B)xX)7 zyqJ2k^Oc*mwVI9pzn%`k>A=mwbeEk!9da^RWazoclbr}Hv@}DeuD?TH`+CxR^m9qC Pl3sVsQ_a3KigfoIk~4@1 literal 0 HcmV?d00001 diff --git a/src/core/libraries/pad/pad.cpp b/src/core/libraries/pad/pad.cpp index 5dfc68e90..098582eed 100644 --- a/src/core/libraries/pad/pad.cpp +++ b/src/core/libraries/pad/pad.cpp @@ -8,6 +8,8 @@ #include "core/libraries/pad/pad_errors.h" #include "input/controller.h" #include "pad.h" +#include +#include namespace Libraries::Pad { @@ -316,22 +318,75 @@ int PS4_SYSV_ABI scePadRead(s32 handle, OrbisPadData* pData, s32 num) { pData[i].angularVelocity.y = states[i].angularVelocity.y; pData[i].angularVelocity.z = states[i].angularVelocity.z; pData[i].orientation = {0.0f, 0.0f, 0.0f, 1.0f}; + pData[i].acceleration.x = states[i].acceleration.x * 0.098; + pData[i].acceleration.y = states[i].acceleration.y * 0.098; + pData[i].acceleration.z = states[i].acceleration.z * 0.098; + pData[i].angularVelocity.x = states[i].angularVelocity.x; + pData[i].angularVelocity.y = states[i].angularVelocity.y; + pData[i].angularVelocity.z = states[i].angularVelocity.z; + pData[i].orientation = {0.0f, 0.0f, 0.0f, 1.0f}; + if (engine) { const auto gyro_poll_rate = engine->GetAccelPollRate(); if (gyro_poll_rate != 0.0f) { - GameController::CalculateOrientation(pData[i].acceleration, - pData[i].angularVelocity, - 1.0f / gyro_poll_rate, pData[i].orientation); + auto now = std::chrono::steady_clock::now(); + float deltaTime = std::chrono::duration_cast( + now - controller->GetLastUpdate()) + .count() / + 1000000.0f; + controller->SetLastUpdate(now); + Libraries::Pad::OrbisFQuaternion lastOrientation = controller->GetLastOrientation(); + Libraries::Pad::OrbisFQuaternion outputOrientation = {0.0f, 0.0f, 0.0f, 1.0f}; + GameController::CalculateOrientation(pData->acceleration, pData->angularVelocity, + deltaTime, lastOrientation, outputOrientation); + pData[i].orientation = outputOrientation; + controller->SetLastOrientation(outputOrientation); } } + pData[i].touchData.touchNum = (states[i].touchpad[0].state ? 1 : 0) + (states[i].touchpad[1].state ? 1 : 0); + + if (controller->GetTouchCount() >= 127) { + controller->SetTouchCount(0); + } + + if (controller->GetSecondaryTouchCount() >= 127) { + controller->SetSecondaryTouchCount(0); + } + + if (pData->touchData.touchNum == 1 && controller->GetPreviousTouchNum() == 0) { + controller->SetTouchCount(controller->GetTouchCount() + 1); + controller->SetSecondaryTouchCount(controller->GetTouchCount()); + } else if (pData->touchData.touchNum == 2 && controller->GetPreviousTouchNum() == 1) { + controller->SetSecondaryTouchCount(controller->GetSecondaryTouchCount() + 1); + } else if (pData->touchData.touchNum == 0 && controller->GetPreviousTouchNum() > 0) { + if (controller->GetTouchCount() < controller->GetSecondaryTouchCount()) { + controller->SetTouchCount(controller->GetSecondaryTouchCount()); + } else { + if (controller->WasSecondaryTouchReset()) { + controller->SetTouchCount(controller->GetSecondaryTouchCount()); + controller->UnsetSecondaryTouchResetBool(); + } + } + } + + controller->SetPreviousTouchNum(pData->touchData.touchNum); + + if (pData->touchData.touchNum == 1) { + states[i].touchpad[0].ID = controller->GetTouchCount(); + states[i].touchpad[1].ID = 0; + } else if (pData->touchData.touchNum == 2) { + states[i].touchpad[0].ID = controller->GetTouchCount(); + states[i].touchpad[1].ID = controller->GetSecondaryTouchCount(); + } + pData[i].touchData.touch[0].x = states[i].touchpad[0].x; pData[i].touchData.touch[0].y = states[i].touchpad[0].y; - pData[i].touchData.touch[0].id = 1; + pData[i].touchData.touch[0].id = states[i].touchpad[0].ID; pData[i].touchData.touch[1].x = states[i].touchpad[1].x; pData[i].touchData.touch[1].y = states[i].touchpad[1].y; - pData[i].touchData.touch[1].id = 2; + pData[i].touchData.touch[1].id = states[i].touchpad[1].ID; pData[i].connected = connected; pData[i].timestamp = states[i].time; pData[i].connectedCount = connected_count; @@ -379,28 +434,74 @@ int PS4_SYSV_ABI scePadReadState(s32 handle, OrbisPadData* pData) { pData->rightStick.y = state.axes[static_cast(Input::Axis::RightY)]; pData->analogButtons.l2 = state.axes[static_cast(Input::Axis::TriggerLeft)]; pData->analogButtons.r2 = state.axes[static_cast(Input::Axis::TriggerRight)]; - pData->acceleration.x = state.acceleration.x; - pData->acceleration.y = state.acceleration.y; - pData->acceleration.z = state.acceleration.z; + pData->acceleration.x = state.acceleration.x * 0.098; + pData->acceleration.y = state.acceleration.y * 0.098; + pData->acceleration.z = state.acceleration.z * 0.098; pData->angularVelocity.x = state.angularVelocity.x; pData->angularVelocity.y = state.angularVelocity.y; pData->angularVelocity.z = state.angularVelocity.z; pData->orientation = {0.0f, 0.0f, 0.0f, 1.0f}; + if (engine) { const auto gyro_poll_rate = engine->GetAccelPollRate(); - if (gyro_poll_rate != 0.0f) { + if (gyro_poll_rate != 0.0f) { + auto now = std::chrono::steady_clock::now(); + float deltaTime = std::chrono::duration_cast(now - controller->GetLastUpdate()).count() / 1000000.0f; + controller->SetLastUpdate(now); + Libraries::Pad::OrbisFQuaternion lastOrientation = controller->GetLastOrientation(); + Libraries::Pad::OrbisFQuaternion outputOrientation = {0.0f,0.0f,0.0f,1.0f}; GameController::CalculateOrientation(pData->acceleration, pData->angularVelocity, - 1.0f / gyro_poll_rate, pData->orientation); + deltaTime, lastOrientation, outputOrientation); + pData->orientation = outputOrientation; + controller->SetLastOrientation(outputOrientation); } } - pData->touchData.touchNum = - (state.touchpad[0].state ? 1 : 0) + (state.touchpad[1].state ? 1 : 0); + pData->touchData.touchNum = (state.touchpad[0].state ? 1 : 0) + (state.touchpad[1].state ? 1 : 0); + + if (controller->GetTouchCount() >= 127) { + controller->SetTouchCount(0); + } + + if (controller->GetSecondaryTouchCount() >= 127) { + controller->SetSecondaryTouchCount(0); + } + + if (pData->touchData.touchNum == 1 && controller->GetPreviousTouchNum() == 0) { + controller->SetTouchCount(controller->GetTouchCount() + 1); + controller->SetSecondaryTouchCount(controller->GetTouchCount()); + } + else if (pData->touchData.touchNum == 2 && controller->GetPreviousTouchNum() == 1) { + controller->SetSecondaryTouchCount(controller->GetSecondaryTouchCount() + 1); + } + else if (pData->touchData.touchNum == 0 && controller->GetPreviousTouchNum() > 0) { + if (controller->GetTouchCount() < controller->GetSecondaryTouchCount()) { + controller->SetTouchCount(controller->GetSecondaryTouchCount()); + } + else { + if (controller->WasSecondaryTouchReset()) { + controller->SetTouchCount(controller->GetSecondaryTouchCount()); + controller->UnsetSecondaryTouchResetBool(); + } + } + } + + controller->SetPreviousTouchNum(pData->touchData.touchNum); + + if (pData->touchData.touchNum == 1) { + state.touchpad[0].ID = controller->GetTouchCount(); + state.touchpad[1].ID = 0; + } + else if (pData->touchData.touchNum == 2) { + state.touchpad[0].ID = controller->GetTouchCount(); + state.touchpad[1].ID = controller->GetSecondaryTouchCount(); + } + pData->touchData.touch[0].x = state.touchpad[0].x; pData->touchData.touch[0].y = state.touchpad[0].y; - pData->touchData.touch[0].id = 1; + pData->touchData.touch[0].id = state.touchpad[0].ID; pData->touchData.touch[1].x = state.touchpad[1].x; pData->touchData.touch[1].y = state.touchpad[1].y; - pData->touchData.touch[1].id = 2; + pData->touchData.touch[1].id = state.touchpad[1].ID; pData->timestamp = state.time; pData->connected = true; // isConnected; //TODO fix me proper pData->connectedCount = 1; // connectedCount; diff --git a/src/input/controller.cpp b/src/input/controller.cpp index bb8db9a7c..e65bc940e 100644 --- a/src/input/controller.cpp +++ b/src/input/controller.cpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright 2024 shadPS4 Emulator Project +// SPDX-FileCopyrightText: Copyright 2024 shadPS4 Emulator Project // SPDX-License-Identifier: GPL-2.0-or-later #include @@ -165,69 +165,36 @@ void GameController::Acceleration(int id, const float acceleration[3]) { AddState(state); } -// Stolen from -// https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MahonyAHRS.cs -float eInt[3] = {0.0f, 0.0f, 0.0f}; // Integral error terms -const float Kp = 50.0f; // Proportional gain -const float Ki = 1.0f; // Integral gain -Libraries::Pad::OrbisFQuaternion o = {1, 0, 0, 0}; void GameController::CalculateOrientation(Libraries::Pad::OrbisFVector3& acceleration, Libraries::Pad::OrbisFVector3& angularVelocity, float deltaTime, + Libraries::Pad::OrbisFQuaternion& lastOrientation, Libraries::Pad::OrbisFQuaternion& orientation) { - float ax = acceleration.x, ay = acceleration.y, az = acceleration.z; - float gx = angularVelocity.x, gy = angularVelocity.y, gz = angularVelocity.z; + Libraries::Pad::OrbisFQuaternion q = lastOrientation; + Libraries::Pad::OrbisFQuaternion ω = {angularVelocity.x, angularVelocity.y,angularVelocity.z,0.0f}; - float q1 = o.w, q2 = o.x, q3 = o.y, q4 = o.z; + Libraries::Pad::OrbisFQuaternion qω = {q.w * ω.x + q.x * ω.w + q.y * ω.z - q.z * ω.y, + q.w * ω.y + q.y * ω.w + q.z * ω.x - q.x * ω.z, + q.w * ω.z + q.z * ω.w + q.x * ω.y - q.y * ω.x, + q.w * ω.w - q.x * ω.x - q.y * ω.y - q.z * ω.z}; - // Normalize accelerometer measurement - float norm = std::sqrt(ax * ax + ay * ay + az * az); - if (norm == 0.0f || deltaTime == 0.0f) - return; // Handle NaN - norm = 1.0f / norm; - ax *= norm; - ay *= norm; - az *= norm; + Libraries::Pad::OrbisFQuaternion qDot = {0.5f * qω.x, 0.5f * qω.y, 0.5f * qω.z, 0.5f * qω.w}; - // Estimated direction of gravity - float vx = 2.0f * (q2 * q4 - q1 * q3); - float vy = 2.0f * (q1 * q2 + q3 * q4); - float vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; + q.x += qDot.x * deltaTime; + q.y += qDot.y * deltaTime; + q.z += qDot.z * deltaTime; + q.w += qDot.w * deltaTime; - // Error is cross product between estimated direction and measured direction of gravity - float ex = (ay * vz - az * vy); - float ey = (az * vx - ax * vz); - float ez = (ax * vy - ay * vx); - if (Ki > 0.0f) { - eInt[0] += ex * deltaTime; // Accumulate integral error - eInt[1] += ey * deltaTime; - eInt[2] += ez * deltaTime; - } else { - eInt[0] = eInt[1] = eInt[2] = 0.0f; // Prevent integral wind-up - } + float norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); + q.x /= norm; + q.y /= norm; + q.z /= norm; + q.w /= norm; - // Apply feedback terms - gx += Kp * ex + Ki * eInt[0]; - gy += Kp * ey + Ki * eInt[1]; - gz += Kp * ez + Ki * eInt[2]; - - //// Integrate rate of change of quaternion - q1 += (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltaTime); - q2 += (q1 * gx + q3 * gz - q4 * gy) * (0.5f * deltaTime); - q3 += (q1 * gy - q2 * gz + q4 * gx) * (0.5f * deltaTime); - q4 += (q1 * gz + q2 * gy - q3 * gx) * (0.5f * deltaTime); - - // Normalize quaternion - norm = std::sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); - norm = 1.0f / norm; - orientation.w = q1 * norm; - orientation.x = q2 * norm; - orientation.y = q3 * norm; - orientation.z = q4 * norm; - o.w = q1 * norm; - o.x = q2 * norm; - o.y = q3 * norm; - o.z = q4 * norm; + orientation.x = q.x; + orientation.y = q.z; + orientation.z = q.y; // yes this is swapped on purpose don't touch it + orientation.w = q.w; LOG_DEBUG(Lib_Pad, "Calculated orientation: {:.2f} {:.2f} {:.2f} {:.2f}", orientation.x, orientation.y, orientation.z, orientation.w); } @@ -260,6 +227,69 @@ void GameController::SetTouchpadState(int touchIndex, bool touchDown, float x, f } } +u8 GameController::GetTouchCount() { + std::scoped_lock lock{m_mutex}; + return m_touch_count; +} + +void GameController::SetTouchCount(u8 touchCount) { + std::scoped_lock lock{m_mutex}; + m_touch_count = touchCount; +} + +u8 GameController::GetSecondaryTouchCount() { + std::scoped_lock lock{m_mutex}; + return m_secondary_touch_count; +} + +void GameController::SetSecondaryTouchCount(u8 touchCount) { + std::scoped_lock lock{m_mutex}; + m_secondary_touch_count = touchCount; + if (touchCount == 0) { + m_was_secondary_reset = true; + } +} + +u8 GameController::GetPreviousTouchNum() { + std::scoped_lock lock{m_mutex}; + return m_previous_touchnum; +} + +void GameController::SetPreviousTouchNum(u8 touchNum) { + std::scoped_lock lock{m_mutex}; + m_previous_touchnum = touchNum; +} + +bool GameController::WasSecondaryTouchReset() { + std::scoped_lock lock{m_mutex}; + return m_was_secondary_reset; +} + +void GameController::UnsetSecondaryTouchResetBool() { + std::scoped_lock lock{m_mutex}; + m_was_secondary_reset = false; +} + +void GameController::SetLastOrientation(Libraries::Pad::OrbisFQuaternion& orientation) { + std::scoped_lock lock{m_mutex}; + m_orientation = orientation; +} + +Libraries::Pad::OrbisFQuaternion GameController::GetLastOrientation() { + std::scoped_lock lock{m_mutex}; + return m_orientation; +} + +std::chrono::steady_clock::time_point GameController::GetLastUpdate() { + std::scoped_lock lock{m_mutex}; + return m_last_update; +} + +void GameController::SetLastUpdate(std::chrono::steady_clock::time_point lastUpdate) { + std::scoped_lock lock{m_mutex}; + m_last_update = lastUpdate; +} + void GameController::SetEngine(std::unique_ptr engine) { std::scoped_lock _{m_mutex}; m_engine = std::move(engine); diff --git a/src/input/controller.h b/src/input/controller.h index bbaed75ea..8cc127d72 100644 --- a/src/input/controller.h +++ b/src/input/controller.h @@ -23,6 +23,7 @@ enum class Axis { }; struct TouchpadEntry { + u8 ID = 0; bool state{}; u16 x{}; u16 y{}; @@ -38,7 +39,7 @@ public: Libraries::Pad::OrbisPadButtonDataOffset buttonsState{}; u64 time = 0; - int axes[static_cast(Axis::AxisMax)] = {128, 128, 128, 128, 0, 0}; + int axes[static_cast(Axis::AxisMax)] = {128, 128, 128, 128, 0, 0}; TouchpadEntry touchpad[2] = {{false, 0, 0}, {false, 0, 0}}; Libraries::Pad::OrbisFVector3 acceleration = {0.0f, 0.0f, 0.0f}; Libraries::Pad::OrbisFVector3 angularVelocity = {0.0f, 0.0f, 0.0f}; @@ -81,10 +82,24 @@ public: void SetEngine(std::unique_ptr); Engine* GetEngine(); u32 Poll(); + + u8 GetTouchCount(); + void SetTouchCount(u8 touchCount); + u8 GetSecondaryTouchCount(); + void SetSecondaryTouchCount(u8 touchCount); + u8 GetPreviousTouchNum(); + void SetPreviousTouchNum(u8 touchNum); + bool WasSecondaryTouchReset(); + void UnsetSecondaryTouchResetBool(); + void SetLastOrientation(Libraries::Pad::OrbisFQuaternion& orientation); + Libraries::Pad::OrbisFQuaternion GetLastOrientation(); + std::chrono::steady_clock::time_point GetLastUpdate(); + void SetLastUpdate(std::chrono::steady_clock::time_point lastUpdate); static void CalculateOrientation(Libraries::Pad::OrbisFVector3& acceleration, Libraries::Pad::OrbisFVector3& angularVelocity, float deltaTime, + Libraries::Pad::OrbisFQuaternion& lastOrientation, Libraries::Pad::OrbisFQuaternion& orientation); private: @@ -98,9 +113,16 @@ private: int m_connected_count = 0; u32 m_states_num = 0; u32 m_first_state = 0; + u8 m_touch_count = 0; + u8 m_secondary_touch_count = 0; + u8 m_previous_touch_count = 0; + u8 m_previous_touchnum = 0; + bool m_was_secondary_reset = false; std::array m_states; std::array m_private; - + std::chrono::steady_clock::time_point m_last_update = {}; + Libraries::Pad::OrbisFQuaternion m_orientation = {0.0f, 0.0f, 0.0f, 1.0f}; + std::unique_ptr m_engine = nullptr; };