From 8647c727781accf79fe78c19eeee9acdb24f2927 Mon Sep 17 00:00:00 2001 From: Narr the Reg Date: Fri, 27 Jan 2023 13:12:54 -0600 Subject: input_common: joycon: Remove magic numbers from calibration protocol --- .../helpers/joycon_protocol/calibration.cpp | 210 ++++++++++++--------- 1 file changed, 122 insertions(+), 88 deletions(-) (limited to 'src/input_common/helpers/joycon_protocol/calibration.cpp') diff --git a/src/input_common/helpers/joycon_protocol/calibration.cpp b/src/input_common/helpers/joycon_protocol/calibration.cpp index f6e7e97d5..d8f040f75 100644 --- a/src/input_common/helpers/joycon_protocol/calibration.cpp +++ b/src/input_common/helpers/joycon_protocol/calibration.cpp @@ -13,33 +13,33 @@ CalibrationProtocol::CalibrationProtocol(std::shared_ptr handle) DriverResult CalibrationProtocol::GetLeftJoyStickCalibration(JoyStickCalibration& calibration) { ScopedSetBlocking sb(this); - std::vector buffer; DriverResult result{DriverResult::Success}; + JoystickLeftSpiCalibration spi_calibration{}; + bool has_user_calibration = false; calibration = {}; - 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); - } + result = HasUserCalibration(SpiAddress::USER_LEFT_MAGIC, has_user_calibration); } - 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)); + // Read User defined calibration + if (result == DriverResult::Success && has_user_calibration) { + result = ReadSPI(SpiAddress::USER_LEFT_DATA, spi_calibration); } - // Nintendo fix for drifting stick - // result = ReadSPI(0x60, 0x86 ,buffer, 16); - // calibration.deadzone = (u16)((buffer[4] << 8) & 0xF00 | buffer[3]); + // Read Factory calibration + if (result == DriverResult::Success && !has_user_calibration) { + result = ReadSPI(SpiAddress::FACT_LEFT_DATA, spi_calibration); + } + + if (result == DriverResult::Success) { + calibration.x.center = GetXAxisCalibrationValue(spi_calibration.center); + calibration.y.center = GetYAxisCalibrationValue(spi_calibration.center); + calibration.x.min = GetXAxisCalibrationValue(spi_calibration.min); + calibration.y.min = GetYAxisCalibrationValue(spi_calibration.min); + calibration.x.max = GetXAxisCalibrationValue(spi_calibration.max); + calibration.y.max = GetYAxisCalibrationValue(spi_calibration.max); + } // Set a valid default calibration if data is missing ValidateCalibration(calibration); @@ -49,33 +49,33 @@ DriverResult CalibrationProtocol::GetLeftJoyStickCalibration(JoyStickCalibration DriverResult CalibrationProtocol::GetRightJoyStickCalibration(JoyStickCalibration& calibration) { ScopedSetBlocking sb(this); - std::vector buffer; DriverResult result{DriverResult::Success}; + JoystickRightSpiCalibration spi_calibration{}; + bool has_user_calibration = false; calibration = {}; - 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); - } + result = HasUserCalibration(SpiAddress::USER_RIGHT_MAGIC, has_user_calibration); } - 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)); + // Read User defined calibration + if (result == DriverResult::Success && has_user_calibration) { + result = ReadSPI(SpiAddress::USER_RIGHT_DATA, spi_calibration); + } + + // Read Factory calibration + if (result == DriverResult::Success && !has_user_calibration) { + result = ReadSPI(SpiAddress::FACT_RIGHT_DATA, spi_calibration); } - // Nintendo fix for drifting stick - // buffer = ReadSPI(0x60, 0x98 , 16); - // joystick.deadzone = (u16)((buffer[4] << 8) & 0xF00 | buffer[3]); + if (result == DriverResult::Success) { + calibration.x.center = GetXAxisCalibrationValue(spi_calibration.center); + calibration.y.center = GetYAxisCalibrationValue(spi_calibration.center); + calibration.x.min = GetXAxisCalibrationValue(spi_calibration.min); + calibration.y.min = GetYAxisCalibrationValue(spi_calibration.min); + calibration.x.max = GetXAxisCalibrationValue(spi_calibration.max); + calibration.y.max = GetYAxisCalibrationValue(spi_calibration.max); + } // Set a valid default calibration if data is missing ValidateCalibration(calibration); @@ -85,39 +85,41 @@ DriverResult CalibrationProtocol::GetRightJoyStickCalibration(JoyStickCalibratio DriverResult CalibrationProtocol::GetImuCalibration(MotionCalibration& calibration) { ScopedSetBlocking sb(this); - std::vector buffer; DriverResult result{DriverResult::Success}; + ImuSpiCalibration spi_calibration{}; + bool has_user_calibration = false; calibration = {}; - 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); - } + result = HasUserCalibration(SpiAddress::USER_IMU_MAGIC, has_user_calibration); + } + + // Read User defined calibration + if (result == DriverResult::Success && has_user_calibration) { + result = ReadSPI(SpiAddress::USER_IMU_DATA, spi_calibration); + } + + // Read Factory calibration + if (result == DriverResult::Success && !has_user_calibration) { + result = ReadSPI(SpiAddress::FACT_IMU_DATA, spi_calibration); } 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].offset = spi_calibration.accelerometer_offset[0]; + calibration.accelerometer[1].offset = spi_calibration.accelerometer_offset[1]; + calibration.accelerometer[2].offset = spi_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.accelerometer[0].scale = spi_calibration.accelerometer_scale[0]; + calibration.accelerometer[1].scale = spi_calibration.accelerometer_scale[1]; + calibration.accelerometer[2].scale = spi_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].offset = spi_calibration.gyroscope_offset[0]; + calibration.gyro[1].offset = spi_calibration.gyroscope_offset[1]; + calibration.gyro[2].offset = spi_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]; + calibration.gyro[0].scale = spi_calibration.gyroscope_scale[0]; + calibration.gyro[1].scale = spi_calibration.gyroscope_scale[1]; + calibration.gyro[2].scale = spi_calibration.gyroscope_scale[2]; } ValidateCalibration(calibration); @@ -127,10 +129,12 @@ DriverResult CalibrationProtocol::GetImuCalibration(MotionCalibration& calibrati DriverResult CalibrationProtocol::GetRingCalibration(RingCalibration& calibration, s16 current_value) { + constexpr s16 DefaultRingRange{800}; + // TODO: Get default calibration form ring itself if (ring_data_max == 0 && ring_data_min == 0) { - ring_data_max = current_value + 800; - ring_data_min = current_value - 800; + ring_data_max = current_value + DefaultRingRange; + ring_data_min = current_value - DefaultRingRange; ring_data_default = current_value; } ring_data_max = std::max(ring_data_max, current_value); @@ -143,42 +147,72 @@ DriverResult CalibrationProtocol::GetRingCalibration(RingCalibration& calibratio return DriverResult::Success; } +DriverResult CalibrationProtocol::HasUserCalibration(SpiAddress address, + bool& has_user_calibration) { + MagicSpiCalibration spi_magic{}; + const DriverResult result{ReadSPI(address, spi_magic)}; + has_user_calibration = false; + if (result == DriverResult::Success) { + has_user_calibration = spi_magic.first == CalibrationMagic::USR_MAGIC_0 && + spi_magic.second == CalibrationMagic::USR_MAGIC_1; + } + return result; +} + +u16 CalibrationProtocol::GetXAxisCalibrationValue(std::span block) const { + return static_cast(((block[1] & 0x0F) << 8) | block[0]); +} + +u16 CalibrationProtocol::GetYAxisCalibrationValue(std::span block) const { + return static_cast((block[2] << 4) | (block[1] >> 4)); +} + void CalibrationProtocol::ValidateCalibration(JoyStickCalibration& calibration) { - constexpr u16 DefaultStickCenter{2048}; - constexpr u16 DefaultStickRange{1740}; + constexpr u16 DefaultStickCenter{0x800}; + constexpr u16 DefaultStickRange{0x6cc}; - 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; + calibration.x.center = ValidateValue(calibration.x.center, DefaultStickCenter); + calibration.x.max = ValidateValue(calibration.x.max, DefaultStickRange); + calibration.x.min = ValidateValue(calibration.x.min, DefaultStickRange); + + calibration.y.center = ValidateValue(calibration.y.center, DefaultStickCenter); + calibration.y.max = ValidateValue(calibration.y.max, DefaultStickRange); + calibration.y.min = ValidateValue(calibration.y.min, DefaultStickRange); +} + +void CalibrationProtocol::ValidateCalibration(MotionCalibration& calibration) { + constexpr s16 DefaultAccelerometerScale{0x4000}; + constexpr s16 DefaultGyroScale{0x3be7}; + constexpr s16 DefaultOffset{0}; + + for (auto& sensor : calibration.accelerometer) { + sensor.scale = ValidateValue(sensor.scale, DefaultAccelerometerScale); + sensor.offset = ValidateValue(sensor.offset, DefaultOffset); } - if (calibration.x.min == 0xFFF || calibration.x.min == 0) { - calibration.x.min = DefaultStickRange; + for (auto& sensor : calibration.gyro) { + sensor.scale = ValidateValue(sensor.scale, DefaultGyroScale); + sensor.offset = ValidateValue(sensor.offset, DefaultOffset); } +} - 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; +u16 CalibrationProtocol::ValidateValue(u16 value, u16 default_value) const { + if (value == 0) { + return default_value; } - if (calibration.y.min == 0xFFF || calibration.y.min == 0) { - calibration.y.min = DefaultStickRange; + if (value == 0xFFF) { + return default_value; } + return value; } -void CalibrationProtocol::ValidateCalibration(MotionCalibration& calibration) { - for (auto& sensor : calibration.accelerometer) { - if (sensor.scale == 0) { - sensor.scale = 0x4000; - } +s16 CalibrationProtocol::ValidateValue(s16 value, s16 default_value) const { + if (value == 0) { + return default_value; } - for (auto& sensor : calibration.gyro) { - if (sensor.scale == 0) { - sensor.scale = 0x3be7; - } + if (value == 0xFFF) { + return default_value; } + return value; } } // namespace InputCommon::Joycon -- cgit v1.2.3