From b884ae62d53c31a7e7f416a33b66e8e64909cd65 Mon Sep 17 00:00:00 2001
From: kalaposfos13 <153381648+kalaposfos13@users.noreply.github.com>
Date: Mon, 24 Feb 2025 19:40:25 +0100
Subject: [PATCH] Fix sensor handling if they are enabled but an error was
 thrown regardless

---
 src/core/libraries/pad/pad.cpp | 12 ++++++------
 src/input/controller.cpp       |  2 +-
 src/sdl_window.cpp             |  6 ++++--
 src/sdl_window.h               |  4 ++--
 4 files changed, 13 insertions(+), 11 deletions(-)

diff --git a/src/core/libraries/pad/pad.cpp b/src/core/libraries/pad/pad.cpp
index bcc90c49b..23e9d1431 100644
--- a/src/core/libraries/pad/pad.cpp
+++ b/src/core/libraries/pad/pad.cpp
@@ -316,11 +316,11 @@ 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;
         if (engine) {
-            const auto accel_poll_rate = engine->GetAccelPollRate();
-            if (accel_poll_rate != 0.0f) {
+            const auto gyro_poll_rate = engine->GetAccelPollRate();
+            if (gyro_poll_rate != 0.0f) {
                 GameController::CalculateOrientation(pData[i].acceleration,
                                                      pData[i].angularVelocity,
-                                                     1.0f / accel_poll_rate, pData[i].orientation);
+                                                     1.0f / gyro_poll_rate, pData[i].orientation);
             }
         }
         pData[i].touchData.touchNum =
@@ -385,10 +385,10 @@ int PS4_SYSV_ABI scePadReadState(s32 handle, OrbisPadData* pData) {
     pData->angularVelocity.y = state.angularVelocity.y;
     pData->angularVelocity.z = state.angularVelocity.z;
     if (engine) {
-        const auto accel_poll_rate = engine->GetAccelPollRate();
-        if (accel_poll_rate != 0.0f) {
+        const auto gyro_poll_rate = engine->GetAccelPollRate();
+        if (gyro_poll_rate != 0.0f) {
             GameController::CalculateOrientation(pData->acceleration, pData->angularVelocity,
-                                                 1.0f / accel_poll_rate, pData->orientation);
+                                                 1.0f / gyro_poll_rate, pData->orientation);
         }
     }
     pData->touchData.touchNum =
diff --git a/src/input/controller.cpp b/src/input/controller.cpp
index ae54553f4..bb8db9a7c 100644
--- a/src/input/controller.cpp
+++ b/src/input/controller.cpp
@@ -182,7 +182,7 @@ void GameController::CalculateOrientation(Libraries::Pad::OrbisFVector3& acceler
 
     // Normalize accelerometer measurement
     float norm = std::sqrt(ax * ax + ay * ay + az * az);
-    if (norm == 0.0f)
+    if (norm == 0.0f || deltaTime == 0.0f)
         return; // Handle NaN
     norm = 1.0f / norm;
     ax *= norm;
diff --git a/src/sdl_window.cpp b/src/sdl_window.cpp
index 943746e3f..6c372a6d5 100644
--- a/src/sdl_window.cpp
+++ b/src/sdl_window.cpp
@@ -134,13 +134,15 @@ void SDLInputEngine::Init() {
             m_gyro_poll_rate = SDL_GetGamepadSensorDataRate(m_gamepad, SDL_SENSOR_GYRO);
             LOG_INFO(Input, "Gyro initialized, poll rate: {}", m_gyro_poll_rate);
         } else {
-            LOG_ERROR(Input, "Failed to initialize gyro controls for gamepad");
+            LOG_ERROR(Input, "Failed to initialize gyro controls for gamepad, error: {}", SDL_GetError());
+            SDL_SetGamepadSensorEnabled(m_gamepad, SDL_SENSOR_GYRO, false);
         }
         if (SDL_SetGamepadSensorEnabled(m_gamepad, SDL_SENSOR_ACCEL, true)) {
             m_accel_poll_rate = SDL_GetGamepadSensorDataRate(m_gamepad, SDL_SENSOR_ACCEL);
             LOG_INFO(Input, "Accel initialized, poll rate: {}", m_accel_poll_rate);
         } else {
-            LOG_ERROR(Input, "Failed to initialize accel controls for gamepad");
+            LOG_ERROR(Input, "Failed to initialize accel controls for gamepad, error: {}", SDL_GetError());
+            SDL_SetGamepadSensorEnabled(m_gamepad, SDL_SENSOR_ACCEL, false);
         }
     }
 
diff --git a/src/sdl_window.h b/src/sdl_window.h
index 9acd2b16b..03ba0797b 100644
--- a/src/sdl_window.h
+++ b/src/sdl_window.h
@@ -27,8 +27,8 @@ public:
 private:
     SDL_Gamepad* m_gamepad = nullptr;
 
-    float m_gyro_poll_rate{};
-    float m_accel_poll_rate{};
+    float m_gyro_poll_rate = 0.0f;
+    float m_accel_poll_rate = 0.0f;
 };
 
 } // namespace Input