From ecdb4f7c1e2fdb3ee9791f2e2a4ae00a6ec25efb Mon Sep 17 00:00:00 2001 From: JibbSmart Date: Tue, 17 Nov 2020 13:27:39 +0800 Subject: [PATCH] Quick and dirty DualSense support, USB only for now --- JoyShockLibrary/InputHelpers.cpp | 109 +++++++++++++++++++++++++++- JoyShockLibrary/JoyShockLibrary.cpp | 5 +- 2 files changed, 108 insertions(+), 6 deletions(-) diff --git a/JoyShockLibrary/InputHelpers.cpp b/JoyShockLibrary/InputHelpers.cpp index 21d6b96..6198685 100644 --- a/JoyShockLibrary/InputHelpers.cpp +++ b/JoyShockLibrary/InputHelpers.cpp @@ -141,9 +141,112 @@ bool handle_input(JoyShock *jc, uint8_t *packet, int len, bool &hasIMU) { } if (jc->controller_type == ControllerType::s_ds) { - printf("%d: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n", - packet[0], packet[1], packet[2], packet[3], packet[4], packet[5], packet[6], packet[7], packet[8], packet[9], - packet[10], packet[11], packet[12], packet[13], packet[14], packet[15], packet[16], packet[17], packet[18], packet[19], packet[20]); + //printf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n", + // packet[0], packet[1], packet[2], packet[3], packet[4], packet[5], packet[6], packet[7], packet[8], packet[9], + // packet[10], packet[11], packet[12], packet[13], packet[14], packet[15], packet[16], packet[17], packet[18], packet[19], packet[20], + // packet[21], packet[22], packet[23], packet[24], packet[25], packet[26], packet[27], packet[28], packet[29], packet[30], + // packet[31], packet[32], packet[33], packet[34], packet[35], packet[36], packet[37], packet[38], packet[39], packet[40], + // packet[41], packet[42], packet[43], packet[44], packet[45], packet[46], packet[47], packet[48], packet[49], packet[50]); + int indexOffset = 1; + + // Gyroscope: + // Gyroscope data is relative (degrees/s) + int16_t gyroSampleX = uint16_to_int16(packet[indexOffset + 15] | (packet[indexOffset + 16] << 8) & 0xFF00); + int16_t gyroSampleY = uint16_to_int16(packet[indexOffset + 17] | (packet[indexOffset + 18] << 8) & 0xFF00); + int16_t gyroSampleZ = uint16_to_int16(packet[indexOffset + 19] | (packet[indexOffset + 20] << 8) & 0xFF00); + int16_t accelSampleX = uint16_to_int16(packet[indexOffset + 21] | (packet[indexOffset + 22] << 8) & 0xFF00); + int16_t accelSampleY = uint16_to_int16(packet[indexOffset + 23] | (packet[indexOffset + 24] << 8) & 0xFF00); + int16_t accelSampleZ = uint16_to_int16(packet[indexOffset + 25] | (packet[indexOffset + 26] << 8) & 0xFF00); + + if ((gyroSampleX | gyroSampleY | gyroSampleZ | accelSampleX | accelSampleY | accelSampleZ) == 0) + { + // all zero? + hasIMU = false; + } + + // convert to real units + jc->imu_state.gyroX = (float)(gyroSampleX) * (2000.0 / 32767.0); + jc->imu_state.gyroY = (float)(gyroSampleY) * (2000.0 / 32767.0); + jc->imu_state.gyroZ = (float)(gyroSampleZ) * (2000.0 / 32767.0); + + jc->imu_state.accelX = (float)(accelSampleX) / 8192.0; + jc->imu_state.accelY = (float)(accelSampleY) / 8192.0; + jc->imu_state.accelZ = (float)(accelSampleZ) / 8192.0; + + //printf("DS accel: %.4f, %.4f, %.4f\n", jc->imu_state.accelX, jc->imu_state.accelY, jc->imu_state.accelZ); + + //printf("%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d\n", + // jc->gyro.yaw, jc->gyro.pitch, jc->gyro.roll, jc->accel.x, jc->accel.y, jc->accel.z, universal_counter++); + + // Touchpad: + jc->last_touch_state = jc->touch_state; + + jc->touch_state.t0Id = (int)(packet[indexOffset + 32] & 0x7F); + jc->touch_state.t1Id = (int)(packet[indexOffset + 36] & 0x7F); + jc->touch_state.t0Down = (packet[indexOffset + 32] & 0x80) == 0; + jc->touch_state.t1Down = (packet[indexOffset + 36] & 0x80) == 0; + + jc->touch_state.t0X = (packet[indexOffset + 33] | (packet[indexOffset + 34] & 0x0F) << 8) / 1920.0f; + jc->touch_state.t0Y = ((packet[indexOffset + 34] & 0xF0) >> 4 | packet[indexOffset + 35] << 4) / 943.0f; + jc->touch_state.t1X = (packet[indexOffset + 37] | (packet[indexOffset + 38] & 0x0F) << 8) / 1920.0f; + jc->touch_state.t1Y = ((packet[indexOffset + 38] & 0xF0) >> 4 | packet[indexOffset + 39] << 4) / 943.0f; + + //printf("DS touch: %d, %d, %d, %d, %.4f, %.4f, %.4f, %.4f\n", + // jc->touch_state.t0Id, jc->touch_state.t1Id, jc->touch_state.t0Down, jc->touch_state.t1Down, + // jc->touch_state.t0X, jc->touch_state.t0Y, jc->touch_state.t1X, jc->touch_state.t1Y); + + // DS dpad is a hat... 0x08 is released, 0=N, 1=NE, 2=E, 3=SE, 4=S, 5=SW, 6=W, 7=NW + // http://eleccelerator.com/wiki/index.php?title=DualShock_4 + uint8_t hat = packet[indexOffset + 7] & 0x0f; + + if ((hat > 2) & (hat < 6)) jc->simple_state.buttons |= JSMASK_DOWN; // down = SE | S | SW + if ((hat == 7) | (hat < 2)) jc->simple_state.buttons |= JSMASK_UP; // up = N | NE | NW + if ((hat > 0) & (hat < 4)) jc->simple_state.buttons |= JSMASK_RIGHT; // right = NE | E | SE + if ((hat > 4) & (hat < 8)) jc->simple_state.buttons |= JSMASK_LEFT; // left = SW | W | NW + + jc->simple_state.buttons |= ((int)(packet[indexOffset + 7] >> 4) << JSOFFSET_W) & JSMASK_W; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 7] >> 7) << JSOFFSET_N) & JSMASK_N; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 7] >> 5) << JSOFFSET_S) & JSMASK_S; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 7] >> 6) << JSOFFSET_E) & JSMASK_E; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 8] >> 6) << JSOFFSET_LCLICK) & JSMASK_LCLICK; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 8] >> 7) << JSOFFSET_RCLICK) & JSMASK_RCLICK; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 8] >> 5) << JSOFFSET_OPTIONS) & JSMASK_OPTIONS; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 8] >> 4) << JSOFFSET_SHARE) & JSMASK_SHARE; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 8] >> 1) << JSOFFSET_R) & JSMASK_R; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 8]) << JSOFFSET_L) & JSMASK_L; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 9]) << JSOFFSET_PS) & JSMASK_PS; + jc->simple_state.buttons |= ((int)(packet[indexOffset + 9] >> 1) << JSOFFSET_TOUCHPAD_CLICK) & JSMASK_TOUCHPAD_CLICK; + //jc->btns.zr = (packet[indexOffset+6] >> 3) & 1; + //jc->btns.zl = (packet[indexOffset+6] >> 2) & 1; + jc->simple_state.rTrigger = packet[indexOffset + 5] / 255.0f; + jc->simple_state.lTrigger = packet[indexOffset + 4] / 255.0f; + + if (jc->simple_state.rTrigger > 0.0) jc->simple_state.buttons |= JSMASK_ZR; + if (jc->simple_state.lTrigger > 0.0) jc->simple_state.buttons |= JSMASK_ZL; + + uint16_t stick_x = packet[indexOffset + 0]; + uint16_t stick_y = packet[indexOffset + 1]; + stick_y = 255 - stick_y; + + uint16_t stick2_x = packet[indexOffset + 2]; + uint16_t stick2_y = packet[indexOffset + 3]; + stick2_y = 255 - stick2_y; + + jc->simple_state.stickLX = (std::fmin)(1.0, (stick_x - 127.0) / 127.0); + jc->simple_state.stickLY = (std::fmin)(1.0, (stick_y - 127.0) / 127.0); + jc->simple_state.stickRX = (std::fmin)(1.0, (stick2_x - 127.0) / 127.0); + jc->simple_state.stickRY = (std::fmin)(1.0, (stick2_y - 127.0) / 127.0); + + if (jc->use_continuous_calibration) { + jc->push_sensor_samples(jc->imu_state.gyroX, jc->imu_state.gyroY, jc->imu_state.gyroZ, + sqrtf(jc->imu_state.accelX * jc->imu_state.accelX + jc->imu_state.accelY * jc->imu_state.accelY + jc->imu_state.accelZ * jc->imu_state.accelZ)); + jc->get_average_gyro(jc->offset_x, jc->offset_y, jc->offset_z, jc->accel_magnitude); + } + + jc->imu_state.gyroX -= jc->offset_x; + jc->imu_state.gyroY -= jc->offset_y; + jc->imu_state.gyroZ -= jc->offset_z; + return true; } diff --git a/JoyShockLibrary/JoyShockLibrary.cpp b/JoyShockLibrary/JoyShockLibrary.cpp index 82a207b..a327a6b 100644 --- a/JoyShockLibrary/JoyShockLibrary.cpp +++ b/JoyShockLibrary/JoyShockLibrary.cpp @@ -139,8 +139,7 @@ void pollIndividualLoop(JoyShock *jc) { _pollCallback(jc->intHandle, jc->simple_state, jc->last_simple_state, jc->imu_state, jc->last_imu_state, jc->delta_time); } // touchpad will have its own callback so that it doesn't change the existing api - // todo: s_ds (DualSense) should be able to do this, too - if (jc->controller_type == ControllerType::s_ds4 && _pollTouchCallback != nullptr) { + if (jc->controller_type != ControllerType::n_switch && _pollTouchCallback != nullptr) { _pollTouchCallback(jc->intHandle, jc->touch_state, jc->last_touch_state, jc->delta_time); } _callbackLock.unlock_shared(); @@ -179,7 +178,7 @@ void pollIndividualLoop(JoyShock *jc) { int JslConnectDevices() { // for writing to console: - freopen("CONOUT$", "w", stdout); + //freopen("CONOUT$", "w", stdout); if (_joyshocks.size() > 0) { // already connected? clean up old stuff! JslDisconnectAndDisposeAll();