chaphidoesstuff/suyu
1
0
Fork 0
forked from suyu/suyu
Code Pull requests Activity

Merge pull request #9689 from german77/joycon-calibration

Browse source 
input_common: joycon: Remove magic numbers from calibration protocol
This commit is contained in:
bunnei 2023-01-29 02:43:14 -08:00 committed by GitHub
commit 9170387e71
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 216 additions and 115 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

206
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

18
src/input_common/helpers/joycon_protocol/calibration.h
View file

@ -53,9 +53,27 @@ public:
DriverResult GetRingCalibration(RingCalibration& calibration, s16 current_value);
private:
/// Returns true if the specified address corresponds to the magic value of user calibration
DriverResult HasUserCalibration(SpiAddress address, bool& has_user_calibration);
/// Converts a raw calibration block to an u16 value containing the x axis value
u16 GetXAxisCalibrationValue(std::span<u8> block) const;
/// Converts a raw calibration block to an u16 value containing the y axis value
u16 GetYAxisCalibrationValue(std::span<u8> block) const;
/// Ensures that all joystick calibration values are set
void ValidateCalibration(JoyStickCalibration& calibration);
/// Ensures that all motion calibration values are set
void ValidateCalibration(MotionCalibration& calibration);
/// Returns the default value if the value is either zero or 0xFFF
u16 ValidateValue(u16 value, u16 default_value) const;
/// Returns the default value if the value is either zero or 0xFFF
s16 ValidateValue(s16 value, s16 default_value) const;
s16 ring_data_max = 0;
s16 ring_data_default = 0;
s16 ring_data_min = 0;

18
src/input_common/helpers/joycon_protocol/common_protocol.cpp
View file

