From 5676c2e17fe895e450e185029991fc20bdf56ec5 Mon Sep 17 00:00:00 2001 From: Narr the Reg Date: Tue, 20 Dec 2022 18:09:59 -0600 Subject: [PATCH] input_common: Use calibration from joycon --- src/input_common/CMakeLists.txt | 2 + src/input_common/helpers/joycon_driver.cpp | 4 + src/input_common/helpers/joycon_driver.h | 2 + .../helpers/joycon_protocol/calibration.cpp | 169 ++++++++++++++++++ .../helpers/joycon_protocol/calibration.h | 54 ++++++ 5 files changed, 231 insertions(+) create mode 100644 src/input_common/helpers/joycon_protocol/calibration.cpp create mode 100644 src/input_common/helpers/joycon_protocol/calibration.h diff --git a/src/input_common/CMakeLists.txt b/src/input_common/CMakeLists.txt index a60cecaf47..d4307351c9 100644 --- a/src/input_common/CMakeLists.txt +++ b/src/input_common/CMakeLists.txt @@ -57,6 +57,8 @@ if (ENABLE_SDL2) drivers/sdl_driver.h helpers/joycon_driver.cpp helpers/joycon_driver.h + helpers/joycon_protocol/calibration.cpp + helpers/joycon_protocol/calibration.h helpers/joycon_protocol/common_protocol.cpp helpers/joycon_protocol/common_protocol.h helpers/joycon_protocol/generic_functions.cpp diff --git a/src/input_common/helpers/joycon_driver.cpp b/src/input_common/helpers/joycon_driver.cpp index 0de55578b2..ac11be1c17 100644 --- a/src/input_common/helpers/joycon_driver.cpp +++ b/src/input_common/helpers/joycon_driver.cpp @@ -64,6 +64,7 @@ DriverResult JoyconDriver::InitializeDevice() { accelerometer_performance = Joycon::AccelerometerPerformance::HZ100; // Initialize HW Protocols + calibration_protocol = std::make_unique(hidapi_handle); generic_protocol = std::make_unique(hidapi_handle); // Get fixed joycon info @@ -79,6 +80,9 @@ DriverResult JoyconDriver::InitializeDevice() { supported_features = GetSupportedFeatures(); // Get Calibration data + calibration_protocol->GetLeftJoyStickCalibration(left_stick_calibration); + calibration_protocol->GetRightJoyStickCalibration(right_stick_calibration); + calibration_protocol->GetImuCalibration(motion_calibration); // Set led status generic_protocol->SetLedBlinkPattern(static_cast(1 + port)); diff --git a/src/input_common/helpers/joycon_driver.h b/src/input_common/helpers/joycon_driver.h index deb50ec772..275c97b913 100644 --- a/src/input_common/helpers/joycon_driver.h +++ b/src/input_common/helpers/joycon_driver.h @@ -8,6 +8,7 @@ #include #include +#include "input_common/helpers/joycon_protocol/calibration.h" #include "input_common/helpers/joycon_protocol/generic_functions.h" #include "input_common/helpers/joycon_protocol/joycon_types.h" @@ -95,6 +96,7 @@ private: void ReadNfcIRMode(std::span buffer); // Protocol Features + std::unique_ptr calibration_protocol = nullptr; std::unique_ptr generic_protocol = nullptr; // Connection status diff --git a/src/input_common/helpers/joycon_protocol/calibration.cpp b/src/input_common/helpers/joycon_protocol/calibration.cpp new file mode 100644 index 0000000000..5c29af545b --- /dev/null +++ b/src/input_common/helpers/joycon_protocol/calibration.cpp @@ -0,0 +1,169 @@ +// SPDX-FileCopyrightText: Copyright 2022 yuzu Emulator Project +// SPDX-License-Identifier: GPL-2.0-or-later + +#include + +#include "input_common/helpers/joycon_protocol/calibration.h" +#include "input_common/helpers/joycon_protocol/joycon_types.h" + +namespace InputCommon::Joycon { + +CalibrationProtocol::CalibrationProtocol(std::shared_ptr handle) + : JoyconCommonProtocol(handle) {} + +DriverResult CalibrationProtocol::GetLeftJoyStickCalibration(JoyStickCalibration& calibration) { + std::vector buffer; + DriverResult result{DriverResult::Success}; + calibration = {}; + SetBlocking(); + + result = ReadSPI(CalAddr::USER_LEFT_MAGIC, sizeof(u16), buffer); + + if (result == DriverResult::Success) { + const bool has_user_calibration = buffer[0] == 0xB2 && buffer[1] == 0xA1; + if (has_user_calibration) { + result = ReadSPI(CalAddr::USER_LEFT_DATA, 9, buffer); + } else { + result = ReadSPI(CalAddr::FACT_LEFT_DATA, 9, buffer); + } + } + + if (result == DriverResult::Success) { + calibration.x.max = static_cast(((buffer[1] & 0x0F) << 8) | buffer[0]); + calibration.y.max = static_cast((buffer[2] << 4) | (buffer[1] >> 4)); + calibration.x.center = static_cast(((buffer[4] & 0x0F) << 8) | buffer[3]); + calibration.y.center = static_cast((buffer[5] << 4) | (buffer[4] >> 4)); + calibration.x.min = static_cast(((buffer[7] & 0x0F) << 8) | buffer[6]); + calibration.y.min = static_cast((buffer[8] << 4) | (buffer[7] >> 4)); + } + + // Nintendo fix for drifting stick + // result = ReadSPI(0x60, 0x86 ,buffer, 16); + // calibration.deadzone = (u16)((buffer[4] << 8) & 0xF00 | buffer[3]); + + // Set a valid default calibration if data is missing + ValidateCalibration(calibration); + + SetNonBlocking(); + return result; +} + +DriverResult CalibrationProtocol::GetRightJoyStickCalibration(JoyStickCalibration& calibration) { + std::vector buffer; + DriverResult result{DriverResult::Success}; + calibration = {}; + SetBlocking(); + + result = ReadSPI(CalAddr::USER_RIGHT_MAGIC, sizeof(u16), buffer); + + if (result == DriverResult::Success) { + const bool has_user_calibration = buffer[0] == 0xB2 && buffer[1] == 0xA1; + if (has_user_calibration) { + result = ReadSPI(CalAddr::USER_RIGHT_DATA, 9, buffer); + } else { + result = ReadSPI(CalAddr::FACT_RIGHT_DATA, 9, buffer); + } + } + + if (result == DriverResult::Success) { + calibration.x.center = static_cast(((buffer[1] & 0x0F) << 8) | buffer[0]); + calibration.y.center = static_cast((buffer[2] << 4) | (buffer[1] >> 4)); + calibration.x.min = static_cast(((buffer[4] & 0x0F) << 8) | buffer[3]); + calibration.y.min = static_cast((buffer[5] << 4) | (buffer[4] >> 4)); + calibration.x.max = static_cast(((buffer[7] & 0x0F) << 8) | buffer[6]); + calibration.y.max = static_cast((buffer[8] << 4) | (buffer[7] >> 4)); + } + + // Nintendo fix for drifting stick + // buffer = ReadSPI(0x60, 0x98 , 16); + // joystick.deadzone = (u16)((buffer[4] << 8) & 0xF00 | buffer[3]); + + // Set a valid default calibration if data is missing + ValidateCalibration(calibration); + + SetNonBlocking(); + return result; +} + +DriverResult CalibrationProtocol::GetImuCalibration(MotionCalibration& calibration) { + std::vector buffer; + DriverResult result{DriverResult::Success}; + calibration = {}; + SetBlocking(); + + result = ReadSPI(CalAddr::USER_IMU_MAGIC, sizeof(u16), buffer); + + if (result == DriverResult::Success) { + const bool has_user_calibration = buffer[0] == 0xB2 && buffer[1] == 0xA1; + if (has_user_calibration) { + result = ReadSPI(CalAddr::USER_IMU_DATA, sizeof(IMUCalibration), buffer); + } else { + result = ReadSPI(CalAddr::FACT_IMU_DATA, sizeof(IMUCalibration), buffer); + } + } + + if (result == DriverResult::Success) { + IMUCalibration device_calibration{}; + memcpy(&device_calibration, buffer.data(), sizeof(IMUCalibration)); + calibration.accelerometer[0].offset = device_calibration.accelerometer_offset[0]; + calibration.accelerometer[1].offset = device_calibration.accelerometer_offset[1]; + calibration.accelerometer[2].offset = device_calibration.accelerometer_offset[2]; + + calibration.accelerometer[0].scale = device_calibration.accelerometer_scale[0]; + calibration.accelerometer[1].scale = device_calibration.accelerometer_scale[1]; + calibration.accelerometer[2].scale = device_calibration.accelerometer_scale[2]; + + calibration.gyro[0].offset = device_calibration.gyroscope_offset[0]; + calibration.gyro[1].offset = device_calibration.gyroscope_offset[1]; + calibration.gyro[2].offset = device_calibration.gyroscope_offset[2]; + + calibration.gyro[0].scale = device_calibration.gyroscope_scale[0]; + calibration.gyro[1].scale = device_calibration.gyroscope_scale[1]; + calibration.gyro[2].scale = device_calibration.gyroscope_scale[2]; + } + + ValidateCalibration(calibration); + + SetNonBlocking(); + return result; +} + +void CalibrationProtocol::ValidateCalibration(JoyStickCalibration& calibration) { + constexpr u16 DefaultStickCenter{2048}; + constexpr u16 DefaultStickRange{1740}; + + if (calibration.x.center == 0xFFF || calibration.x.center == 0) { + calibration.x.center = DefaultStickCenter; + } + if (calibration.x.max == 0xFFF || calibration.x.max == 0) { + calibration.x.max = DefaultStickRange; + } + if (calibration.x.min == 0xFFF || calibration.x.min == 0) { + calibration.x.min = DefaultStickRange; + } + + if (calibration.y.center == 0xFFF || calibration.y.center == 0) { + calibration.y.center = DefaultStickCenter; + } + if (calibration.y.max == 0xFFF || calibration.y.max == 0) { + calibration.y.max = DefaultStickRange; + } + if (calibration.y.min == 0xFFF || calibration.y.min == 0) { + calibration.y.min = DefaultStickRange; + } +} + +void CalibrationProtocol::ValidateCalibration(MotionCalibration& calibration) { + for (auto& sensor : calibration.accelerometer) { + if (sensor.scale == 0) { + sensor.scale = 0x4000; + } + } + for (auto& sensor : calibration.gyro) { + if (sensor.scale == 0) { + sensor.scale = 0x3be7; + } + } +} + +} // namespace InputCommon::Joycon diff --git a/src/input_common/helpers/joycon_protocol/calibration.h b/src/input_common/helpers/joycon_protocol/calibration.h new file mode 100644 index 0000000000..38214eed41 --- /dev/null +++ b/src/input_common/helpers/joycon_protocol/calibration.h @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright 2022 yuzu Emulator Project +// SPDX-License-Identifier: GPL-2.0-or-later + +// Based on dkms-hid-nintendo implementation, CTCaer joycon toolkit and dekuNukem reverse +// engineering https://github.com/nicman23/dkms-hid-nintendo/blob/master/src/hid-nintendo.c +// https://github.com/CTCaer/jc_toolkit +// https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering + +#pragma once + +#include + +#include "input_common/helpers/joycon_protocol/common_protocol.h" + +namespace InputCommon::Joycon { +enum class DriverResult; +struct JoyStickCalibration; +struct IMUCalibration; +struct JoyconHandle; +} // namespace InputCommon::Joycon + +namespace InputCommon::Joycon { + +/// Driver functions related to retrieving calibration data from the device +class CalibrationProtocol final : private JoyconCommonProtocol { +public: + CalibrationProtocol(std::shared_ptr handle); + + /** + * Sends a request to obtain the left stick calibration from memory + * @param is_factory_calibration if true factory values will be returned + * @returns JoyStickCalibration of the left joystick + */ + DriverResult GetLeftJoyStickCalibration(JoyStickCalibration& calibration); + + /** + * Sends a request to obtain the right stick calibration from memory + * @param is_factory_calibration if true factory values will be returned + * @returns JoyStickCalibration of the right joystick + */ + DriverResult GetRightJoyStickCalibration(JoyStickCalibration& calibration); + + /** + * Sends a request to obtain the motion calibration from memory + * @returns ImuCalibration of the motion sensor + */ + DriverResult GetImuCalibration(MotionCalibration& calibration); + +private: + void ValidateCalibration(JoyStickCalibration& calibration); + void ValidateCalibration(MotionCalibration& calibration); +}; + +} // namespace InputCommon::Joycon