diff --git a/core/libs/prx/libScePad/include/Pad.hpp b/core/libs/prx/libScePad/include/Pad.hpp index ca6513109..6484c286e 100644 --- a/core/libs/prx/libScePad/include/Pad.hpp +++ b/core/libs/prx/libScePad/include/Pad.hpp @@ -1,6 +1,8 @@ #ifndef CORE_LIBS_PRX_LIBSCEPAD_PAD_HPP #define CORE_LIBS_PRX_LIBSCEPAD_PAD_HPP +#include + constexpr int PAD_OK = 0; constexpr int PAD_ERROR_INVALID_ARG = -2137915390; constexpr int PAD_ERROR_INVALID_HANDLE = -2137915384; @@ -14,4 +16,37 @@ constexpr int PAD_HANDLE = 1; constexpr int PAD_CONNECTION_TYPE_LOCAL = 0; constexpr int PAD_DEVICE_CLASS_STANDARD = 0; +// ScePadDeviceType values reconstructed from DualSense HID telemetry; no current title queries them, +// they only describe the virtual device. +constexpr int PAD_DEVICE_TYPE_NONE = 0; +constexpr int PAD_DEVICE_TYPE_NAVIGATOR = 1; +constexpr int PAD_DEVICE_TYPE_DUAL_SHOCK_4 = 2; +constexpr int PAD_DEVICE_TYPE_DUAL_SENSE = 3; +constexpr int PAD_DEVICE_TYPE_DUAL_SENSE_EDGE = 4; + +// ScePadConnectType values from the PS4/PS5 SDK. +constexpr int PAD_CONNECT_TYPE_UNREGISTERED = 0; +constexpr int PAD_CONNECT_TYPE_USB = 1; +constexpr int PAD_CONNECT_TYPE_BLUETOOTH = 2; + +// Bluetooth public address of the virtual pad; DC:8C:37 is the Sony OUI a retail DualSense uses. +inline constexpr std::uint8_t PAD_BD_ADDRESS[6] = {0xDC, 0x8C, 0x37, 0x51, 0x01, 0xF4}; + +// Mirrors scePadGetInfo's ScePadInfo: per-port slot table of the pad service. The exact SDK field names +// are not public; this is the layout every out-param filler in this library writes and documents. +struct PadInfo { + std::uint8_t maxConnectCount; // 0x00 + std::uint8_t connectedCount[2]; // 0x01 per personal port type (standard, special) + std::uint8_t reserved0[2]; // 0x03 + struct PadTypeInfo { + std::uint8_t connectPort; // index within the port type + std::uint8_t status; // 0 free, 1 connected + std::uint8_t deviceType; // PAD_DEVICE_TYPE_* + std::uint8_t connectType; // PAD_CONNECT_TYPE_* + std::uint8_t bluetoothMacAddress[6]; // empty unless status == 1 + } padTypeInfo[8]; // 0x05, 10 bytes per slot + std::uint8_t reserved1[4]; // 0x55 +}; +static_assert(sizeof(PadInfo) == 0x59); + #endif diff --git a/core/libs/prx/libScePad/include/PadState.hpp b/core/libs/prx/libScePad/include/PadState.hpp index 4d8646d29..090948961 100644 --- a/core/libs/prx/libScePad/include/PadState.hpp +++ b/core/libs/prx/libScePad/include/PadState.hpp @@ -6,6 +6,15 @@ #include #include "SceTypes.hpp" +struct PadTouchPoint { + bool active = false; + std::uint16_t x = 0; // 0..1919 + std::uint16_t y = 0; // 0..942 + std::uint8_t id = 0; + + bool operator==(const PadTouchPoint&) const = default; +}; + struct PadInputState { std::uint32_t buttons = 0; std::array sticks{128, 128, 128, 128}; @@ -13,14 +22,49 @@ struct PadInputState { std::uint8_t analogButtonsR2 = 0; bool touchLeft = false; bool touchRight = false; + // Host gamepad extras (all neutral when no gamepad is present). + bool hasMotion = false; // host pad delivers accelerometer + gyroscope + std::array accel{0.0f, 9.80665f, 0.0f}; // m/s^2, +Y up when the pad lies flat + std::array gyro{0.0f, 0.0f, 0.0f}; // rad/s + std::array touch{}; // host touchpad fingers + std::uint8_t deviceKind = 0; // 0 none, 1 DualSense, 2 DualShock 4, 3 other pad +}; + +// Output requests issued by the guest (rumble, light bar, adaptive triggers), applied to the host pad by the window thread. +struct PadTriggerRequest { + bool valid = false; // an effect other than "off" is engaged + std::uint8_t effect[11]{}; // DualSense native trigger effect block + std::uint8_t fallback = 0; // 0..255 rumble strength for pads without adaptive triggers + std::uint32_t mode = 0; // guest ScePadTriggerEffectMode +}; + +struct PadOutputState { + std::uint32_t sequence = 0; // bumped on every change + std::uint8_t vibrationLarge = 0; + std::uint8_t vibrationSmall = 0; + bool lightBarValid = false; // false: default colour + std::uint8_t lightBar[3]{}; + PadTriggerRequest trigger[2]; // 0 = L2, 1 = R2 + bool triggerTouched = false; // an effect struct was received at least once + bool motionEnabled = true; + int vibrationMode = 0; // last ScePadSetVibrationMode argument (0/r0 = desktop, 1/r1 = embedded) }; namespace Pad { void Initialize(); PadData ReadState(); +void SetVibration(std::uint8_t large, std::uint8_t small); +void SetVibrationMode(int mode); +void SetLightBar(bool valid, std::uint8_t r, std::uint8_t g, std::uint8_t b); +// trigger: 0 = L2, 1 = R2; command is a ScePadTriggerEffectCommandData (56 bytes: u32 mode, padding, command data at +8). +void SetTriggerCommand(int trigger, const std::uint8_t* command); +void ResetOrientation(); +void SetMotionEnabled(bool enabled); } extern "C" void PadPublishInput_nid_postfix(const PadInputState& input); extern "C" void PadReportInputFailure_nid_postfix(std::exception_ptr error); +// Copies the current output request when it differs from *seenSequence, updating it; returns true when a copy was made. +extern "C" bool PadFetchOutput_nid_postfix(std::uint32_t* seenSequence, PadOutputState* out); #endif diff --git a/core/libs/prx/libScePad/src/PadState.cpp b/core/libs/prx/libScePad/src/PadState.cpp index b9158a4f3..f64b60723 100644 --- a/core/libs/prx/libScePad/src/PadState.cpp +++ b/core/libs/prx/libScePad/src/PadState.cpp @@ -1,20 +1,103 @@ #include "prx/libScePad/include/PadState.hpp" +#include "prx/libScePad/include/Pad.hpp" #include "prx/libkernel/Time/include/Time.hpp" +#include +#include +#include #include #include namespace { std::mutex stateMutex; PadInputState state; + PadOutputState output; std::uint64_t timestamp = 0; std::exception_ptr failure; bool initialized = false; + + // Orientation fusion: gyro integration with accelerometer tilt correction (Mahony filter). The identity + // quaternion is the pad lying flat and face up, which is also what a pad without sensors reports. + struct Quat { float w = 1.0f, x = 0.0f, y = 0.0f, z = 0.0f; }; + Quat orientation; + std::uint64_t lastFuseTime = 0; + float biasIntegral[3] = {0.0f, 0.0f, 0.0f}; + std::uint8_t nextTouchId = 0; + bool prevTouchActive[2] = {false, false}; + std::uint8_t touchIds[2] = {0, 0}; + + void Normalize(Quat& q) { + const float n = std::sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); + if (n < 1e-6f) { q = Quat{}; return; } + q.w /= n; q.x /= n; q.y /= n; q.z /= n; + } + + void Fuse(float dt, const std::array& accel, const std::array& gyro) { + float gx = gyro[0], gy = gyro[1], gz = gyro[2]; + const float an = std::sqrt(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); + if (an > 1.0f) { + const float ax = accel[0] / an, ay = accel[1] / an, az = accel[2] / an; + // Gravity direction predicted by the current orientation, in the body frame (rest pose: +Y). + const float vx = 2.0f * (orientation.x * orientation.y + orientation.w * orientation.z); + const float vy = orientation.w * orientation.w - orientation.x * orientation.x + orientation.y * orientation.y - orientation.z * orientation.z; + const float vz = 2.0f * (orientation.y * orientation.z - orientation.w * orientation.x); + const float ex = ay * vz - az * vy; + const float ey = az * vx - ax * vz; + const float ez = ax * vy - ay * vx; + constexpr float kp = 1.5f; + constexpr float ki = 0.02f; + biasIntegral[0] += ki * ex * dt; + biasIntegral[1] += ki * ey * dt; + biasIntegral[2] += ki * ez * dt; + gx += kp * ex + biasIntegral[0]; + gy += kp * ey + biasIntegral[1]; + gz += kp * ez + biasIntegral[2]; + } + const float hx = 0.5f * gx * dt, hy = 0.5f * gy * dt, hz = 0.5f * gz * dt; + const Quat q = orientation; + orientation.w = q.w - q.x * hx - q.y * hy - q.z * hz; + orientation.x = q.x + q.w * hx + q.y * hz - q.z * hy; + orientation.y = q.y + q.w * hy - q.x * hz + q.z * hx; + orientation.z = q.z + q.w * hz + q.x * hy - q.y * hx; + Normalize(orientation); + } + + // DualSense native effect block: byte 0 = mode, [1..2] = active zone mask, [3..6] = 3-bit strength per zone, + // [9] = frequency (vibration modes). Mode 0x05 means "no effect". + void EncodeZones(std::uint8_t* dst, std::uint8_t mode, const std::uint8_t* strengths, int count, std::uint8_t frequency) { + std::uint16_t zones = 0; + std::uint32_t packed = 0; + for (int i = 0; i < count && i < 10; ++i) { + const std::uint8_t s = strengths[i]; + if (s == 0 || s > 8) continue; + zones |= static_cast(1u << i); + packed |= static_cast(s - 1) << (i * 3); + } + if (zones == 0 || (mode == 0x26 && frequency == 0)) { dst[0] = 0x05; return; } + dst[0] = mode; + dst[1] = static_cast(zones & 0xFF); + dst[2] = static_cast(zones >> 8); + std::memcpy(dst + 3, &packed, 4); + dst[9] = frequency; + } + + std::uint8_t MaxOf(const std::uint8_t* v, int n) { + std::uint8_t m = 0; + for (int i = 0; i < n; ++i) m = std::max(m, v[i]); + return m; + } + + std::uint8_t ScaleStrength(std::uint8_t s) { + return s == 0 ? 0 : static_cast(std::min(255, 32 * std::min(s, 8))); + } } void Pad::Initialize() { std::lock_guard lock(stateMutex); if (failure) std::rethrow_exception(failure); - if (!initialized) timestamp = sceKernelGetProcessTime(); + if (!initialized) { + timestamp = sceKernelGetProcessTime(); + lastFuseTime = timestamp; + } initialized = true; } @@ -22,34 +105,185 @@ PadData Pad::ReadState() { std::lock_guard lock(stateMutex); if (failure) std::rethrow_exception(failure); if (!initialized) throw std::runtime_error("Pad: read before initialization"); + const std::uint64_t now = sceKernelGetProcessTime(); PadData data{}; data.buttons = state.buttons; data.left_stick_x = state.sticks[0]; data.left_stick_y = state.sticks[1]; data.right_stick_x = state.sticks[2]; data.right_stick_y = state.sticks[3]; - data.analog_buttons_l2 = state.analogButtonsL2; - data.analog_buttons_r2 = state.analogButtonsR2; - data.acceleration_y = 1.0f; - data.orientation_w = 1.0f; + data.analog_buttons_l2 = std::max(state.analogButtonsL2, (state.buttons & 0x100) != 0 ? 255 : 0); + data.analog_buttons_r2 = std::max(state.analogButtonsR2, (state.buttons & 0x200) != 0 ? 255 : 0); + + // Motion: real sensor data when the host pad has it and the guest enabled the sensors, otherwise the pad at rest + // (gravity on +Y, no rotation, identity orientation). + const bool live = state.hasMotion && output.motionEnabled; + const std::array rest{0.0f, 9.80665f, 0.0f}; + const std::array& accel = live ? state.accel : rest; + const std::array gyro = live ? state.gyro : std::array{0.0f, 0.0f, 0.0f}; + if (live) { + float dt = lastFuseTime != 0 && now > lastFuseTime ? static_cast(now - lastFuseTime) * 1e-6f : 0.0f; + dt = std::min(dt, 0.1f); + Fuse(dt, accel, gyro); + } else { + orientation = Quat{}; + biasIntegral[0] = biasIntegral[1] = biasIntegral[2] = 0.0f; + } + lastFuseTime = now; + data.orientation_x = orientation.x; + data.orientation_y = orientation.y; + data.orientation_z = orientation.z; + data.orientation_w = orientation.w; + // ScePadData reports acceleration in units of g (rest = 1.0 on the "up" axis), not m/s^2; the fusion + // math above stays in m/s^2 because Fuse()'s free-fall gate assumes that scale. + constexpr float kGravity = 9.80665f; + data.acceleration_x = accel[0] / kGravity; + data.acceleration_y = accel[1] / kGravity; + data.acceleration_z = accel[2] / kGravity; + data.angular_velocity_x = gyro[0]; + data.angular_velocity_y = gyro[1]; + data.angular_velocity_z = gyro[2]; + data.connected = true; data.connected_count = 1; data.timestamp = timestamp; - if (state.touchLeft || state.touchRight) { + + // Touch: host touchpad fingers, plus the keyboard emulation (BACKSPACE / TAB) when no finger is down. + std::uint8_t touchNum = 0; + for (int i = 0; i < 2; ++i) { + const PadTouchPoint& tp = state.touch[i]; + if (!tp.active) { prevTouchActive[i] = false; continue; } + if (!prevTouchActive[i]) touchIds[i] = static_cast(nextTouchId++ & 0x7F); + prevTouchActive[i] = true; + if (touchNum == 0) { + data.touch_data_touch0_x = tp.x; data.touch_data_touch0_y = tp.y; data.touch_data_touch0_id = touchIds[i]; + } else { + data.touch_data_touch1_x = tp.x; data.touch_data_touch1_y = tp.y; data.touch_data_touch1_id = touchIds[i]; + } + ++touchNum; + } + if (touchNum == 0 && (state.touchLeft || state.touchRight)) { data.buttons |= 0x100000; - data.touch_data_touch_num = 1; + touchNum = 1; data.touch_data_touch0_x = state.touchRight ? 1440 : 480; data.touch_data_touch0_y = 471; + data.touch_data_touch0_id = 0; } + data.touch_data_touch_num = touchNum; return data; } +void Pad::SetVibration(std::uint8_t large, std::uint8_t small) { + std::lock_guard lock(stateMutex); + if (output.vibrationLarge == large && output.vibrationSmall == small) return; + output.vibrationLarge = large; + output.vibrationSmall = small; + ++output.sequence; +} + +void Pad::SetVibrationMode(int mode) { + std::lock_guard lock(stateMutex); + if (output.vibrationMode == mode) return; + output.vibrationMode = mode; + ++output.sequence; +} + +void Pad::SetLightBar(bool valid, std::uint8_t r, std::uint8_t g, std::uint8_t b) { + std::lock_guard lock(stateMutex); + output.lightBarValid = valid; + output.lightBar[0] = r; + output.lightBar[1] = g; + output.lightBar[2] = b; + ++output.sequence; +} + +void Pad::SetTriggerCommand(int trigger, const std::uint8_t* command) { + if (trigger < 0 || trigger > 1 || command == nullptr) return; + std::uint32_t mode = 0; + std::memcpy(&mode, command, 4); + const std::uint8_t* p = command + 8; + PadTriggerRequest req; + req.mode = mode; + req.effect[0] = 0x05; + switch (mode) { + case 1: { // feedback { position, strength } + std::uint8_t strengths[10] = {}; + if (p[0] <= 9 && p[1] >= 1 && p[1] <= 8) for (int i = p[0]; i < 10; ++i) strengths[i] = p[1]; + EncodeZones(req.effect, 0x21, strengths, 10, 0); + req.fallback = ScaleStrength(p[1]); + break; + } + case 2: { // weapon { startPosition, endPosition, strength } + if (p[0] >= 2 && p[0] <= 7 && p[1] > p[0] && p[1] <= 8 && p[2] >= 1 && p[2] <= 8) { + const std::uint16_t zones = static_cast((1u << p[0]) | (1u << p[1])); + req.effect[0] = 0x25; + req.effect[1] = static_cast(zones & 0xFF); + req.effect[2] = static_cast(zones >> 8); + req.effect[3] = static_cast(p[2] - 1); + } + req.fallback = ScaleStrength(p[2]); + break; + } + case 3: { // vibration { position, amplitude, frequency } + std::uint8_t strengths[10] = {}; + if (p[0] <= 9 && p[1] >= 1 && p[1] <= 8) for (int i = p[0]; i < 10; ++i) strengths[i] = p[1]; + EncodeZones(req.effect, 0x26, strengths, 10, p[2]); + req.fallback = ScaleStrength(p[1]); + break; + } + case 4: // multi-position feedback { strength[10] } + EncodeZones(req.effect, 0x21, p, 10, 0); + req.fallback = ScaleStrength(MaxOf(p, 10)); + break; + case 5: { // slope feedback { startPosition, endPosition, startStrength, endStrength } + std::uint8_t strengths[10] = {}; + if (p[0] <= 8 && p[1] > p[0] && p[1] <= 9 && p[2] >= 1 && p[2] <= 8 && p[3] >= 1 && p[3] <= 8) { + const int dist = p[1] - p[0]; + for (int i = p[0]; i < 10; ++i) + strengths[i] = i <= p[1] ? static_cast(std::lround(p[2] + (p[3] - p[2]) * (i - p[0]) / static_cast(dist))) : p[3]; + } + EncodeZones(req.effect, 0x21, strengths, 10, 0); + req.fallback = ScaleStrength(std::max(p[2], p[3])); + break; + } + case 6: // multi-position vibration { frequency, amplitude[10] } + EncodeZones(req.effect, 0x26, p + 1, 10, p[0]); + req.fallback = p[0] == 0 ? 0 : ScaleStrength(MaxOf(p + 1, 10)); + break; + default: break; // 0 = off + } + req.valid = req.effect[0] != 0x05; + if (!req.valid) req.fallback = 0; + std::lock_guard lock(stateMutex); + output.trigger[trigger] = req; + output.triggerTouched = true; + ++output.sequence; +} + +void Pad::ResetOrientation() { + std::lock_guard lock(stateMutex); + orientation = Quat{}; + biasIntegral[0] = biasIntegral[1] = biasIntegral[2] = 0.0f; +} + +void Pad::SetMotionEnabled(bool enabled) { + std::lock_guard lock(stateMutex); + if (output.motionEnabled == enabled) return; + output.motionEnabled = enabled; + ++output.sequence; +} + extern "C" void PadPublishInput_nid_postfix(const PadInputState& input) { std::lock_guard lock(stateMutex); if (failure) std::rethrow_exception(failure); + // Change-detection kept from upstream, extended to the motion/touch fields: a pad with live sensors + // changes on essentially every call (so this is a no-op there), while a pad without one still only + // bumps the reported timestamp when something actually changed. if (state.buttons == input.buttons && state.sticks == input.sticks && state.analogButtonsL2 == input.analogButtonsL2 && state.analogButtonsR2 == input.analogButtonsR2 && - state.touchLeft == input.touchLeft && state.touchRight == input.touchRight) return; + state.touchLeft == input.touchLeft && state.touchRight == input.touchRight && + state.hasMotion == input.hasMotion && state.accel == input.accel && state.gyro == input.gyro && + state.touch == input.touch && state.deviceKind == input.deviceKind) return; state = input; timestamp = sceKernelGetProcessTime(); } @@ -59,3 +293,11 @@ extern "C" void PadReportInputFailure_nid_postfix(std::exception_ptr error) { std::lock_guard lock(stateMutex); if (!failure) failure = error; } + +extern "C" bool PadFetchOutput_nid_postfix(std::uint32_t* seenSequence, PadOutputState* out) { + std::lock_guard lock(stateMutex); + if (seenSequence == nullptr || out == nullptr || *seenSequence == output.sequence) return false; + *seenSequence = output.sequence; + *out = output; + return true; +}