Skip to content

Commit

Permalink
fix: PS5 gyro and acceleration
Browse files Browse the repository at this point in the history
  • Loading branch information
@ABeltramo
ABeltramo committed May 31, 2024
1 parent 597bba7 commit 053f906
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 87 deletions.
72 changes: 1 addition & 71 deletions src/uhid/include/uhid/ps5.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,77 +208,7 @@ static constexpr unsigned char ps5_calibration_info[] = {
0xD8, 0x0B, 0x00, 0x00, 0x00, 0x00, 0x00,
};

static constexpr int gyro_calib_bias = 0;

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L997
* speed_2x = (gyro_speed_plus + gyro_speed_minus);
* sens_numer = speed_2x * PS5_GYRO_RES_PER_DEG_S
*/
static constexpr int gyro_calib_sens_numer = (ps5_calibration_info[19] + ps5_calibration_info[21]) *
PS5_GYRO_RES_PER_DEG_S;

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L998-L999
* sens_denom = abs(gyro_pitch_plus - gyro_pitch_bias) + abs(gyro_pitch_minus - gyro_pitch_bias);
*/
static constexpr int gyro_calib_pitch_denom = ps5_calibration_info[7] - ps5_calibration_info[1] +
ps5_calibration_info[9] - ps5_calibration_info[1];

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L1004-L1005
* sens_denom = abs(gyro_yaw_plus - gyro_yaw_bias) + abs(gyro_yaw_minus - gyro_yaw_bias);
*/
static constexpr int gyro_calib_yaw_denom = ps5_calibration_info[11] - ps5_calibration_info[3] +
ps5_calibration_info[13] - ps5_calibration_info[3];

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L1010-L1011
* sens_denom = abs(gyro_roll_plus - gyro_roll_bias) + abs(gyro_roll_minus - gyro_roll_bias);
*/
static constexpr int gyro_calib_roll_denom = ps5_calibration_info[15] - ps5_calibration_info[5] +
ps5_calibration_info[17] - ps5_calibration_info[5];

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L1036
* range_2g = acc_x_plus - acc_x_minus;
* sens_denom = range_2g;
*/
static constexpr int acc_calib_denom = ps5_calibration_info[23] - ps5_calibration_info[25];

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L1034
* range_2g = acc_x_plus - acc_x_minus;
* bias = acc_x_plus - range_2g / 2
*/
static constexpr int acc_calib_x_bias = ps5_calibration_info[23] - acc_calib_denom / 2;

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L1035
* sens_numer = 2*DS_ACC_RES_PER_G;
*/
static constexpr int acc_calib_sens_numer = 2 * PS5_ACC_RES_PER_G;

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L1040
* bias = acc_y_plus - range_2g / 2;
*/
static constexpr int acc_calib_y_bias = ps5_calibration_info[27] - acc_calib_denom / 2;

/**
* see:
* https://github.com/torvalds/linux/blob/fa4b851b4ad632dc673627f38a8a552547568a2c/drivers/hid/hid-playstation.c#L1046
* bias = acc_z_plus - range_2g / 2;
*/
static constexpr int acc_calib_z_bias = ps5_calibration_info[31] - acc_calib_denom / 2;
static constexpr int gyro_resolution = 1145;