@ -22,8 +22,8 @@ void JoyconCommonProtocol::SetNonBlocking() {
}
DriverResult JoyconCommonProtocol::GetDeviceType(ControllerType& controller_type) {
std::vector<u8> buffer;
const auto result = ReadSPI(CalAddr::DEVICE_TYPE, 1, buffer);
std::array<u8, 1> buffer{};
const auto result = ReadRawSPI(SpiAddress::DEVICE_TYPE, buffer);
controller_type = ControllerType::None;
if (result == DriverResult::Success) {
@ -148,11 +148,13 @@ DriverResult JoyconCommonProtocol::SendVibrationReport(std::span<const u8> buffe
return SendData(local_buffer);
}
DriverResult JoyconCommonProtocol::ReadSPI(CalAddr addr, u8 size, std::vector<u8>& output) {
DriverResult JoyconCommonProtocol::ReadRawSPI(SpiAddress addr, std::span<u8> output) {
constexpr std::size_t HeaderSize = 20;
constexpr std::size_t MaxTries = 10;
const auto size = output.size();
std::size_t tries = 0;
std::array<u8, 5> buffer = {0x00, 0x00, 0x00, 0x00, size};
std::vector<u8> local_buffer(size + 20);
std::array<u8, 5> buffer = {0x00, 0x00, 0x00, 0x00, static_cast<u8>(size)};
std::vector<u8> local_buffer{};
buffer[0] = static_cast<u8>(static_cast<u16>(addr) & 0x00FF);
buffer[1] = static_cast<u8>((static_cast<u16>(addr) & 0xFF00) >> 8);
@ -167,8 +169,12 @@ DriverResult JoyconCommonProtocol::ReadSPI(CalAddr addr, u8 size, std::vector<u8
}
} while (local_buffer[15] != buffer[0] || local_buffer[16] != buffer[1]);
if (local_buffer.size() < size + HeaderSize) {
return DriverResult::WrongReply;
}
// Remove header from output
output = std::vector<u8>(local_buffer.begin() + 20, local_buffer.begin() + 20 + size);
memcpy(output.data(), local_buffer.data() + HeaderSize, size);
return DriverResult::Success;
}

23
src/input_common/helpers/joycon_protocol/common_protocol.h
View file

@ -97,10 +97,29 @@ public:
/**
* Reads the SPI memory stored on the joycon
* @param Initial address location
* @param size in bytes to be read
* @returns output buffer containing the responce
*/
DriverResult ReadSPI(CalAddr addr, u8 size, std::vector<u8>& output);
DriverResult ReadRawSPI(SpiAddress addr, std::span<u8> output);
/**
* Reads the SPI memory stored on the joycon
* @param Initial address location
* @returns output object containing the responce
*/
template <typename Output>
requires std::is_trivially_copyable_v<Output> DriverResult ReadSPI(SpiAddress addr,
Output& output) {
std::array<u8, sizeof(Output)> buffer;
output = {};
const auto result = ReadRawSPI(addr, buffer);
if (result != DriverResult::Success) {
return result;
}
std::memcpy(&output, buffer.data(), sizeof(Output));
return DriverResult::Success;
}
/**
* Enables MCU chip on the joycon

8
src/input_common/helpers/joycon_protocol/generic_functions.cpp
View file

@ -71,8 +71,8 @@ DriverResult GenericProtocol::GetBattery(u32& battery_level) {
DriverResult GenericProtocol::GetColor(Color& color) {
ScopedSetBlocking sb(this);
std::vector<u8> buffer;
const auto result = ReadSPI(CalAddr::COLOR_DATA, 12, buffer);
std::array<u8, 12> buffer{};
const auto result = ReadRawSPI(SpiAddress::COLOR_DATA, buffer);
color = {};
if (result == DriverResult::Success) {
@ -87,8 +87,8 @@ DriverResult GenericProtocol::GetColor(Color& color) {
DriverResult GenericProtocol::GetSerialNumber(SerialNumber& serial_number) {
ScopedSetBlocking sb(this);
std::vector<u8> buffer;
const auto result = ReadSPI(CalAddr::SERIAL_NUMBER, 16, buffer);
std::array<u8, 16> buffer{};
const auto result = ReadRawSPI(SpiAddress::SERIAL_NUMBER, buffer);
serial_number = {};
if (result == DriverResult::Success) {

52
src/input_common/helpers/joycon_protocol/joycon_types.h
View file

@ -159,13 +159,12 @@ enum class UsbSubCommand : u8 {
SEND_UART = 0x92,
};
enum class CalMagic : u8 {
enum class CalibrationMagic : u8 {
USR_MAGIC_0 = 0xB2,
USR_MAGIC_1 = 0xA1,
USRR_MAGI_SIZE = 2,
};
enum class CalAddr {
enum class SpiAddress {
SERIAL_NUMBER = 0X6000,
DEVICE_TYPE = 0X6012,
COLOR_EXIST = 0X601B,
@ -396,10 +395,35 @@ struct MotionData {
u64 delta_timestamp{};
};
// Output from SPI read command containing user calibration magic
struct MagicSpiCalibration {
CalibrationMagic first;
CalibrationMagic second;
};
static_assert(sizeof(MagicSpiCalibration) == 0x2, "MagicSpiCalibration is an invalid size");
// Output from SPI read command containing left joystick calibration
struct JoystickLeftSpiCalibration {
std::array<u8, 3> max;
std::array<u8, 3> center;
std::array<u8, 3> min;
};
static_assert(sizeof(JoystickLeftSpiCalibration) == 0x9,
"JoystickLeftSpiCalibration is an invalid size");
// Output from SPI read command containing right joystick calibration
struct JoystickRightSpiCalibration {
std::array<u8, 3> center;
std::array<u8, 3> min;
std::array<u8, 3> max;
};
static_assert(sizeof(JoystickRightSpiCalibration) == 0x9,
"JoystickRightSpiCalibration is an invalid size");
struct JoyStickAxisCalibration {
u16 max{1};
u16 min{1};
u16 center{0};
u16 max;
u16 min;
u16 center;
};
struct JoyStickCalibration {
@ -407,6 +431,14 @@ struct JoyStickCalibration {
JoyStickAxisCalibration y;
};
struct ImuSpiCalibration {
std::array<s16, 3> accelerometer_offset;
std::array<s16, 3> accelerometer_scale;
std::array<s16, 3> gyroscope_offset;
std::array<s16, 3> gyroscope_scale;
};
static_assert(sizeof(ImuSpiCalibration) == 0x18, "ImuSpiCalibration is an invalid size");
struct RingCalibration {
s16 default_value;
s16 max_value;
@ -488,14 +520,6 @@ struct InputReportNfcIr {
static_assert(sizeof(InputReportNfcIr) == 0x29, "InputReportNfcIr is an invalid size");
#pragma pack(pop)
struct IMUCalibration {
std::array<s16, 3> accelerometer_offset;
std::array<s16, 3> accelerometer_scale;
std::array<s16, 3> gyroscope_offset;
std::array<s16, 3> gyroscope_scale;
};
static_assert(sizeof(IMUCalibration) == 0x18, "IMUCalibration is an invalid size");
struct NFCReadBlock {
u8 start;
u8 end;