diff --git a/src/core/libraries/pad/pad.cpp b/src/core/libraries/pad/pad.cpp index 5dfc68e90..42582783b 100644 --- a/src/core/libraries/pad/pad.cpp +++ b/src/core/libraries/pad/pad.cpp @@ -316,22 +316,79 @@ 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}; - if (engine) { + 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; + + if (engine && handle == 1) { 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 (handle == 1) { + 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(); + } + } else { + states[i].touchpad[0].ID = 1; + states[i].touchpad[1].ID = 2; + } + 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; @@ -376,31 +433,85 @@ int PS4_SYSV_ABI scePadReadState(s32 handle, OrbisPadData* pData) { pData->leftStick.x = state.axes[static_cast(Input::Axis::LeftX)]; pData->leftStick.y = state.axes[static_cast(Input::Axis::LeftY)]; pData->rightStick.x = state.axes[static_cast(Input::Axis::RightX)]; + pData->rightStick.x = state.axes[static_cast(Input::Axis::RightX)]; 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) { + + // Only do this on handle 1 for now + if (engine && handle == 1) { const auto gyro_poll_rate = engine->GetAccelPollRate(); 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); + + // Only do this on handle 1 for now + if (handle == 1) { + 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(); + } + } else { + state.touchpad[0].ID = 1; + state.touchpad[1].ID = 2; + } + 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..42cabb837 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,37 @@ 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.y; + orientation.z = q.z; + orientation.w = q.w; LOG_DEBUG(Lib_Pad, "Calculated orientation: {:.2f} {:.2f} {:.2f} {:.2f}", orientation.x, orientation.y, orientation.z, orientation.w); } @@ -260,6 +228,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..f427a55ec 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{}; @@ -82,9 +83,23 @@ public: 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,8 +113,15 @@ 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; };