Fully implemented Switch pro controller (USB mode)

source/ControllerLib/Controllers/BaseController.cpp

@@ -126,18 +126,49 @@ ControllerResult BaseController::SetRumble(uint16_t input_idx, float amp_high, f
     return CONTROLLER_STATUS_NOT_IMPLEMENTED;
 }
 
-static_assert(MAX_PIN_BY_BUTTONS == 2, "You need to update IsPinPressed macro !");
-#define IsPinPressed(rawData, controllerButton) \
-    (rawData.buttons[GetConfig().buttons_pin[controllerButton][0]] || rawData.buttons[GetConfig().buttons_pin[controllerButton][1]]) ? true : false
+ControllerResult BaseController::ReadNextBuffer(uint8_t *buffer, size_t *size, uint16_t *input_idx, uint32_t timeout_us)
+{
+    uint16_t controller_idx = m_current_controller_idx;
+    m_current_controller_idx = (m_current_controller_idx + 1) % m_inPipe.size();
+
+    *size = std::min((size_t)m_inPipe[controller_idx]->GetDescriptor()->wMaxPacketSize, *size);
+
+    ControllerResult result = m_inPipe[controller_idx]->Read(buffer, size, timeout_us);
+    if (result != CONTROLLER_STATUS_SUCCESS)
+        return result;
+
+    if (*size == 0)
+        return CONTROLLER_STATUS_NOTHING_TODO;
+
+    *input_idx = controller_idx;
+
+    return CONTROLLER_STATUS_SUCCESS;
+}
 
 ControllerResult BaseController::ReadInput(NormalizedButtonData *normalData, uint16_t *input_idx, uint32_t timeout_us)
 {
     RawInputData rawData;
+    uint8_t input_bytes[CONTROLLER_INPUT_BUFFER_SIZE];
+    size_t size = sizeof(input_bytes);
+
+    ControllerResult result = ReadNextBuffer(input_bytes, &size, input_idx, timeout_us);
+    if (result != CONTROLLER_STATUS_SUCCESS)
+        return result;
 
-    ControllerResult result = ReadRawInput(&rawData, input_idx, timeout_us);
+    result = ParseData(input_bytes, size, &rawData, input_idx);
     if (result != CONTROLLER_STATUS_SUCCESS)
         return result;
 
+    MapRawInputToNormalized(rawData, normalData);
+    return CONTROLLER_STATUS_SUCCESS;
+}
+
+static_assert(MAX_PIN_BY_BUTTONS == 2, "You need to update IsPinPressed macro !");
+#define IsPinPressed(rawData, controllerButton) \
+    (rawData.buttons[GetConfig().buttons_pin[controllerButton][0]] || rawData.buttons[GetConfig().buttons_pin[controllerButton][1]]) ? true : false
+
+void BaseController::MapRawInputToNormalized(RawInputData &rawData, NormalizedButtonData *normalData)
+{
     Log(LogLevelDebug, "Controller[%04x-%04x] DATA: X=%d%%, Y=%d%%, Z=%d%%, Rz=%d%%, B1=%d, B2=%d, B3=%d, B4=%d, B5=%d, B6=%d, B7=%d, B8=%d, B9=%d, B10=%d",
         m_device->GetVendor(), m_device->GetProduct(),
         (int)(rawData.X * 100.0), (int)(rawData.Y * 100.0), (int)(rawData.Z * 100.0), (int)(rawData.Rz * 100.0),
@@ -282,8 +313,6 @@ ControllerResult BaseController::ReadInput(NormalizedButtonData *normalData, uin
             normalData->buttons[GetConfig().simulateCapture[1]] = false;
         }
     }
-
-    return CONTROLLER_STATUS_SUCCESS;
 }
 
 float BaseController::ApplyDeadzone(uint8_t deadzonePercent, float value)
