mirror of
https://github.com/shadps4-emu/shadPS4.git
synced 2025-01-15 11:25:13 +00:00
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
This commit is contained in:
kalaposfos13
GitHub
parent
5631a31640
commit
15c9bb0e83
|
|
@ -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<int>(Input::Axis::RightY)];
|
||||
pData[i].analogButtons.l2 = states[i].axes[static_cast<int>(Input::Axis::TriggerLeft)];
|
||||
pData[i].analogButtons.r2 = states[i].axes[static_cast<int>(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<int>(Input::Axis::RightY)];
|
||||
pData->analogButtons.l2 = state.axes[static_cast<int>(Input::Axis::TriggerLeft)];
|
||||
pData->analogButtons.r2 = state.axes[static_cast<int>(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() {
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@
|
|||
// SPDX-License-Identifier: GPL-2.0-or-later
|
||||
|
||||
#include <SDL3/SDL.h>
|
||||
#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);
|
||||
|
|
|
|||
|
|
@ -33,6 +33,9 @@ struct State {
|
|||
u64 time = 0;
|
||||
int axes[static_cast<int>(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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in a new issue