static constexpr unsigned char ps5_firmware_info[] = {
0x20, 0x4A, 0x75, 0x6E, 0x20, 0x31, 0x39, 0x20, 0x32, 0x30, 0x32, 0x33, 0x31, 0x34, 0x3A, 0x34,
Expand Down
25 changes: 14 additions & 11 deletions src/uhid/joypad_ps5.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -337,26 +337,29 @@ void PS5Joypad::set_on_rumble(const std::function<void(int, int)> &callback) {
this->_state->on_rumble = callback;
}

static inline float rad2deg(float rad) {
return rad * (180.0f / (float)M_PI);
static __le16 to_le_signed(float original, float value) {
auto le = htole16(value);
if (original < 0) { // adjust sign bit
le |= (1 << 15); // set the last bit (bit 15) to 1
}
return le;
}

void PS5Joypad::set_motion(PS5Joypad::MOTION_TYPE type, float x, float y, float z) {
switch (type) {
case ACCELERATION: {
this->_state->current_state.accel[0] = htole16((x * uhid::SDL_STANDARD_GRAVITY * 100));
this->_state->current_state.accel[1] = htole16((y * uhid::SDL_STANDARD_GRAVITY * 100));
this->_state->current_state.accel[2] = htole16((z * uhid::SDL_STANDARD_GRAVITY * 100));
this->_state->current_state.accel[0] = to_le_signed(x, (x * uhid::SDL_STANDARD_GRAVITY * 100));
this->_state->current_state.accel[1] = to_le_signed(y, (y * uhid::SDL_STANDARD_GRAVITY * 100));
this->_state->current_state.accel[2] = to_le_signed(z, (z * uhid::SDL_STANDARD_GRAVITY * 100));

send_report(*this->_state);
break;
}
case GYROSCOPE: {
this->_state->current_state.gyro[0] =
htole16(rad2deg((x + uhid::gyro_calib_bias) / uhid::gyro_calib_pitch_denom) * uhid::PS5_GYRO_RES_PER_DEG_S * 5);
this->_state->current_state.gyro[1] =
htole16(rad2deg((y + uhid::gyro_calib_bias) / uhid::gyro_calib_yaw_denom) * uhid::PS5_GYRO_RES_PER_DEG_S * 5);
this->_state->current_state.gyro[2] =
htole16(rad2deg((z + uhid::gyro_calib_bias) / uhid::gyro_calib_roll_denom) * uhid::PS5_GYRO_RES_PER_DEG_S * 5);
this->_state->current_state.gyro[0] = to_le_signed(x, x * uhid::gyro_resolution);
this->_state->current_state.gyro[1] = to_le_signed(y, y * uhid::gyro_resolution);
this->_state->current_state.gyro[2] = to_le_signed(z, z * uhid::gyro_resolution);

send_report(*this->_state);
break;
}
Expand Down
54 changes: 49 additions & 5 deletions tests/testJoypads.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,17 +210,38 @@ TEST_CASE_METHOD(SDLTestsFixture, "PS Joypad", "[SDL]") {
REQUIRE_THAT(event.csensor.data[1], WithinAbs(acceleration_data[1], 0.9f));
REQUIRE_THAT(event.csensor.data[2], WithinAbs(acceleration_data[2], 0.9f));
flush_sdl_events();

// Now lets test the negatives
acceleration_data = {-9.8f, -0.0f, -20.0f};
joypad.set_motion(inputtino::PS5Joypad::ACCELERATION,
acceleration_data[0],
acceleration_data[1],
acceleration_data[2]);
SDL_GameControllerUpdate();
SDL_SensorUpdate();
while (SDL_PollEvent(&event) != 0) {
if (event.type == SDL_CONTROLLERSENSORUPDATE) {
break;
}
}
REQUIRE(event.type == SDL_CONTROLLERSENSORUPDATE);
REQUIRE(event.csensor.sensor == SDL_SENSOR_ACCEL);
REQUIRE_THAT(event.csensor.data[0], WithinAbs(acceleration_data[0], 0.9f));
REQUIRE_THAT(event.csensor.data[1], WithinAbs(acceleration_data[1], 0.9f));
REQUIRE_THAT(event.csensor.data[2], WithinAbs(acceleration_data[2], 0.9f));
flush_sdl_events();
}
{ // test gyro
REQUIRE(SDL_GameControllerHasSensor(gc, SDL_SENSOR_GYRO));
if (SDL_GameControllerSetSensorEnabled(gc, SDL_SENSOR_GYRO, SDL_TRUE) != 0) {
WARN(SDL_GetError());
}

std::array<float, 3> gyro_data = {0.0f, 10.0f, 20.0f};
std::array<float, 3> gyro_data = {0.0f,
M_PI_2f, // half a turn (180 degrees)
M_PIf}; // full turn (360 degrees)
joypad.set_motion(inputtino::PS5Joypad::GYROSCOPE, gyro_data[0], gyro_data[1], gyro_data[2]);
std::this_thread::sleep_for(10ms);
joypad.set_motion(inputtino::PS5Joypad::GYROSCOPE, gyro_data[0], gyro_data[1], gyro_data[2]);

SDL_GameControllerUpdate();
SDL_SensorUpdate();
Expand All @@ -232,9 +253,32 @@ TEST_CASE_METHOD(SDLTestsFixture, "PS Joypad", "[SDL]") {
}
REQUIRE(event.type == SDL_CONTROLLERSENSORUPDATE);
REQUIRE(event.csensor.sensor == SDL_SENSOR_GYRO);
REQUIRE_THAT(event.csensor.data[0], WithinAbs(gyro_data[0], 0.001f));
REQUIRE_THAT(event.csensor.data[1], WithinAbs(gyro_data[1], 0.001f));
REQUIRE_THAT(event.csensor.data[2], WithinAbs(gyro_data[2], 0.001f));
REQUIRE_THAT(event.csensor.data[0], WithinAbs(gyro_data[0], 0.01f));
REQUIRE_THAT(event.csensor.data[1], WithinAbs(gyro_data[1], 0.01f));
REQUIRE_THAT(event.csensor.data[2], WithinAbs(gyro_data[2], 0.01f));
flush_sdl_events();

// Now let's try the negatives
gyro_data = {
-0.0f,
-M_PI_2f, // half a turn (180 degrees)
-M_PIf // full turn (360 degrees)
};
joypad.set_motion(inputtino::PS5Joypad::GYROSCOPE, gyro_data[0], gyro_data[1], gyro_data[2]);
std::this_thread::sleep_for(10ms);

SDL_GameControllerUpdate();
SDL_SensorUpdate();
while (SDL_PollEvent(&event) != 0) {
if (event.type == SDL_CONTROLLERSENSORUPDATE) {
break;
}
}
REQUIRE(event.type == SDL_CONTROLLERSENSORUPDATE);
REQUIRE(event.csensor.sensor == SDL_SENSOR_GYRO);
REQUIRE_THAT(event.csensor.data[0], WithinAbs(gyro_data[0], 0.01f));
REQUIRE_THAT(event.csensor.data[1], WithinAbs(gyro_data[1], 0.01f));
REQUIRE_THAT(event.csensor.data[2], WithinAbs(gyro_data[2], 0.01f));
}

{ // Test touchpad
Expand Down

0 comments on commit 053f906

Please sign in to comment.