From 8647c727781accf79fe78c19eeee9acdb24f2927 Mon Sep 17 00:00:00 2001
From: Narr the Reg <juangerman-13@hotmail.com>
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 f6e7e97d59..d8f040f75b 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<JoyconHandle> handle)
DriverResult CalibrationProtocol::GetLeftJoyStickCalibration(JoyStickCalibration& calibration) {
ScopedSetBlocking sb(this);
- std::vector<u8> 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<u16>(((buffer[1] & 0x0F) << 8) | buffer[0]);
- calibration.y.max = static_cast<u16>((buffer[2] << 4) | (buffer[1] >> 4));
- calibration.x.center = static_cast<u16>(((buffer[4] & 0x0F) << 8) | buffer[3]);
- calibration.y.center = static_cast<u16>((buffer[5] << 4) | (buffer[4] >> 4));
- calibration.x.min = static_cast<u16>(((buffer[7] & 0x0F) << 8) | buffer[6]);
- calibration.y.min = static_cast<u16>((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<u8> 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<u16>(((buffer[1] & 0x0F) << 8) | buffer[0]);
- calibration.y.center = static_cast<u16>((buffer[2] << 4) | (buffer[1] >> 4));
- calibration.x.min = static_cast<u16>(((buffer[4] & 0x0F) << 8) | buffer[3]);
- calibration.y.min = static_cast<u16>((buffer[5] << 4) | (buffer[4] >> 4));
- calibration.x.max = static_cast<u16>(((buffer[7] & 0x0F) << 8) | buffer[6]);
- calibration.y.max = static_cast<u16>((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<u8> 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<u8> block) const {
+ return static_cast<u16>(((block[1] & 0x0F) << 8) | block[0]);
+}
+
+u16 CalibrationProtocol::GetYAxisCalibrationValue(std::span<u8> block) const {
+ return static_cast<u16>((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-70-g09d2