Can now control the LEDs and rumble on the Switch Pro controller

SwitchProBT.h

@@ -66,8 +66,10 @@ protected:
                 SwitchProParser::Reset();
 
                 // Only call this is a user function has not been set
-                if (!pFuncOnInit)
+                if (!pFuncOnInit) {
                         setLedOn(LED1); // Turn on the LED1
+                        setLedHomeOn(); // Turn on the home LED
+                }
         };
 
         /** Used to reset the different buffers to there default values */
@@ -77,12 +79,15 @@ protected:
         /**@}*/
 
         /** @name SwitchProParser implementation */
-        virtual void sendOutputReport(SwitchProOutput *output) {
+        virtual void sendOutputReport(uint8_t *data, uint8_t len) {
-                output->reportChanged = false;
+                uint8_t buf[1 /* BT DATA Output Report */ + len];
-#if 0
+
+                // Send as a Bluetooth HID DATA output report on the interrupt channel
+                buf[0] = 0xA2; // HID BT DATA (0xA0) | Report Type (Output 0x02)
+                memcpy(&buf[1], data, len);
+
                 // Send the Bluetooth DATA output report on the interrupt channel
                 pBtd->L2CAP_Command(hci_handle, buf, sizeof(buf), interrupt_scid[0], interrupt_scid[1]);
-#endif
         };
         /**@}*/
 };

SwitchProParser.cpp

@@ -84,8 +84,10 @@ void SwitchProParser::Parse(uint8_t len, uint8_t *buf) {
                 }
 #endif
 
-                if (buf[0] == 0x3F) // Check report ID
+                if (buf[0] == 0x3F) // Simple input report
                         memcpy(&switchProData, buf + 1, min((uint8_t)(len - 1), MFK_CASTUINT8T sizeof(switchProData)));