@@ -303,18 +332,57 @@ float BaseController::ApplyDeadzone(uint8_t deadzonePercent, float value)
 
 float BaseController::Normalize(int32_t value, int32_t min, int32_t max)
 {
-    float range = floor((max - min) / 2.0f);
-    float offset = range;
-
-    if (range == max)
-        offset = 0;
+    return Normalize(value, min, max, (max + min) / 2);
+}
 
-    float ret = (value - offset) / range;
+float BaseController::Normalize(int32_t value, int32_t min, int32_t max, int32_t center)
+{
+    float ret = 0.0f;
+    if (value < center)
+    {
+        float offset = (float)min;
+        float range = (float)(center - min);
+        ret = ((value - offset) / range) - 1.0f;
+    }
+    else
+    {
+        float offset = (float)center;
+        float range = (float)(max - center);
+        ret = (value - offset) / range;
+    }
 
     if (ret > 1.0f)
         ret = 1.0f;
     else if (ret < -1.0f)
         ret = -1.0f;
 
     return ret;
+}
+
+uint32_t BaseController::ReadBitsLE(uint8_t *buffer, uint32_t bitOffset, uint32_t bitLength)
+{
+    // Calculate the starting byte index and bit index within that byte
+    uint32_t byteIndex = bitOffset / 8;
+    uint32_t bitIndex = bitOffset % 8; // Little endian, LSB is at index 0
+
+    uint32_t result = 0;
+
+    for (uint32_t i = 0; i < bitLength; ++i)
+    {
+        // Check if we need to move to the next byte
+        if (bitIndex > 7)
+        {
+            ++byteIndex;
+            bitIndex = 0;
+        }
+
+        // Get the bit at the current position and add it to the result
+        uint8_t bit = (buffer[byteIndex] >> bitIndex) & 0x01;
+        result |= (bit << i);
+
+        // Move to the next bit
+        ++bitIndex;
+    }
+
+    return result;
 }

source/ControllerLib/Controllers/BaseController.h

@@ -33,6 +33,12 @@ class BaseController : public IController
     std::vector<IUSBEndpoint *> m_inPipe;
     std::vector<IUSBEndpoint *> m_outPipe;
     std::vector<IUSBInterface *> m_interfaces;
+    uint8_t m_current_controller_idx = 0;
+
+    virtual ControllerResult ReadNextBuffer(uint8_t *buffer, size_t *size, uint16_t *input_idx, uint32_t timeout_us);
+    virtual void MapRawInputToNormalized(RawInputData &rawData, NormalizedButtonData *normalData);
+
+    virtual ControllerResult ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx) = 0;
 
 public:
     BaseController(std::unique_ptr<IUSBDevice> &&device, const ControllerConfig &config, std::unique_ptr<ILogger> &&logger);
@@ -49,11 +55,12 @@ class BaseController : public IController
     virtual uint16_t GetInputCount() override;
 
     ControllerResult ReadInput(NormalizedButtonData *normalData, uint16_t *input_idx, uint32_t timeout_us) override;
-    virtual ControllerResult ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us) = 0;
 
     ControllerResult SetRumble(uint16_t input_idx, float amp_high, float amp_low) override;
 
     // Helper functions
     static float Normalize(int32_t value, int32_t min, int32_t max);
+    static float Normalize(int32_t value, int32_t min, int32_t max, int32_t center);
     static float ApplyDeadzone(uint8_t deadzonePercent, float value);
+    static uint32_t ReadBitsLE(uint8_t *buffer, uint32_t bitOffset, uint32_t bitLength);
 };

source/ControllerLib/Controllers/Dualshock3Controller.cpp

@@ -36,21 +36,16 @@ ControllerResult Dualshock3Controller::OpenInterfaces()
     return CONTROLLER_STATUS_SUCCESS;
 }
 
