From 3915e96962e8a71334d90da1119f5a65d3907e4c Mon Sep 17 00:00:00 2001
From: Kristian Sloth Lauszus <lauszus@gmail.com>
Date: Sun, 24 Jan 2021 14:33:44 +0100
Subject: [PATCH] Added support for rumble functionality on the Xbox One S
 controller via Bluetooth

---
 XBOXONESBT.h                            | 17 +++--------------
 XBOXONESParser.cpp                      | 18 +++++++++++++++---
 XBOXONESParser.h                        |  6 +++---
 examples/Xbox/XBOXONESBT/XBOXONESBT.ino |  2 --
 4 files changed, 21 insertions(+), 22 deletions(-)

diff --git a/XBOXONESBT.h b/XBOXONESBT.h
index 58395790..53f81c7a 100644
--- a/XBOXONESBT.h
+++ b/XBOXONESBT.h
@@ -74,28 +74,17 @@ protected:
         };
         /**@}*/
 
-#if 0
         /** @name XBOXONESParser implementation */
         virtual void sendOutputReport(uint8_t *data, uint8_t nbytes) {
                 // See: https://lore.kernel.org/patchwork/patch/973394/
                 uint8_t buf[nbytes + 2];
-
-                buf[0] = 0x52; // HID BT Set_report (0x50) | Report Type (Output 0x02)
+                buf[0] = 0xA2; // HID BT DATA (0xA0) | Report Type (Output 0x02)
                 buf[1] = 0x03; // Report ID
-
                 memcpy(buf + 2, data, nbytes);
 
-                Notify(PSTR("\r\n"), 0x80);
-                for (uint8_t i = 0; i < sizeof(buf); i++) {
-                        D_PrintHex<uint8_t > (buf[i], 0x80);
-                        Notify(PSTR(" "), 0x80);
-                }
-                Notify(PSTR("\r\n"), 0x80);
-
-                //pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]);
-                pBtd->L2CAP_Command(hci_handle, buf, sizeof(buf), control_scid[0], control_scid[1]);
+                // 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
 };
 #endif
diff --git a/XBOXONESParser.cpp b/XBOXONESParser.cpp
index e8f0064f..9e5edd59 100644
--- a/XBOXONESParser.cpp
+++ b/XBOXONESParser.cpp
@@ -183,11 +183,24 @@ void XBOXONESParser::Reset() {
         oldDpad = 0;
 };
 
-#if 0
+void XBOXONESParser::setRumbleOff() {
+        // See: https://lore.kernel.org/patchwork/patch/973394/
+        uint8_t buf[8];
+        buf[0] = 0x0F; // Disable all rumble motors
+        buf[1] = 0;
+        buf[2] = 0;
+        buf[3] = 0;
+        buf[4] = 0;
+        buf[5] = 0; // Duration of effect in 10 ms
+        buf[6] = 0; // Start delay in 10 ms
+        buf[7] = 0; // Loop count
+        sendOutputReport(buf, sizeof(buf));
+}
+
 void XBOXONESParser::setRumbleOn(uint8_t leftTrigger, uint8_t rightTrigger, uint8_t leftMotor, uint8_t rightMotor) {
         // See: https://lore.kernel.org/patchwork/patch/973394/
         uint8_t buf[8];
-        buf[0] = 0x0F;//1 << 1 | 1 << 0; // Enable weak and strong feedback
+        buf[0] = 1 << 3 /* Left trigger */ | 1 << 2 /* Right trigger */ | 1 << 1 /* Left motor */ | 1 << 0 /* Right motor */;
         buf[1] = leftTrigger;
         buf[2] = rightTrigger;
         buf[3] = leftMotor;
@@ -197,4 +210,3 @@ void XBOXONESParser::setRumbleOn(uint8_t leftTrigger, uint8_t rightTrigger, uint
         buf[7] = 255; // Loop count
         sendOutputReport(buf, sizeof(buf));
 }
-#endif
diff --git a/XBOXONESParser.h b/XBOXONESParser.h
index 41dc30b2..d031631a 100644
--- a/XBOXONESParser.h
+++ b/XBOXONESParser.h
@@ -81,7 +81,7 @@ public:
         int16_t getAnalogHat(AnalogHatEnum a);
 
         /** Used to set the rumble off. */
-        //void setRumbleOff();
+        void setRumbleOff();
 
         /**
          * Used to turn on rumble continuously.
@@ -90,7 +90,7 @@ public:
          * @param leftMotor    Left motor force.
          * @param rightMotor   Right motor force.
          */
-        //void setRumbleOn(uint8_t leftTrigger, uint8_t rightTrigger, uint8_t leftMotor, uint8_t rightMotor);
+        void setRumbleOn(uint8_t leftTrigger, uint8_t rightTrigger, uint8_t leftMotor, uint8_t rightMotor);
 
 protected:
         /**
@@ -108,7 +108,7 @@ protected:
          * @param output Pointer to data buffer.
          * @param nbytes Bytes to send.
          */
-        //virtual void sendOutputReport(uint8_t *data, uint8_t nbytes) = 0;
+        virtual void sendOutputReport(uint8_t *data, uint8_t nbytes) = 0;
 
 private:
         bool checkDpad(ButtonEnum b); // Used to check Xbox One S DPAD buttons
diff --git a/examples/Xbox/XBOXONESBT/XBOXONESBT.ino b/examples/Xbox/XBOXONESBT/XBOXONESBT.ino
index c691e9bf..e7d6c419 100644
--- a/examples/Xbox/XBOXONESBT/XBOXONESBT.ino
+++ b/examples/Xbox/XBOXONESBT/XBOXONESBT.ino
@@ -77,7 +77,6 @@ void loop() {
       Serial.println();
     }
 
-#if 0 // This is currently not working
     // Set rumble effect
     static uint16_t oldL2Value, oldR2Value;
     if (Xbox.getButtonPress(L2) != oldL2Value || Xbox.getButtonPress(R2) != oldR2Value) {
@@ -90,7 +89,6 @@ void loop() {
       else
         Xbox.setRumbleOff();
     }
-#endif
 
     if (Xbox.getButtonClick(UP))
       Serial.println(F("Up"));