+                else if (buf[0] == 0x21) // Subcommand reply
+                        return;
                 else {
 #ifdef DEBUG_USB_HOST
                         Notify(PSTR("\r\nUnknown report id: "), 0x80);
@@ -125,8 +127,114 @@ void SwitchProParser::Parse(uint8_t len, uint8_t *buf) {
                 message_counter++;
         }
 
-        if (switchProOutput.reportChanged)
+        if (switchProOutput.ledReportChanged || switchProOutput.ledHomeReportChanged)
-                sendOutputReport(&switchProOutput); // Send output report
+                sendLedOutputReport(); // Send output report
+        else if (switchProOutput.leftRumbleOn || switchProOutput.rightRumbleOn) {
+                // We need to send the rumble report repeatedly to keep it on
+                uint32_t now = millis();
+                if  (now - rumble_on_timer > 1000) {
+                        rumble_on_timer = now;
+                        sendRumbleOutputReport();
+                }
+        } else
+                rumble_on_timer = 0;
+}
+
+void SwitchProParser::sendLedOutputReport() {
+        // See: https://github.com/Dan611/hid-procon
+        //      https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering
+        //      https://github.com/HisashiKato/USB_Host_Shield_Library_2.0_BTXBOX/blob/master/src/SWProBTParser.h#L152-L153
+        uint8_t buf[14] = { 0 };
+        buf[0x00] = 0x01; // Report ID - PROCON_CMD_AND_RUMBLE
+        buf[0x01] = output_sequence_counter++; // Lowest 4-bit is a sequence number, which needs to be increased for every report
+
+        // Left rumble data
+        if (switchProOutput.leftRumbleOn) {
+                buf[0x02 + 0] = 0x28;
+                buf[0x02 + 1] = 0x88;
+                buf[0x02 + 2] = 0x60;
+                buf[0x02 + 3] = 0x61;
+        } else {
+                buf[0x02 + 0] = 0x00;
+                buf[0x02 + 1] = 0x01;
+                buf[0x02 + 2] = 0x40;
+                buf[0x02 + 3] = 0x40;
+        }
+
+        // Right rumble data
+        if (switchProOutput.rightRumbleOn) {
+                buf[0x02 + 4] = 0x28;
+                buf[0x02 + 5] = 0x88;
+                buf[0x02 + 6] = 0x60;
+                buf[0x02 + 7] = 0x61;
+        } else {
+                buf[0x02 + 4] = 0x00;
+                buf[0x02 + 5] = 0x01;
+                buf[0x02 + 6] = 0x40;
+                buf[0x02 + 7] = 0x40;
+        }
+
+        // Sub commands
+        if (switchProOutput.ledReportChanged) {
+                switchProOutput.ledReportChanged = false;
+
+                // See: https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/bluetooth_hid_subcommands_notes.md#subcommand-0x30-set-player-lights
+                buf[0x0A + 0] = 0x30; // PROCON_CMD_LED
+
+                buf[0x0A + 1] = switchProOutput.ledMask; // Lower 4-bits sets the LEDs constantly on, the higher 4-bits can be used to flash the LEDs
+
+                sendOutputReport(buf, 10 + 2);
+        } else if (switchProOutput.ledHomeReportChanged) {
+                 switchProOutput.ledHomeReportChanged = false;
+
+                // It is possible set up to 15 mini cycles, but we simply just set the LED constantly on/off
+                // See: https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/bluetooth_hid_subcommands_notes.md#subcommand-0x38-set-home-light
+                buf[0x0A + 0] = 0x38; // PROCON_CMD_LED_HOME
+
+                buf[0x0A + 1] = (0 /* Number of cycles */ << 4) | (switchProOutput.ledHome ? 0xF : 0) /* Global mini cycle duration */;
+                buf[0x0A + 2] = (0xF /* LED start intensity */ << 4) | 0x0 /* Number of full cycles */;
+                buf[0x0A + 3] = (0xF /* Mini Cycle 1 LED intensity */ << 4) | 0x0 /* Mini Cycle 2 LED intensity */;
+
+                sendOutputReport(buf, 10 + 4);
+        }
+}
+
+void SwitchProParser::sendRumbleOutputReport() {
+        // See: https://github.com/Dan611/hid-procon
+        //      https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering
+        //      https://github.com/HisashiKato/USB_Host_Shield_Library_2.0_BTXBOX/blob/master/src/SWProBTParser.h#L152-L153
+
+        uint8_t buf[10] = { 0 };
+        buf[0x00] = 0x10; // Report ID - PROCON_CMD_RUMBLE_ONLY
+        buf[0x01] = output_sequence_counter++; // Lowest 4-bit is a sequence number, which needs to be increased for every report
+
+        // Left rumble data
+        if (switchProOutput.leftRumbleOn) {
+                buf[0x02 + 0] = 0x28;
+                buf[0x02 + 1] = 0x88;
+                buf[0x02 + 2] = 0x60;
+                buf[0x02 + 3] = 0x61;
+        } else {
+                buf[0x02 + 0] = 0x00;
+                buf[0x02 + 1] = 0x01;
+                buf[0x02 + 2] = 0x40;
+                buf[0x02 + 3] = 0x40;
+        }
+
+        // Right rumble data
+        if (switchProOutput.rightRumbleOn) {
+                buf[0x02 + 4] = 0x28;
+                buf[0x02 + 5] = 0x88;
+                buf[0x02 + 6] = 0x60;
+                buf[0x02 + 7] = 0x61;
+        } else {
+                buf[0x02 + 4] = 0x00;
+                buf[0x02 + 5] = 0x01;
+                buf[0x02 + 6] = 0x40;
+                buf[0x02 + 7] = 0x40;
+        }
+
+        sendOutputReport(buf, 10);
 }
 
 void SwitchProParser::Reset() {
@@ -139,13 +247,13 @@ void SwitchProParser::Reset() {
         oldButtonState.dpad = DPAD_OFF;
         buttonClickState.dpad = 0;
         oldDpad = 0;
+        output_sequence_counter = 0;
+        rumble_on_timer = 0;
 
-#if 0
+        switchProOutput.leftRumbleOn = false;
-        ps5Output.bigRumble = ps5Output.smallRumble = 0;
+        switchProOutput.rightRumbleOn = false;
-        ps5Output.microphoneLed = 0;
+        switchProOutput.ledMask = 0;
-        ps5Output.disableLeds = 0;
+        switchProOutput.ledHome = false;
-        ps5Output.playerLeds = 0;
+        switchProOutput.ledReportChanged = false;
-        ps5Output.r = ps5Output.g = ps5Output.b = 0;
+        switchProOutput.ledHomeReportChanged = false;
-#endif
-        switchProOutput.reportChanged = false;
 };

SwitchProParser.h

@@ -21,6 +21,22 @@
 #include "Usb.h"
 #include "controllerEnums.h"
 
+/** Used to set the LEDs on the controller */
+const uint8_t SWITCH_PRO_LEDS[] PROGMEM = {
+        0x00, // OFF
+        0x01, // LED1
+        0x02, // LED2
+        0x04, // LED3
+        0x08, // LED4
+
+        0x09, // LED5
+        0x0A, // LED6
+        0x0C, // LED7
+        0x0D, // LED8
+        0x0E, // LED9
+        0x0F, // LED10
+};
+
 /** Buttons on the controller */
 const uint8_t SWITCH_PRO_BUTTONS[] PROGMEM = {
         0x10, // UP
@@ -82,14 +98,21 @@ struct SwitchProData {
 } __attribute__((packed));
 
 struct SwitchProOutput {
-        bool reportChanged; // The data is send when data is received from the controller
+        bool leftRumbleOn;
+        bool rightRumbleOn;
+        uint8_t ledMask; // Higher nibble flashes the LEDs, lower nibble set them on/off
+        bool ledHome;
+
+        // Used to only send the report when the state changes
+        bool ledReportChanged;
+        bool ledHomeReportChanged;
 } __attribute__((packed));
 
 /** This class parses all the data sent by the Switch Pro controller */
 class SwitchProParser {
 public:
         /** Constructor for the SwitchProParser class. */
-        SwitchProParser() {
+        SwitchProParser() : output_sequence_counter(0) {
                 Reset();
         };
 
@@ -111,7 +134,7 @@ public:
         /**
          * Used to read the analog joystick.
          * @param  a ::LeftHatX, ::LeftHatY, ::RightHatX, and ::RightHatY.
-         * @return   Return the analog value in the range of 0-255.
+         * @return   Return the analog value as a signed 16-bit value.
          */
         int16_t getAnalogHat(AnalogHatEnum a);
 
@@ -157,6 +180,7 @@ public:
         void setAllOff() {
                 setRumbleOff();
                 setLedOff();
+                setLedHomeOff();
         };
 
         /** Set rumble off. */
@@ -166,73 +190,78 @@ public:
 
         /** Toggle rumble. */
         void setRumbleToggle() {
-                // TODO: Implement this
+                setRumbleOn(!switchProOutput.leftRumbleOn, !switchProOutput.rightRumbleOn);
-                switchProOutput.reportChanged = true;
         }
 
         /**
          * Turn on rumble.
-         * @param mode Either ::RumbleHigh or ::RumbleLow.
+         * @param leftRumbleOn   Turn on left rumble motor.
+         * @param rightRumbleOn  Turn on right rumble motor.
          */
-        void setRumbleOn(RumbleEnum mode) {
+        void setRumbleOn(bool leftRumbleOn, bool rightRumbleOn) {
-                if (mode == RumbleLow)
+                switchProOutput.leftRumbleOn = leftRumbleOn;
-                        setRumbleOn(0x00, 0xFF);
+                switchProOutput.rightRumbleOn = rightRumbleOn;
-                else
+                switchProOutput.ledReportChanged = true; // Set this, so the rumble effect gets changed immediately
-                        setRumbleOn(0xFF, 0x00);
-        }
-
-        /**
-         * Turn on rumble.
-         * @param bigRumble   Value for big motor.
-         * @param smallRumble Value for small motor.
-         */
-        void setRumbleOn(uint8_t bigRumble, uint8_t smallRumble) {
-                // TODO: Implement this
-                (void)bigRumble;
-                (void)smallRumble;
-                switchProOutput.reportChanged = true;
         }
 
         /**
          * Set LED value without using the ::LEDEnum.
+         * This can also be used to flash the LEDs by setting the high 4-bits of the mask.
          * @param value See: ::LEDEnum.
          */
-        void setLedRaw(uint8_t value) {
+        void setLedRaw(uint8_t mask) {
-                // TODO: Implement this
+                switchProOutput.ledMask = mask;
-                (void)value;
+                switchProOutput.ledReportChanged = true;
-                switchProOutput.reportChanged = true;
         }
 
         /** Turn all LEDs off. */
         void setLedOff() {
                 setLedRaw(0);
         }
+
         /**
          * Turn the specific ::LEDEnum off.
          * @param a The ::LEDEnum to turn off.
          */
         void setLedOff(LEDEnum a) {
-                // TODO: Implement this
+                switchProOutput.ledMask &= ~((uint8_t)(pgm_read_byte(&SWITCH_PRO_LEDS[(uint8_t)a]) & 0x0f));
-                (void)a;
+                switchProOutput.ledReportChanged = true;
-                switchProOutput.reportChanged = true;
         }
+
         /**
          * Turn the specific ::LEDEnum on.
          * @param a The ::LEDEnum to turn on.
          */
         void setLedOn(LEDEnum a) {
-                // TODO: Implement this
+                switchProOutput.ledMask |= (uint8_t)(pgm_read_byte(&SWITCH_PRO_LEDS[(uint8_t)a]) & 0x0f);
-                (void)a;
+                switchProOutput.ledReportChanged = true;
-                switchProOutput.reportChanged = true;
         }
+
         /**
          * Toggle the specific ::LEDEnum.
          * @param a The ::LEDEnum to toggle.
          */
         void setLedToggle(LEDEnum a) {
-                // TODO: Implement this
+                switchProOutput.ledMask ^= (uint8_t)(pgm_read_byte(&SWITCH_PRO_LEDS[(uint8_t)a]) & 0x0f);
-                (void)a;
+                switchProOutput.ledReportChanged = true;
-                switchProOutput.reportChanged = true;
+        }
+
+        /** Turn home LED off. */
+        void setLedHomeOff() {
+                switchProOutput.ledHome = false;
+                switchProOutput.ledHomeReportChanged = true;
+        }
+
+        /** Turn home LED on. */
+        void setLedHomeOn() {
+                switchProOutput.ledHome = true;
+                switchProOutput.ledHomeReportChanged = true;
+        }
+
+        /** Toggle home LED. */
+        void setLedHomeToggle() {
+                switchProOutput.ledHome = !switchProOutput.ledHome;
+                switchProOutput.ledHomeReportChanged = true;
         }
 
         /** Get the incoming message count. */
@@ -253,19 +282,24 @@ protected:
 
         /**
          * Send the output to the Switch Pro controller. This is implemented in SwitchProBT.h and SwitchProUSB.h.
-         * @param output Pointer to SwitchProOutput buffer;
+         * @param data  Pointer to buffer to send by the derived class.
+         * @param len   Length of buffer.
          */
-        virtual void sendOutputReport(SwitchProOutput *output) = 0;
+        virtual void sendOutputReport(uint8_t *data, uint8_t len) = 0;
-
 
 private:
         static int8_t getButtonIndexSwitchPro(ButtonEnum b);
         bool checkDpad(ButtonEnum b); // Used to check Switch Pro DPAD buttons
 
+        void sendLedOutputReport();
+        void sendRumbleOutputReport();
+
         SwitchProData switchProData;
         SwitchProButtons oldButtonState, buttonClickState;
         SwitchProOutput switchProOutput;
         uint8_t oldDpad;
         uint16_t message_counter = 0;
+        uint8_t output_sequence_counter : 4;
+        uint32_t rumble_on_timer = 0;
 };
 #endif

examples/Bluetooth/SwitchPro/SwitchPro.ino

@@ -26,7 +26,7 @@ SwitchProBT SwitchPro(&Btd, PAIR);
 //SwitchProBT SwitchPro(&Btd);
 
 uint16_t lastMessageCounter = -1;
-uint32_t home_timer;
+uint32_t capture_timer;
 
 void setup() {
   Serial.begin(115200);
@@ -60,55 +60,62 @@ void loop() {
       Serial.print(SwitchPro.getAnalogHat(RightHatY));
     }
 
-    // Hold the HOME button for 1 second to disconnect the controller
+    // Hold the CAPTURE button for 1 second to disconnect the controller
     // This prevents the controller from disconnecting when it is reconnected,
-    // as the HOME button is sent when it reconnects
+    // as the CAPTURE button is sent when it reconnects
-    if (SwitchPro.getButtonPress(HOME)) {
+    if (SwitchPro.getButtonPress(CAPTURE)) {
-      if (millis() - home_timer > 1000)
+      if (millis() - capture_timer > 1000)
         SwitchPro.disconnect();
     } else
-      home_timer = millis();
+      capture_timer = millis();
 
-    if (SwitchPro.getButtonClick(HOME))
-      Serial.print(F("\r\nHome"));
     if (SwitchPro.getButtonClick(CAPTURE))
       Serial.print(F("\r\nCapture"));
+    if (SwitchPro.getButtonClick(HOME)) {
+      Serial.print(F("\r\nHome"));
+      SwitchPro.setLedHomeToggle(); // Toggle the home LED
+    }
+
     if (SwitchPro.getButtonClick(LEFT)) {
-      /*SwitchPro.setLedOff();
+      SwitchPro.setLedOff();
-      SwitchPro.setLedOn(LED1);*/
+      SwitchPro.setLedOn(LED1);
       Serial.print(F("\r\nLeft"));
     }
+    if (SwitchPro.getButtonClick(UP)) {
+      SwitchPro.setLedOff();
+      SwitchPro.setLedOn(LED2);
+      Serial.print(F("\r\nUp"));
+    }
     if (SwitchPro.getButtonClick(RIGHT)) {
-      /*SwitchPro.setLedOff();
+      SwitchPro.setLedOff();
-      SwitchPro.setLedOn(LED3);*/
+      SwitchPro.setLedOn(LED3);
       Serial.print(F("\r\nRight"));
     }
     if (SwitchPro.getButtonClick(DOWN)) {
-      /*SwitchPro.setLedOff();
+      SwitchPro.setLedOff();
-      SwitchPro.setLedOn(LED4);*/
+      SwitchPro.setLedOn(LED4);
       Serial.print(F("\r\nDown"));
     }
-    if (SwitchPro.getButtonClick(UP)) {
-      /*SwitchPro.setLedOff();
-      SwitchPro.setLedOn(LED2);*/
-      Serial.print(F("\r\nUp"));
-    }
 
     if (SwitchPro.getButtonClick(PLUS))
       Serial.print(F("\r\nPlus"));
     if (SwitchPro.getButtonClick(MINUS))
       Serial.print(F("\r\nMinus"));
 
-    if (SwitchPro.getButtonClick(A))
+    if (SwitchPro.getButtonClick(A)) {
+      SwitchPro.setRumbleOn(false, true); // Turn on the right rumble motor
       Serial.print(F("\r\nA"));
+    }
     if (SwitchPro.getButtonClick(B)) {
-      //SwitchPro.setRumbleToggle();
+      SwitchPro.setRumbleOn(true, false); // Turn on the left rumble motor
       Serial.print(F("\r\nB"));
     }
     if (SwitchPro.getButtonClick(X))
       Serial.print(F("\r\nX"));
-    if (SwitchPro.getButtonClick(Y))
+    if (SwitchPro.getButtonClick(Y)) {
+      SwitchPro.setRumbleOn(false, false);
       Serial.print(F("\r\nY"));
+    }
 
     if (SwitchPro.getButtonClick(L))
       Serial.print(F("\r\nL"));

keywords.txt

@@ -71,6 +71,10 @@ setLedFlash	KEYWORD2
 moveSetBulb	KEYWORD2
 moveSetRumble	KEYWORD2
 
+setLedHomeOff	KEYWORD2
+setLedHomeOn	KEYWORD2
+setLedHomeToggle	KEYWORD2
+
 attachOnInit	KEYWORD2
 
 PS3Connected	KEYWORD2