octo/yuzu-android
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

input_common: joycon: Remove magic numbers from calibration protocol

Browse Source
This commit is contained in:
Narr the Reg
2023-01-27 13:12:54 -06:00
parent e54d08fc1f
commit 8647c72778
6 changed files with 203 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

212
src/input_common/helpers/joycon_protocol/calibration.cpp
View File

@ -13,34 +13,34 @@ 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);
}
// Read User defined calibration
if (result == DriverResult::Success && has_user_calibration) {
result = ReadSPI(SpiAddress::USER_LEFT_DATA, spi_calibration);
}
// Read Factory calibration
if (result == DriverResult::Success && !has_user_calibration) {
result = ReadSPI(SpiAddress::FACT_LEFT_DATA, spi_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));
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);
}
// 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);
@ -49,34 +49,34 @@ 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);
}
// 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);
}
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));
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);
}
// 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);
@ -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;
}
if (calibration.x.min == 0xFFF || calibration.x.min == 0) {
calibration.x.min = 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);
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;
}
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) {
if (sensor.scale == 0) {
sensor.scale = 0x4000;
}
sensor.scale = ValidateValue(sensor.scale, DefaultAccelerometerScale);
sensor.offset = ValidateValue(sensor.offset, DefaultOffset);
}
for (auto& sensor : calibration.gyro) {
if (sensor.scale == 0) {
sensor.scale = 0x3be7;
}
sensor.scale = ValidateValue(sensor.scale, DefaultGyroScale);
sensor.offset = ValidateValue(sensor.offset, DefaultOffset);
}
}
u16 CalibrationProtocol::ValidateValue(u16 value, u16 default_value) const {
if (value == 0) {
return default_value;
}
if (value == 0xFFF) {
return default_value;
}
return value;
}
s16 CalibrationProtocol::ValidateValue(s16 value, s16 default_value) const {
if (value == 0) {
return default_value;
}
if (value == 0xFFF) {
return default_value;
}
return value;
}
} // namespace InputCommon::Joycon