From ec8890d40ee888fedd84a955862ec6126d3edd1d Mon Sep 17 00:00:00 2001
From: Kristian Sloth Lauszus <lauszus@gmail.com>
Date: Sun, 9 May 2021 21:07:14 +0200
Subject: [PATCH] Can now also parse the battery level and charging status

---
 SwitchProParser.cpp                           |  6 ++---
 SwitchProParser.h                             | 25 ++++++++++++++++---
 .../Bluetooth/SwitchProBT/SwitchProBT.ino     |  7 +++++-
 examples/SwitchProUSB/SwitchProUSB.ino        |  7 +++++-
 4 files changed, 37 insertions(+), 8 deletions(-)

diff --git a/SwitchProParser.cpp b/SwitchProParser.cpp
index 10c9155a..61704cc1 100644
--- a/SwitchProParser.cpp
+++ b/SwitchProParser.cpp
@@ -71,14 +71,14 @@ void SwitchProParser::Parse(uint8_t len, uint8_t *buf) {
                 if (buf[0] == 0x3F) // Simple input report via Bluetooth
                         switchProOutput.enableFullReportMode = true; // Switch over to the full report
                 else if (buf[0] == 0x30) { // Standard full mode
-                        if (len < 4) {
+                        if (len < 3) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nReport is too short: "), 0x80);
                                 D_PrintHex<uint8_t > (len, 0x80);
 #endif
                                 return;
                         }
-                        memcpy(&switchProData, buf + 3, min((uint8_t)(len - 3), MFK_CASTUINT8T sizeof(switchProData)));
+                        memcpy(&switchProData, buf + 2, min((uint8_t)(len - 2), MFK_CASTUINT8T sizeof(switchProData)));
 
                         if (switchProData.btn.val != oldButtonState.val) { // Check if anything has changed
                                 buttonClickState.val = switchProData.btn.val & ~oldButtonState.val; // Update click state variable
@@ -250,4 +250,4 @@ void SwitchProParser::Reset() {
         switchProOutput.enableImu = -1;
         switchProOutput.sendHandshake = false;
         switchProOutput.disableTimeout = false;
-};
+}
diff --git a/SwitchProParser.h b/SwitchProParser.h
index 72d1fcb7..9eebf405 100644
--- a/SwitchProParser.h
+++ b/SwitchProParser.h
@@ -100,7 +100,10 @@ struct ImuData {
 } __attribute__((packed));
 
 struct SwitchProData {
-        // TODO: Add byte 2 containing battery level and connection info
+        struct {
+                uint8_t connection_info : 4;
+                uint8_t battery_level : 4;
+        } __attribute__((packed));
 
         /* Button and joystick values */
         SwitchProButtons btn; // Bytes 3-5
@@ -316,10 +319,27 @@ public:
         }
 
         /** Get the incoming message count. */
-        uint16_t getMessageCounter(){
+        uint16_t getMessageCounter() {
                 return message_counter;
         }
 
+        /**
+        * Return the battery level of the Switch Pro Controller.
+        * @return The battery level as a bit mask according to ::SwitchProBatteryLevel:
+         *        4=full, 3=medium, 2=low, 1=critical, 0=empty.
+        */
+        uint8_t getBatteryLevel() {
+                return switchProData.battery_level >> 1;
+        }
+
+        /**
+         * Returns whenever the controller is plugged in and charging.
+         * @return Returns True if the controller is charging.
+         */
+        bool isCharging() {
+                return switchProData.battery_level & 0x01;
+        }
+
 protected:
         /**
          * Used to parse data sent from the Switch Pro controller.
@@ -352,7 +372,6 @@ protected:
 
 private:
         static int8_t getButtonIndexSwitchPro(ButtonEnum b);
-        bool checkDpad(ButtonEnum b); // Used to check Switch Pro DPAD buttons
 
         void sendOutputCmd();
         void sendRumbleOutputReport();
diff --git a/examples/Bluetooth/SwitchProBT/SwitchProBT.ino b/examples/Bluetooth/SwitchProBT/SwitchProBT.ino
index b21f693c..7e53c1eb 100644
--- a/examples/Bluetooth/SwitchProBT/SwitchProBT.ino
+++ b/examples/Bluetooth/SwitchProBT/SwitchProBT.ino
@@ -109,8 +109,13 @@ void loop() {
 
     if (SwitchPro.getButtonClick(A))
       Serial.print(F("\r\nA"));
-    if (SwitchPro.getButtonClick(B))
+    if (SwitchPro.getButtonClick(B)) {
       Serial.print(F("\r\nB"));
+      Serial.print(F("\r\nBattery level: "));
+      Serial.print(SwitchPro.getBatteryLevel());
+      Serial.print(F(", charging: "));
+      Serial.print(SwitchPro.isCharging());
+    }
     if (SwitchPro.getButtonClick(X))
       Serial.print(F("\r\nX"));
     if (SwitchPro.getButtonClick(Y))
diff --git a/examples/SwitchProUSB/SwitchProUSB.ino b/examples/SwitchProUSB/SwitchProUSB.ino
index b2b74d43..842e6394 100644
--- a/examples/SwitchProUSB/SwitchProUSB.ino
+++ b/examples/SwitchProUSB/SwitchProUSB.ino
@@ -90,8 +90,13 @@ void loop() {
 
     if (SwitchPro.getButtonClick(A))
       Serial.print(F("\r\nA"));
-    if (SwitchPro.getButtonClick(B))
+    if (SwitchPro.getButtonClick(B)) {
       Serial.print(F("\r\nB"));
+      Serial.print(F("\r\nBattery level: "));
+      Serial.print(SwitchPro.getBatteryLevel());
+      Serial.print(F(", charging: "));
+      Serial.print(SwitchPro.isCharging());
+    }
     if (SwitchPro.getButtonClick(X))
       Serial.print(F("\r\nX"));
     if (SwitchPro.getButtonClick(Y))