From 15c9bb0e83dd809790b366dc125855c050ca8197 Mon Sep 17 00:00:00 2001 From: kalaposfos13 <153381648+kalaposfos13@users.noreply.github.com> Date: Wed, 1 Jan 2025 19:05:22 +0100 Subject: [PATCH] Motion controls (#1984) * Initial motion controls * Store sensor polling rate, and add more logging * Revert commented out logging for testing purposes * Code cleanup & clang * New orientation handling * clang --- src/core/libraries/pad/pad.cpp | 46 +++++++------- src/input/controller.cpp | 110 +++++++++++++++++++++++++++++++++ src/input/controller.h | 12 ++++ src/sdl_window.cpp | 14 +++++ 4 files changed, 160 insertions(+), 22 deletions(-) diff --git a/src/core/libraries/pad/pad.cpp b/src/core/libraries/pad/pad.cpp index 98f086dd..27564294 100644 --- a/src/core/libraries/pad/pad.cpp +++ b/src/core/libraries/pad/pad.cpp @@ -104,8 +104,8 @@ int PS4_SYSV_ABI scePadGetControllerInformation(s32 handle, OrbisPadControllerIn pInfo->touchPadInfo.pixelDensity = 1; pInfo->touchPadInfo.resolution.x = 1920; pInfo->touchPadInfo.resolution.y = 950; - pInfo->stickInfo.deadZoneLeft = 2; - pInfo->stickInfo.deadZoneRight = 2; + pInfo->stickInfo.deadZoneLeft = 20; + pInfo->stickInfo.deadZoneRight = 20; pInfo->connectionType = ORBIS_PAD_PORT_TYPE_STANDARD; pInfo->connectedCount = 1; pInfo->connected = true; @@ -286,6 +286,7 @@ int PS4_SYSV_ABI scePadOutputReport() { } int PS4_SYSV_ABI scePadRead(s32 handle, OrbisPadData* pData, s32 num) { + LOG_TRACE(Lib_Pad, "called"); int connected_count = 0; bool connected = false; Input::State states[64]; @@ -304,16 +305,15 @@ int PS4_SYSV_ABI scePadRead(s32 handle, OrbisPadData* pData, s32 num) { pData[i].rightStick.y = states[i].axes[static_cast(Input::Axis::RightY)]; pData[i].analogButtons.l2 = states[i].axes[static_cast(Input::Axis::TriggerLeft)]; pData[i].analogButtons.r2 = states[i].axes[static_cast(Input::Axis::TriggerRight)]; - pData[i].orientation.x = 0.0f; - pData[i].orientation.y = 0.0f; - pData[i].orientation.z = 0.0f; - pData[i].orientation.w = 1.0f; - pData[i].acceleration.x = 0.0f; - pData[i].acceleration.y = 0.0f; - pData[i].acceleration.z = 0.0f; - pData[i].angularVelocity.x = 0.0f; - pData[i].angularVelocity.y = 0.0f; - pData[i].angularVelocity.z = 0.0f; + pData[i].acceleration.x = states[i].acceleration.x; + pData[i].acceleration.y = states[i].acceleration.y; + pData[i].acceleration.z = states[i].acceleration.z; + pData[i].angularVelocity.x = states[i].angularVelocity.x; + pData[i].angularVelocity.y = states[i].angularVelocity.y; + pData[i].angularVelocity.z = states[i].angularVelocity.z; + Input::GameController::CalculateOrientation(pData[i].acceleration, pData[i].angularVelocity, + 1.0f / controller->accel_poll_rate, + pData[i].orientation); pData[i].touchData.touchNum = (states[i].touchpad[0].state ? 1 : 0) + (states[i].touchpad[1].state ? 1 : 0); pData[i].touchData.touch[0].x = states[i].touchpad[0].x; @@ -352,6 +352,7 @@ int PS4_SYSV_ABI scePadReadHistory() { } int PS4_SYSV_ABI scePadReadState(s32 handle, OrbisPadData* pData) { + LOG_TRACE(Lib_Pad, "called"); if (handle == ORBIS_PAD_ERROR_DEVICE_NO_HANDLE) { return ORBIS_PAD_ERROR_INVALID_HANDLE; } @@ -367,16 +368,15 @@ 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->orientation.x = 0; - pData->orientation.y = 0; - pData->orientation.z = 0; - pData->orientation.w = 1; - pData->acceleration.x = 0.0f; - pData->acceleration.y = 0.0f; - pData->acceleration.z = 0.0f; - pData->angularVelocity.x = 0.0f; - pData->angularVelocity.y = 0.0f; - pData->angularVelocity.z = 0.0f; + pData->acceleration.x = state.acceleration.x; + pData->acceleration.y = state.acceleration.y; + pData->acceleration.z = state.acceleration.z; + pData->angularVelocity.x = state.angularVelocity.x; + pData->angularVelocity.y = state.angularVelocity.y; + pData->angularVelocity.z = state.angularVelocity.z; + Input::GameController::CalculateOrientation(pData->acceleration, pData->angularVelocity, + 1.0f / controller->accel_poll_rate, + pData->orientation); pData->touchData.touchNum = (state.touchpad[0].state ? 1 : 0) + (state.touchpad[1].state ? 1 : 0); pData->touchData.touch[0].x = state.touchpad[0].x; @@ -498,6 +498,8 @@ int PS4_SYSV_ABI scePadSetLoginUserNumber() { int PS4_SYSV_ABI scePadSetMotionSensorState(s32 handle, bool bEnable) { LOG_ERROR(Lib_Pad, "(STUBBED) called"); return ORBIS_OK; + // it's already handled by the SDL backend and will be on no matter what + // (assuming the controller supports it) } int PS4_SYSV_ABI scePadSetProcessFocus() { diff --git a/src/input/controller.cpp b/src/input/controller.cpp index 3927b096..daef9c94 100644 --- a/src/input/controller.cpp +++ b/src/input/controller.cpp @@ -2,6 +2,7 @@ // SPDX-License-Identifier: GPL-2.0-or-later #include +#include "common/logging/log.h" #include "core/libraries/kernel/time.h" #include "core/libraries/pad/pad.h" #include "input/controller.h" @@ -116,6 +117,103 @@ void GameController::Axis(int id, Input::Axis axis, int value) { AddState(state); } +void GameController::Gyro(int id, const float gyro[3]) { + std::scoped_lock lock{m_mutex}; + auto state = GetLastState(); + state.time = Libraries::Kernel::sceKernelGetProcessTime(); + + // Update the angular velocity (gyro data) + state.angularVelocity.x = gyro[0]; // X-axis + state.angularVelocity.y = gyro[1]; // Y-axis + state.angularVelocity.z = gyro[2]; // Z-axis + + AddState(state); +} +void GameController::Acceleration(int id, const float acceleration[3]) { + std::scoped_lock lock{m_mutex}; + auto state = GetLastState(); + state.time = Libraries::Kernel::sceKernelGetProcessTime(); + + // Update the acceleration values + state.acceleration.x = acceleration[0]; // X-axis + state.acceleration.y = acceleration[1]; // Y-axis + state.acceleration.z = acceleration[2]; // Z-axis + + 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& orientation) { + float ax = acceleration.x, ay = acceleration.y, az = acceleration.z; + float gx = angularVelocity.x, gy = angularVelocity.y, gz = angularVelocity.z; + + float q1 = o.w, q2 = o.x, q3 = o.y, q4 = o.z; + + // Normalize accelerometer measurement + float norm = std::sqrt(ax * ax + ay * ay + az * az); + if (norm == 0.0f) + return; // Handle NaN + norm = 1.0f / norm; + ax *= norm; + ay *= norm; + az *= norm; + + // 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; + + // 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 + } + + // 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 + // float pa = q2, pb = q3, pc = q4; + // q1 += (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltaTime); + // q2 += (pa * gx + pb * gz - pc * gy) * (0.5f * deltaTime); + // q3 += (pb * gy - pa * gz + pc * gx) * (0.5f * deltaTime); + // q4 += (pc * gz + pa * gy - pb * gx) * (0.5f * deltaTime); + 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; + LOG_DEBUG(Lib_Pad, "Calculated orientation: {:.2f} {:.2f} {:.2f} {:.2f}", orientation.x, + orientation.y, orientation.z, orientation.w); +} + void GameController::SetLightBarRGB(u8 r, u8 g, u8 b) { if (m_sdl_gamepad != nullptr) { SDL_SetGamepadLED(m_sdl_gamepad, r, g, b); @@ -149,6 +247,18 @@ void GameController::TryOpenSDLController() { int gamepad_count; SDL_JoystickID* gamepads = SDL_GetGamepads(&gamepad_count); m_sdl_gamepad = gamepad_count > 0 ? SDL_OpenGamepad(gamepads[0]) : nullptr; + if (SDL_SetGamepadSensorEnabled(m_sdl_gamepad, SDL_SENSOR_GYRO, true)) { + gyro_poll_rate = SDL_GetGamepadSensorDataRate(m_sdl_gamepad, SDL_SENSOR_GYRO); + LOG_INFO(Input, "Gyro initialized, poll rate: {}", gyro_poll_rate); + } else { + LOG_ERROR(Input, "Failed to initialize gyro controls for gamepad"); + } + if (SDL_SetGamepadSensorEnabled(m_sdl_gamepad, SDL_SENSOR_ACCEL, true)) { + accel_poll_rate = SDL_GetGamepadSensorDataRate(m_sdl_gamepad, SDL_SENSOR_ACCEL); + LOG_INFO(Input, "Accel initialized, poll rate: {}", accel_poll_rate); + } else { + LOG_ERROR(Input, "Failed to initialize accel controls for gamepad"); + } SDL_free(gamepads); SetLightBarRGB(0, 0, 255); diff --git a/src/input/controller.h b/src/input/controller.h index d425fb46..c6fc02c2 100644 --- a/src/input/controller.h +++ b/src/input/controller.h @@ -33,6 +33,9 @@ struct State { u64 time = 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}; + Libraries::Pad::OrbisFQuaternion orientation = {0.0f, 0.0f, 0.0f, 1.0f}; }; inline int GetAxis(int min, int max, int value) { @@ -53,12 +56,21 @@ public: void CheckButton(int id, Libraries::Pad::OrbisPadButtonDataOffset button, bool isPressed); void AddState(const State& state); void Axis(int id, Input::Axis axis, int value); + void Gyro(int id, const float gyro[3]); + void Acceleration(int id, const float acceleration[3]); void SetLightBarRGB(u8 r, u8 g, u8 b); bool SetVibration(u8 smallMotor, u8 largeMotor); void SetTouchpadState(int touchIndex, bool touchDown, float x, float y); void TryOpenSDLController(); u32 Poll(); + float gyro_poll_rate; + float accel_poll_rate; + static void CalculateOrientation(Libraries::Pad::OrbisFVector3& acceleration, + Libraries::Pad::OrbisFVector3& angularVelocity, + float deltaTime, + Libraries::Pad::OrbisFQuaternion& orientation); + private: struct StateInternal { bool obtained = false; diff --git a/src/sdl_window.cpp b/src/sdl_window.cpp index 50c3e93e..d694b093 100644 --- a/src/sdl_window.cpp +++ b/src/sdl_window.cpp @@ -161,6 +161,20 @@ void WindowSDL::WaitEvent() { case SDL_EVENT_GAMEPAD_TOUCHPAD_MOTION: OnGamepadEvent(&event); break; + // i really would have appreciated ANY KIND OF DOCUMENTATION ON THIS + // AND IT DOESN'T EVEN USE PROPER ENUMS + case SDL_EVENT_GAMEPAD_SENSOR_UPDATE: + switch ((SDL_SensorType)event.gsensor.sensor) { + case SDL_SENSOR_GYRO: + controller->Gyro(0, event.gsensor.data); + break; + case SDL_SENSOR_ACCEL: + controller->Acceleration(0, event.gsensor.data); + break; + default: + break; + } + break; case SDL_EVENT_QUIT: is_open = false; break;