-ControllerResult Dualshock3Controller::ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us)
+ControllerResult Dualshock3Controller::ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx)
 {
-    uint8_t input_bytes[CONTROLLER_INPUT_BUFFER_SIZE];
-    size_t size = sizeof(input_bytes);
+    (void)input_idx;
+    Dualshock3ButtonData *buttonData = reinterpret_cast<Dualshock3ButtonData *>(buffer);
 
-    ControllerResult result = m_inPipe[0]->Read(input_bytes, &size, timeout_us);
-    if (result != CONTROLLER_STATUS_SUCCESS)
-        return result;
+    if (size < sizeof(Dualshock3ButtonData))
+        return CONTROLLER_STATUS_UNEXPECTED_DATA;
 
-    *input_idx = 0;
-
-    if (input_bytes[0] == Ds3InputPacket_Button)
+    if (buttonData->type == Ds3InputPacket_Button)
     {
-        Dualshock3ButtonData *buttonData = reinterpret_cast<Dualshock3ButtonData *>(input_bytes);
-
         rawData->buttons[1] = buttonData->button1;
         rawData->buttons[2] = buttonData->button2;
         rawData->buttons[3] = buttonData->button3;

source/ControllerLib/Controllers/Dualshock3Controller.h

@@ -121,5 +121,5 @@ class Dualshock3Controller : public BaseController
     virtual ControllerResult Initialize() override;
     virtual ControllerResult OpenInterfaces() override;
 
-    ControllerResult ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us);
+    virtual ControllerResult ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx) override;
 };

source/ControllerLib/Controllers/GenericHIDController.cpp

@@ -65,20 +65,11 @@ uint16_t GenericHIDController::GetInputCount()
     return std::min((int)m_joystick_count, CONTROLLER_MAX_INPUTS);
 }
 
-ControllerResult GenericHIDController::ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us)
+ControllerResult GenericHIDController::ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx)
 {
     HIDJoystickData joystick_data;
-    uint8_t input_bytes[CONTROLLER_INPUT_BUFFER_SIZE];
-    size_t size = std::min((size_t)m_inPipe[0]->GetDescriptor()->wMaxPacketSize, sizeof(input_bytes));
 
-    ControllerResult result = m_inPipe[0]->Read(input_bytes, &size, timeout_us);
-    if (result != CONTROLLER_STATUS_SUCCESS)
-        return result;
-
-    if (size == 0)
-        return CONTROLLER_STATUS_NOTHING_TODO;
-
-    if (!m_joystick->parseData(input_bytes, (uint16_t)size, &joystick_data))
+    if (!m_joystick->parseData(buffer, (uint16_t)size, &joystick_data))
     {
         Log(LogLevelError, "GenericHIDController[%04x-%04x] Failed to parse input data (size=%d)", m_device->GetVendor(), m_device->GetProduct(), size);
         return CONTROLLER_STATUS_UNEXPECTED_DATA;
@@ -87,7 +78,12 @@ ControllerResult GenericHIDController::ReadRawInput(RawInputData *rawData, uint1
     if (joystick_data.index >= GetInputCount())
         return CONTROLLER_STATUS_UNEXPECTED_DATA;
 
-    *input_idx = joystick_data.index;
+    /*
+         Special case for generic HID, input_idx might be bigger than 0 in case of multiple interfaces.
+         If this is the case we expect to have 1 input per interface, thus we don't want to overwrite the input index.
+    */
+    if (input_idx != NULL && *input_idx == 0)
+        *input_idx = joystick_data.index;
 
     for (int i = 0; i < MAX_CONTROLLER_BUTTONS; i++)
         rawData->buttons[i] = joystick_data.buttons[i];

source/ControllerLib/Controllers/GenericHIDController.h

@@ -19,5 +19,5 @@ class GenericHIDController : public BaseController
 
     virtual uint16_t GetInputCount() override;
 
-    virtual ControllerResult ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us) override;
+    virtual ControllerResult ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx) override;
 };

source/ControllerLib/Controllers/SwitchController.cpp

@@ -1,8 +1,12 @@
 #include "Controllers/SwitchController.h"
 
