mirror of
https://github.com/KytyPS5/KytyPS5.git
synced 2026-10-29 08:41:20 +00:00
Compare commits
2
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
44798085bd | ||
|
|
b7a1fac898 |
No files matched your search
+60
-13
@@ -133,6 +133,9 @@ private:
|
||||
int m_connected_count = 0;
|
||||
bool m_motion_enabled = true;
|
||||
uint64_t m_gyro_time = 0;
|
||||
// Accelerometer gravity direction in the reset frame.
|
||||
std::array<float, 3> m_up_reference {0.0f, 1.0f, 0.0f};
|
||||
bool m_up_reference_valid = false;
|
||||
ControllerState m_state;
|
||||
ControllerState m_states[STATES_MAX];
|
||||
bool m_obtained[STATES_MAX] {};
|
||||
@@ -348,6 +351,7 @@ void GameController::CheckActive() {
|
||||
m_connected = new_connected;
|
||||
m_state = {};
|
||||
m_gyro_time = 0;
|
||||
m_up_reference_valid = false;
|
||||
m_states_num = 0;
|
||||
m_first_state = 0;
|
||||
m_next_touch_id = 1;
|
||||
@@ -439,6 +443,29 @@ void GameController::TouchPad(int id, int finger, bool down, float x, float y) {
|
||||
}
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
using Vec3 = std::array<float, 3>;
|
||||
using Quat = std::array<float, 4>; // x, y, z, w
|
||||
|
||||
Vec3 QuatRotate(const Quat& q, const Vec3& v) {
|
||||
const Vec3 t {2.0f * (q[1] * v[2] - q[2] * v[1]), 2.0f * (q[2] * v[0] - q[0] * v[2]),
|
||||
2.0f * (q[0] * v[1] - q[1] * v[0])};
|
||||
return {v[0] + q[3] * t[0] + (q[1] * t[2] - q[2] * t[1]),
|
||||
v[1] + q[3] * t[1] + (q[2] * t[0] - q[0] * t[2]),
|
||||
v[2] + q[3] * t[2] + (q[0] * t[1] - q[1] * t[0])};
|
||||
}
|
||||
|
||||
Quat QuatConjugate(const Quat& q) {
|
||||
return {-q[0], -q[1], -q[2], q[3]};
|
||||
}
|
||||
|
||||
float Length(const Vec3& v) {
|
||||
return std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void GameController::Motion(int id, Sensor sensor, const float* data, uint64_t time_us) {
|
||||
Common::LockGuard lock(m_mutex);
|
||||
if (id != m_active_id || !m_motion_enabled) {
|
||||
@@ -450,20 +477,38 @@ void GameController::Motion(int id, Sensor sensor, const float* data, uint64_t t
|
||||
for (int i = 0; i < 3; i++) {
|
||||
m_state.accel[i] = data[i] / SDL_STANDARD_GRAVITY;
|
||||
}
|
||||
// Capture gravity near 1 G to limit interference from linear acceleration.
|
||||
const float magnitude = Length(m_state.accel);
|
||||
if (!m_up_reference_valid && magnitude > 0.9f && magnitude < 1.1f) {
|
||||
const Vec3 up {m_state.accel[0] / magnitude, m_state.accel[1] / magnitude,
|
||||
m_state.accel[2] / magnitude};
|
||||
m_up_reference = QuatRotate(m_state.orientation, up);
|
||||
m_up_reference_valid = true;
|
||||
}
|
||||
} else {
|
||||
std::copy_n(data, 3, m_state.gyro.begin());
|
||||
// Mahony tilt correction; heading is unobservable and reported gyro values stay raw.
|
||||
auto rate = m_state.gyro;
|
||||
const float magnitude = Length(m_state.accel);
|
||||
if (m_up_reference_valid && magnitude > 0.8f && magnitude < 1.2f) {
|
||||
const Vec3 measured {m_state.accel[0] / magnitude, m_state.accel[1] / magnitude,
|
||||
m_state.accel[2] / magnitude};
|
||||
const Vec3 predicted = QuatRotate(QuatConjugate(m_state.orientation), m_up_reference);
|
||||
rate[0] += measured[1] * predicted[2] - measured[2] * predicted[1];
|
||||
rate[1] += measured[2] * predicted[0] - measured[0] * predicted[2];
|
||||
rate[2] += measured[0] * predicted[1] - measured[1] * predicted[0];
|
||||
}
|
||||
// Do not extrapolate a single sample across lost reports (e.g. loss of window focus).
|
||||
constexpr uint64_t max_gyro_interval_us = 100000;
|
||||
if (m_gyro_time != 0 && time_us > m_gyro_time &&
|
||||
time_us - m_gyro_time <= max_gyro_interval_us) {
|
||||
const float dt = static_cast<float>(time_us - m_gyro_time) * 0.000001f;
|
||||
const float speed =
|
||||
std::sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]);
|
||||
const float dt = static_cast<float>(time_us - m_gyro_time) * 0.000001f;
|
||||
const float speed = Length(rate);
|
||||
const float half_angle = speed * dt * 0.5f;
|
||||
const float scale = speed > 0.0f ? std::sin(half_angle) / speed : 0.0f;
|
||||
const float x = data[0] * scale;
|
||||
const float y = data[1] * scale;
|
||||
const float z = data[2] * scale;
|
||||
const float x = rate[0] * scale;
|
||||
const float y = rate[1] * scale;
|
||||
const float z = rate[2] * scale;
|
||||
const float w = std::cos(half_angle);
|
||||
const auto q = m_state.orientation;
|
||||
// Accumulate body-local rotation relative to connection / orientation reset.
|
||||
@@ -489,19 +534,21 @@ void GameController::Motion(int id, Sensor sensor, const float* data, uint64_t t
|
||||
void GameController::SetMotionSensorState(bool enable) {
|
||||
Common::LockGuard lock(m_mutex);
|
||||
if (m_motion_enabled != enable) {
|
||||
m_motion_enabled = enable;
|
||||
m_gyro_time = 0;
|
||||
m_state.accel = {0.0f, 1.0f, 0.0f};
|
||||
m_state.gyro = {};
|
||||
m_state.time = LibKernel::KernelGetProcessTime();
|
||||
m_motion_enabled = enable;
|
||||
m_gyro_time = 0;
|
||||
m_state.accel = {0.0f, 1.0f, 0.0f};
|
||||
m_state.gyro = {};
|
||||
m_up_reference_valid = false;
|
||||
m_state.time = LibKernel::KernelGetProcessTime();
|
||||
AddState();
|
||||
}
|
||||
}
|
||||
|
||||
void GameController::ResetOrientation() {
|
||||
Common::LockGuard lock(m_mutex);
|
||||
m_state.orientation = {0.0f, 0.0f, 0.0f, 1.0f};
|
||||
m_gyro_time = 0;
|
||||
m_state.orientation = {0.0f, 0.0f, 0.0f, 1.0f};
|
||||
m_gyro_time = 0;
|
||||
m_up_reference_valid = false;
|
||||
m_state.time = LibKernel::KernelGetProcessTime();
|
||||
AddState();
|
||||
}
|
||||
|
||||
+29
-7
@@ -997,14 +997,31 @@ static int ConvertHostSocketError(int error) {
|
||||
case EAGAIN: posix_error = Posix::POSIX_EWOULDBLOCK; break;
|
||||
case EBADF: posix_error = Posix::POSIX_EBADF; break;
|
||||
case EFAULT: posix_error = Posix::POSIX_EFAULT; break;
|
||||
case EINTR: posix_error = Posix::POSIX_EINTR; break;
|
||||
case EINVAL: posix_error = Posix::POSIX_EINVAL; break;
|
||||
case EISCONN: posix_error = Posix::POSIX_EISCONN; break;
|
||||
case EMFILE: posix_error = Posix::POSIX_EMFILE; break;
|
||||
case EMSGSIZE: posix_error = Posix::POSIX_EMSGSIZE; break;
|
||||
case ENFILE: posix_error = Posix::POSIX_ENFILE; break;
|
||||
case ENOBUFS: posix_error = Posix::POSIX_ENOBUFS; break;
|
||||
case ENOMEM: posix_error = Posix::POSIX_ENOMEM; break;
|
||||
case ENETDOWN: posix_error = Posix::POSIX_ENETDOWN; break;
|
||||
case ENETRESET: posix_error = Posix::POSIX_ENETRESET; break;
|
||||
case ENETUNREACH: posix_error = Posix::POSIX_ENETUNREACH; break;
|
||||
case ENOTCONN: posix_error = Posix::POSIX_ENOTCONN; break;
|
||||
case ENOTSOCK: posix_error = Posix::POSIX_ENOTSOCK; break;
|
||||
case EOPNOTSUPP: posix_error = Posix::POSIX_EOPNOTSUPP; break;
|
||||
case EPIPE: posix_error = Posix::POSIX_EPIPE; break;
|
||||
case EPROTONOSUPPORT: posix_error = Posix::POSIX_EPROTONOSUPPORT; break;
|
||||
case ESHUTDOWN: posix_error = Posix::POSIX_ESHUTDOWN; break;
|
||||
case ETIMEDOUT: posix_error = Posix::POSIX_ETIMEDOUT; break;
|
||||
case ECONNABORTED: posix_error = Posix::POSIX_ECONNABORTED; break;
|
||||
case ECONNREFUSED: posix_error = Posix::POSIX_ECONNREFUSED; break;
|
||||
case ECONNRESET: posix_error = Posix::POSIX_ECONNRESET; break;
|
||||
case EDESTADDRREQ: posix_error = Posix::POSIX_EDESTADDRREQ; break;
|
||||
case EHOSTUNREACH: posix_error = Posix::POSIX_EHOSTUNREACH; break;
|
||||
case EINPROGRESS: posix_error = Posix::POSIX_EINPROGRESS; break;
|
||||
case EALREADY: posix_error = Posix::POSIX_EALREADY; break;
|
||||
default: break;
|
||||
}
|
||||
#endif
|
||||
@@ -2098,17 +2115,22 @@ int KYTY_SYSV_ABI Setsockopt(int s, int level, int optname, const void* optval,
|
||||
return 0;
|
||||
}
|
||||
|
||||
#if defined(_WIN32)
|
||||
constexpr int ORBIS_SO_NBIO = 0x1200;
|
||||
if (ConvertSocketOptionLevel(level) == SOL_SOCKET && optname == ORBIS_SO_NBIO &&
|
||||
optlen >= sizeof(int)) {
|
||||
u_long enabled = (*static_cast<const int*>(optval) != 0 ? 1 : 0);
|
||||
if (ioctlsocket(socket, FIONBIO, &enabled) == SOCKET_ERROR) {
|
||||
return SetHostSocketError();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
const bool enabled = *static_cast<const int*>(optval) != 0;
|
||||
#if defined(_WIN32)
|
||||
u_long mode = enabled ? 1 : 0;
|
||||
const bool failed = ioctlsocket(socket, FIONBIO, &mode) == SOCKET_ERROR;
|
||||
#else
|
||||
const int flags = ::fcntl(socket, F_GETFL, 0);
|
||||
const bool failed =
|
||||
flags < 0 ||
|
||||
::fcntl(socket, F_SETFL, enabled ? (flags | O_NONBLOCK) : (flags & ~O_NONBLOCK)) != 0;
|
||||
#endif
|
||||
return failed ? SetHostSocketError() : 0;
|
||||
}
|
||||
#if !defined(_WIN32)
|
||||
// Guest TCP options: IPPROTO_TCP=6, TCP_NODELAY=1.
|
||||
if (level != 6 || optname != 1) {
|
||||
return SetGuestSocketError(Posix::POSIX_ENOPROTOOPT);
|
||||
|
||||
Reference in new issue
Block a user