+static_assert(CONTROLLER_INPUT_BUFFER_SIZE == 64, "Input byte for switch as to be 64 bytes long");
+
 SwitchController::SwitchController(std::unique_ptr<IUSBDevice> &&device, const ControllerConfig &config, std::unique_ptr<ILogger> &&logger)
     : BaseController(std::move(device), config, std::move(logger))
 {
+    cal_left_x.max = cal_left_y.max = cal_right_x.max = cal_right_y.max = 3400;
+    cal_left_x.min = cal_left_y.min = cal_right_x.min = cal_right_y.min = 600;
 }
 
 SwitchController::~SwitchController()
@@ -31,20 +35,13 @@ ControllerResult SwitchController::Initialize()
     return CONTROLLER_STATUS_SUCCESS;
 }
 
-ControllerResult SwitchController::ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us)
+ControllerResult SwitchController::ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx)
 {
-    uint8_t input_bytes[CONTROLLER_INPUT_BUFFER_SIZE];
-    size_t size = sizeof(input_bytes);
-
-    static_assert(sizeof(input_bytes) == 64, "Input byte for switch as to be 64 bytes long");
+    (void)input_idx;
+    SwitchButtonData *buttonData = reinterpret_cast<SwitchButtonData *>(buffer);
 
-    ControllerResult result = m_inPipe[0]->Read(input_bytes, &size, timeout_us);
-    if (result != CONTROLLER_STATUS_SUCCESS)
-        return result;
-
-    SwitchButtonData *buttonData = reinterpret_cast<SwitchButtonData *>(input_bytes);
-
-    *input_idx = 0;
+    if (size < sizeof(SwitchButtonData))
+        return CONTROLLER_STATUS_UNEXPECTED_DATA;
 
     if (buttonData->report_id != 0x30)
         return CONTROLLER_STATUS_NOTHING_TODO;
@@ -69,12 +66,33 @@ ControllerResult SwitchController::ReadRawInput(RawInputData *rawData, uint16_t
     rawData->buttons[18] = buttonData->button18;
     rawData->buttons[19] = buttonData->button19;
 
-    /*
-    rawData->X = BaseController::Normalize(buttonData->stick_left_x, -32768, 32767);
-    rawData->Y = BaseController::Normalize(-buttonData->stick_left_y, -32768, 32767);
-    rawData->Z = BaseController::Normalize(buttonData->stick_right_x, -32768, 32767);
-    rawData->Rz = BaseController::Normalize(-buttonData->stick_right_y, -32768, 32767);
-    */
+    uint16_t left_x = BaseController::ReadBitsLE(buttonData->stick_left, 0, 12);
+    uint16_t left_y = BaseController::ReadBitsLE(buttonData->stick_left, 12, 12);
+    uint16_t right_x = BaseController::ReadBitsLE(buttonData->stick_right, 0, 12);
+    uint16_t right_y = BaseController::ReadBitsLE(buttonData->stick_right, 12, 12);
+
+    cal_left_x.max = std::max(left_x, cal_left_x.max);
+    cal_left_x.min = std::min(left_x, cal_left_x.min);
+    cal_left_y.max = std::max(left_y, cal_left_y.max);
+    cal_left_y.min = std::min(left_y, cal_left_y.min);
+    cal_right_x.max = std::max(right_x, cal_right_x.max);
+    cal_right_x.min = std::min(right_x, cal_right_x.min);
+    cal_right_y.max = std::max(right_y, cal_right_y.max);
+    cal_right_y.min = std::min(right_y, cal_right_y.min);
+
+    Log(LogLevelTrace, "X=%u, Y=%u, Z=%u, Rz=%u (Calib: X=[%u,%u], Y=[%u,%u], Z=[%u,%u], Rz=[%u,%u])",
+        left_x, left_y, right_x, right_y,
+        cal_left_x.min, cal_left_x.max, cal_left_y.min, cal_left_y.max,
+        cal_right_x.min, cal_right_x.max, cal_right_y.min, cal_right_y.max);
+
+    rawData->X = BaseController::Normalize(left_x, cal_left_x.min, cal_left_x.max, 2000);
+    rawData->Y = BaseController::Normalize(left_y, cal_left_y.min, cal_left_y.max, 2000);
+    rawData->Z = BaseController::Normalize(right_x, cal_right_x.min, cal_right_x.max, 2000);
+    rawData->Rz = BaseController::Normalize(right_y, cal_right_y.min, cal_right_y.max, 2000);
+
+    rawData->Y = -rawData->Y;
+    rawData->Rz = -rawData->Rz;
+
     rawData->dpad_up = buttonData->dpad_up;
     rawData->dpad_right = buttonData->dpad_right;
     rawData->dpad_down = buttonData->dpad_down;

source/ControllerLib/Controllers/SwitchController.h

@@ -40,21 +40,31 @@ struct SwitchButtonData
     bool button18 : 1;
     bool button19 : 1;
 
-    int16_t stick_left_x;
-    int16_t stick_left_y;
-    int16_t stick_right_x;
-    int16_t stick_right_y;
+    uint8_t stick_left[3];
+    uint8_t stick_right[3];
+};
+
+struct SwitchCalibration
+{
+    uint16_t min;
+    uint16_t max;
 };
 
 class SwitchController : public BaseController
 {
+private:
+    SwitchCalibration cal_left_x;
+    SwitchCalibration cal_left_y;
+    SwitchCalibration cal_right_x;
+    SwitchCalibration cal_right_y;
+
 public:
     SwitchController(std::unique_ptr<IUSBDevice> &&device, const ControllerConfig &config, std::unique_ptr<ILogger> &&logger);
     virtual ~SwitchController() override;
 
     ControllerResult Initialize() override;
 
-    virtual ControllerResult ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us) override;
+    virtual ControllerResult ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx) override;
 
     bool Support(ControllerFeature feature) override;
 };

source/ControllerLib/Controllers/Xbox360Controller.cpp

@@ -22,22 +22,16 @@ ControllerResult Xbox360Controller::Initialize()
     return CONTROLLER_STATUS_SUCCESS;
 }
 
-ControllerResult Xbox360Controller::ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us)
+ControllerResult Xbox360Controller::ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx)
 {
-    uint8_t input_bytes[CONTROLLER_INPUT_BUFFER_SIZE];
-    size_t size = sizeof(input_bytes);
+    (void)input_idx;
+    Xbox360ButtonData *buttonData = reinterpret_cast<Xbox360ButtonData *>(buffer);
 
-    ControllerResult result = m_inPipe[0]->Read(input_bytes, &size, timeout_us);
-    if (result != CONTROLLER_STATUS_SUCCESS)
-        return result;
-
-    Xbox360ButtonData *buttonData = reinterpret_cast<Xbox360ButtonData *>(input_bytes);
-
-    *input_idx = 0;
+    if (size < sizeof(Xbox360ButtonData))
+        return CONTROLLER_STATUS_UNEXPECTED_DATA;
 
     if (buttonData->type == XBOX360INPUT_BUTTON) // Button data
     {
-
         rawData->buttons[1] = buttonData->button1;
         rawData->buttons[2] = buttonData->button2;
         rawData->buttons[3] = buttonData->button3;

source/ControllerLib/Controllers/Xbox360Controller.h

@@ -77,7 +77,7 @@ class Xbox360Controller : public BaseController
 
     ControllerResult Initialize() override;
 
-    ControllerResult ReadRawInput(RawInputData *rawData, uint16_t *input_idx, uint32_t timeout_us) override;
+    virtual ControllerResult ParseData(uint8_t *buffer, size_t size, RawInputData *rawData, uint16_t *input_idx) override;
 
     bool Support(ControllerFeature feature) override;