From d480d034a5be751c24f8c8159f82fb6153cbc629 Mon Sep 17 00:00:00 2001
From: Solidgeek <thesolidgeek@gmail.com>
Date: Sun, 18 Dec 2022 22:18:56 +0100
Subject: [PATCH 01/13] Minor fixes to processReadPacket

---
 src/VescUart.cpp | 12 +++++-------
 src/VescUart.h   | 20 ++++++++++----------
 2 files changed, 15 insertions(+), 17 deletions(-)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index 4d25e18..d4768bc 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -188,10 +188,11 @@ bool VescUart::processReadPacket(uint8_t * message) {
 
 	switch (packetId){
 		case COMM_FW_VERSION: // Structure defined here: https://github.com/vedderb/bldc/blob/43c3bbaf91f5052a35b75c2ff17b5fe99fad94d1/commands.c#L164
-
 			fw_version.major = message[index++];
 			fw_version.minor = message[index++];
-			return true;
+			
+            return true;
+        break;
 		case COMM_GET_VALUES: // Structure defined here: https://github.com/vedderb/bldc/blob/43c3bbaf91f5052a35b75c2ff17b5fe99fad94d1/commands.c#L164
 
 			data.tempMosfet 		= buffer_get_float16(message, 10.0, &index); 	// 2 bytes - mc_interface_temp_fet_filtered()
@@ -214,17 +215,14 @@ bool VescUart::processReadPacket(uint8_t * message) {
 			data.id					= message[index++];								// 1 byte  - app_get_configuration()->controller_id	
 
 			return true;
-
 		break;
 
-		/* case COMM_GET_VALUES_SELECTIVE:
-
-			uint32_t mask = 0xFFFFFFFF; */
-
 		default:
 			return false;
 		break;
 	}
+
+    return false;
 }
 
 bool VescUart::getFWversion(void){
diff --git a/src/VescUart.h b/src/VescUart.h
index dd8492b..03ecc0f 100644
--- a/src/VescUart.h
+++ b/src/VescUart.h
@@ -10,8 +10,8 @@ class VescUart
 {
 
 	/** Struct to store the telemetry data returned by the VESC */
-	struct dataPackage {
-       float avgMotorCurrent;
+	typedef struct {
+        float avgMotorCurrent;
         float avgInputCurrent;
         float dutyCycleNow;
         float rpm;
@@ -27,20 +27,20 @@ class VescUart
         float pidPos;
         uint8_t id;
         mc_fault_code error; 
-	};
+	} vesc_telemetry_t;
 
 	/** Struct to hold the nunchuck values to send over UART */
-	struct nunchuckPackage {
+	typedef struct  {
 		int	valueX;
 		int	valueY;
 		bool upperButton; // valUpperButton
 		bool lowerButton; // valLowerButton
-	};
+	} vesc_nunchuck_t;
 
-    struct FWversionPackage {
+    typedef struct {
         uint8_t major;
         uint8_t minor;
-    };
+    } vesc_firmware_t ;
 
 	//Timeout - specifies how long the function will wait for the vesc to respond
 	const uint32_t _TIMEOUT;
@@ -52,13 +52,13 @@ class VescUart
 		VescUart(uint32_t timeout_ms = 100);
 
 		/** Variabel to hold measurements returned from VESC */
-		dataPackage data; 
+		vesc_telemetry_t data; 
 
 		/** Variabel to hold nunchuck values */
-		nunchuckPackage nunchuck; 
+		vesc_nunchuck_t nunchuck; 
 
        /** Variable to hold firmware version */
-        FWversionPackage fw_version; 
+        vesc_firmware_t fw_version; 
 
         /**
          * @brief      Set the serial port for uart communication

From 67d793577991836c14a0713cff924dbd7d51503e Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Tue, 27 Feb 2024 15:42:26 -0500
Subject: [PATCH 02/13] Create a first attempt at collecting IMU data

---
 src/VescUart.cpp | 25 +++++++++++++++++++++++++
 src/VescUart.h   |  9 +++++++++
 2 files changed, 34 insertions(+)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index 4d25e18..c871568 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -217,6 +217,22 @@ bool VescUart::processReadPacket(uint8_t * message) {
 
 		break;
 
+		case COMM_GET_IMU_DATA: // Structure defined here: https://github.com/vedderb/bldc/blob/31a313129ded1d355a921c572854820921d3030a/commands.c#L995
+
+			data.imuRoll			= buffer_get_float32(message, 1.0, &index);
+			data.imuPitch			= buffer_get_float32(message, 1.0, &index);
+			data.imuYaw 			= buffer_get_float32(message, 1.0, &index);
+			data.accX				= buffer_get_float32(message, 1.0, &index);
+			data.accY				= buffer_get_float32(message, 1.0, &index);
+			data.accZ				= buffer_get_float32(message, 1.0, &index);
+			data.gyroX				= buffer_get_float32(message, 1.0, &index);
+			data.gyroY				= buffer_get_float32(message, 1.0, &index);
+			data.gyroZ				= buffer_get_float32(message, 1.0, &index);
+
+			return true;
+
+		break;
+
 		/* case COMM_GET_VALUES_SELECTIVE:
 
 			uint32_t mask = 0xFFFFFFFF; */
@@ -436,5 +452,14 @@ void VescUart::printVescValues() {
 		debugPort->print("tempMosfet: "); 		debugPort->println(data.tempMosfet);
 		debugPort->print("tempMotor: "); 		debugPort->println(data.tempMotor);
 		debugPort->print("error: "); 			debugPort->println(data.error);
+		debugPort->print("imuRoll: "); 			debugPort->println(data.imuRoll);
+		debugPort->print("imuPitch: "); 		debugPort->println(data.imuPitch);
+		debugPort->print("imuYaw: "); 			debugPort->println(data.imuYaw);
+		debugPort->print("accX: "); 			debugPort->println(data.accX);
+		debugPort->print("accY: "); 			debugPort->println(data.accY);
+		debugPort->print("accZ: "); 			debugPort->println(data.accZ);
+		debugPort->print("gyroX: "); 			debugPort->println(data.gyroX);
+		debugPort->print("gyroY: "); 			debugPort->println(data.gyroY);
+		debugPort->print("gyroZ: "); 			debugPort->println(data.gyroZ);
 	}
 }
diff --git a/src/VescUart.h b/src/VescUart.h
index dd8492b..27ae865 100644
--- a/src/VescUart.h
+++ b/src/VescUart.h
@@ -27,6 +27,15 @@ class VescUart
         float pidPos;
         uint8_t id;
         mc_fault_code error; 
+        float imuRoll;
+        float imuPitch;
+        float imuYaw;
+        float accX;
+        float accY;
+        float accZ;
+        float gyroX;
+        float gyroY;
+        float gyroZ; 
 	};
 
 	/** Struct to hold the nunchuck values to send over UART */

From 3a68ebd0c8e06dc1898249a0deb223bd26c6dbea Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Thu, 29 Feb 2024 12:04:22 -0500
Subject: [PATCH 03/13] Fix some typos

---
 src/VescUart.cpp |  2 +-
 src/VescUart.h   | 14 +++++++-------
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index c871568..e2262c9 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -59,7 +59,7 @@ int VescUart::receiveUartMessage(uint8_t * payloadReceived) {
 
 					default:
 						if( debugPort != NULL ){
-							debugPort->println("Unvalid start bit");
+							debugPort->println("Invalid start bit");
 						}
 					break;
 				}
diff --git a/src/VescUart.h b/src/VescUart.h
index 27ae865..4a63740 100644
--- a/src/VescUart.h
+++ b/src/VescUart.h
@@ -60,10 +60,10 @@ class VescUart
 		 */
 		VescUart(uint32_t timeout_ms = 100);
 
-		/** Variabel to hold measurements returned from VESC */
+		/** Variable to hold measurements returned from VESC */
 		dataPackage data; 
 
-		/** Variabel to hold nunchuck values */
+		/** Variable to hold nunchuck values */
 		nunchuckPackage nunchuck; 
 
        /** Variable to hold firmware version */
@@ -192,10 +192,10 @@ class VescUart
 
 	private: 
 
-		/** Variabel to hold the reference to the Serial object to use for UART */
+		/** Variable to hold the reference to the Serial object to use for UART */
 		Stream* serialPort = NULL;
 
-		/** Variabel to hold the reference to the Serial object to use for debugging. 
+		/** Variable to hold the reference to the Serial object to use for debugging. 
 		  * Uses the class Stream instead of HarwareSerial */
 		Stream* debugPort = NULL;
 
@@ -212,7 +212,7 @@ class VescUart
 		 * @brief      Receives the message over Serial
 		 *
 		 * @param      payloadReceived  - The received payload as a unit8_t Array
-		 * @return     The number of bytes receeived within the payload
+		 * @return     The number of bytes received within the payload
 		 */
 		int receiveUartMessage(uint8_t * payloadReceived);
 
@@ -220,7 +220,7 @@ class VescUart
 		 * @brief      Verifies the message (CRC-16) and extracts the payload
 		 *
 		 * @param      message  - The received UART message
-		 * @param      lenMes   - The lenght of the message
+		 * @param      lenMes   - The length of the message
 		 * @param      payload  - The final payload ready to extract data from
 		 * @return     True if the process was a success
 		 */
@@ -238,7 +238,7 @@ class VescUart
 		 * @brief      Help Function to print uint8_t array over Serial for Debug
 		 *
 		 * @param      data  - Data array to print
-		 * @param      len   - Lenght of the array to print
+		 * @param      len   - Length of the array to print
 		 */
 		void serialPrint(uint8_t * data, int len);
 

From 0f14bd88d4626c5360ea6b0c8c9193ab5500e01a Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Tue, 5 Mar 2024 17:07:49 -0500
Subject: [PATCH 04/13] Make Indentation Consistent

---
 src/VescUart.h | 246 ++++++++++++++++++++++++-------------------------
 1 file changed, 123 insertions(+), 123 deletions(-)

diff --git a/src/VescUart.h b/src/VescUart.h
index 4a63740..ab783a6 100644
--- a/src/VescUart.h
+++ b/src/VescUart.h
@@ -66,129 +66,129 @@ class VescUart
 		/** Variable to hold nunchuck values */
 		nunchuckPackage nunchuck; 
 
-       /** Variable to hold firmware version */
-        FWversionPackage fw_version; 
-
-        /**
-         * @brief      Set the serial port for uart communication
-         * @param      port  - Reference to Serial port (pointer) 
-         */
-        void setSerialPort(Stream* port);
-
-        /**
-         * @brief      Set the serial port for debugging
-         * @param      port  - Reference to Serial port (pointer) 
-         */
-        void setDebugPort(Stream* port);
-
-        /**
-         * @brief      Populate the firmware version variables
-         *
-         * @return     True if successfull otherwise false
-         */
-        bool getFWversion(void);
-
-        /**
-         * @brief      Populate the firmware version variables
-         *
-         * @param      canId  - The CAN ID of the VESC
-         * @return     True if successfull otherwise false
-         */
-        bool getFWversion(uint8_t canId);
-
-        /**
-         * @brief      Sends a command to VESC and stores the returned data
-         *
-         * @return     True if successfull otherwise false
-         */
-        bool getVescValues(void);
-
-        /**
-         * @brief      Sends a command to VESC and stores the returned data
-         * @param      canId  - The CAN ID of the VESC
-         *
-         * @return     True if successfull otherwise false
-         */
-        bool getVescValues(uint8_t canId);
-
-        /**
-         * @brief      Sends values for joystick and buttons to the nunchuck app
-         */
-        void setNunchuckValues(void);
-        /**
-         * @brief      Sends values for joystick and buttons to the nunchuck app
-         * @param      canId  - The CAN ID of the VESC
-         */
-        void setNunchuckValues(uint8_t canId);
-
-        /**
-         * @brief      Set the current to drive the motor
-         * @param      current  - The current to apply
-         */
-        void setCurrent(float current);
-
-        /**
-         * @brief      Set the current to drive the motor
-         * @param      current  - The current to apply
-         * @param      canId  - The CAN ID of the VESC
-         */
-        void setCurrent(float current, uint8_t canId);
-
-        /**
-         * @brief      Set the current to brake the motor
-         * @param      brakeCurrent  - The current to apply
-         */
-        void setBrakeCurrent(float brakeCurrent);
-
-        /**
-         * @brief      Set the current to brake the motor
-         * @param      brakeCurrent  - The current to apply
-         * @param      canId  - The CAN ID of the VESC
-         */
-        void setBrakeCurrent(float brakeCurrent, uint8_t canId);
-
-
-        /**
-         * @brief      Set the rpm of the motor
-         * @param      rpm  - The desired RPM (actually eRPM = RPM * poles)
-         */
-        void setRPM(float rpm);		
-
-        /**
-         * @brief      Set the rpm of the motor
-         * @param      rpm  - The desired RPM (actually eRPM = RPM * poles)
-         * @param      canId  - The CAN ID of the VESC
-         */
-        void setRPM(float rpm, uint8_t canId);
-
-        /**
-         * @brief      Set the duty of the motor
-         * @param      duty  - The desired duty (0.0-1.0)
-         */
-        void setDuty(float duty);
-
-        /**
-         * @brief      Set the duty of the motor
-         * @param      duty  - The desired duty (0.0-1.0)
-         * @param      canId  - The CAN ID of the VESC
-         */
-        void setDuty(float duty, uint8_t canId);
-
-        /**
-         * @brief      Send a keepalive message
-         */
-        void sendKeepalive(void);
-
-        /**
-         * @brief      Send a keepalive message
-         * @param      canId  - The CAN ID of the VESC
-         */
-        void sendKeepalive(uint8_t canId);
-
-        /**
-         * @brief      Help Function to print struct dataPackage over Serial for Debug
-         */
-        void printVescValues(void);
+    /** Variable to hold firmware version */
+    FWversionPackage fw_version; 
+
+    /**
+     * @brief      Set the serial port for uart communication
+     * @param      port  - Reference to Serial port (pointer) 
+     */
+    void setSerialPort(Stream* port);
+
+    /**
+     * @brief      Set the serial port for debugging
+     * @param      port  - Reference to Serial port (pointer) 
+     */
+    void setDebugPort(Stream* port);
+
+    /**
+     * @brief      Populate the firmware version variables
+     *
+     * @return     True if successfull otherwise false
+     */
+    bool getFWversion(void);
+
+    /**
+     * @brief      Populate the firmware version variables
+     *
+     * @param      canId  - The CAN ID of the VESC
+     * @return     True if successfull otherwise false
+     */
+    bool getFWversion(uint8_t canId);
+
+    /**
+     * @brief      Sends a command to VESC and stores the returned data
+     *
+     * @return     True if successfull otherwise false
+     */
+    bool getVescValues(void);
+
+    /**
+     * @brief      Sends a command to VESC and stores the returned data
+     * @param      canId  - The CAN ID of the VESC
+     *
+     * @return     True if successfull otherwise false
+     */
+    bool getVescValues(uint8_t canId);
+
+    /**
+     * @brief      Sends values for joystick and buttons to the nunchuck app
+     */
+    void setNunchuckValues(void);
+    /**
+     * @brief      Sends values for joystick and buttons to the nunchuck app
+     * @param      canId  - The CAN ID of the VESC
+     */
+    void setNunchuckValues(uint8_t canId);
+
+    /**
+     * @brief      Set the current to drive the motor
+     * @param      current  - The current to apply
+     */
+    void setCurrent(float current);
+
+    /**
+     * @brief      Set the current to drive the motor
+     * @param      current  - The current to apply
+     * @param      canId  - The CAN ID of the VESC
+     */
+    void setCurrent(float current, uint8_t canId);
+
+    /**
+     * @brief      Set the current to brake the motor
+     * @param      brakeCurrent  - The current to apply
+     */
+    void setBrakeCurrent(float brakeCurrent);
+
+    /**
+     * @brief      Set the current to brake the motor
+     * @param      brakeCurrent  - The current to apply
+     * @param      canId  - The CAN ID of the VESC
+     */
+    void setBrakeCurrent(float brakeCurrent, uint8_t canId);
+
+
+    /**
+     * @brief      Set the rpm of the motor
+     * @param      rpm  - The desired RPM (actually eRPM = RPM * poles)
+     */
+    void setRPM(float rpm);		
+
+    /**
+     * @brief      Set the rpm of the motor
+     * @param      rpm  - The desired RPM (actually eRPM = RPM * poles)
+     * @param      canId  - The CAN ID of the VESC
+     */
+    void setRPM(float rpm, uint8_t canId);
+
+    /**
+     * @brief      Set the duty of the motor
+     * @param      duty  - The desired duty (0.0-1.0)
+     */
+    void setDuty(float duty);
+
+    /**
+     * @brief      Set the duty of the motor
+     * @param      duty  - The desired duty (0.0-1.0)
+     * @param      canId  - The CAN ID of the VESC
+     */
+    void setDuty(float duty, uint8_t canId);
+
+    /**
+     * @brief      Send a keepalive message
+     */
+    void sendKeepalive(void);
+
+    /**
+     * @brief      Send a keepalive message
+     * @param      canId  - The CAN ID of the VESC
+     */
+    void sendKeepalive(uint8_t canId);
+
+    /**
+     * @brief      Help Function to print struct dataPackage over Serial for Debug
+     */
+    void printVescValues(void);
 
 	private: 
 

From 485ab003c99b7b7c4123fa1ae1ffcc9fe0827942 Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Tue, 5 Mar 2024 18:10:47 -0500
Subject: [PATCH 05/13] Create a Function to Poll for IMU Data

based directly off COMM GET VALUES
---
 src/VescUart.cpp | 31 +++++++++++++++++++++++++++++++
 src/VescUart.h   | 15 +++++++++++++++
 2 files changed, 46 insertions(+)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index e2262c9..24ecee3 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -298,6 +298,37 @@ bool VescUart::getVescValues(uint8_t canId) {
 	}
 	return false;
 }
+
+bool VescUart::getImuData(void) {
+	return getImuData(0);
+}
+
+bool VescUart::getImuData(uint8_t canId) {
+
+	if (debugPort!=NULL){
+		debugPort->println("Command: COMM_GET_IMU_DATA "+String(canId));
+	}
+
+	int32_t index = 0;
+	int payloadSize = (canId == 0 ? 1 : 3);
+	uint8_t payload[payloadSize];
+	if (canId != 0) {
+		payload[index++] = { COMM_FORWARD_CAN };
+		payload[index++] = canId;
+	}
+	payload[index++] = { COMM_GET_IMU_DATA};
+
+	packSendPayload(payload, payloadSize);
+
+	uint8_t message[256];
+	int messageLength = receiveUartMessage(message);
+
+	if (messageLength > 55) {
+		return processReadPacket(message); 
+	}
+	return false;
+}
+
 void VescUart::setNunchuckValues() {
 	return setNunchuckValues(0);
 }
diff --git a/src/VescUart.h b/src/VescUart.h
index ab783a6..4bd80ec 100644
--- a/src/VescUart.h
+++ b/src/VescUart.h
@@ -111,6 +111,21 @@ class VescUart
      */
     bool getVescValues(uint8_t canId);
 
+     /**
+     * @brief      Sends a command to VESC and stores the returned IMU data
+     *
+     * @return     True if successfull otherwise false
+     */
+    bool getImuData(void);
+
+    /**
+     * @brief      Sends a command to VESC and stores the returned IMU data
+     * @param      canId  - The CAN ID of the VESC
+     *
+     * @return     True if successfull otherwise false
+     */
+    bool getImuData(uint8_t canId);
+
     /**
      * @brief      Sends values for joystick and buttons to the nunchuck app
      */

From 8664ddcef9d9eb80b7dd64080b6d89b608bdda76 Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Tue, 5 Mar 2024 18:41:59 -0500
Subject: [PATCH 06/13] Update comment to point to IMU info in newest BLDC

---
 src/VescUart.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index 24ecee3..c6988b5 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -217,7 +217,7 @@ bool VescUart::processReadPacket(uint8_t * message) {
 
 		break;
 
-		case COMM_GET_IMU_DATA: // Structure defined here: https://github.com/vedderb/bldc/blob/31a313129ded1d355a921c572854820921d3030a/commands.c#L995
+		case COMM_GET_IMU_DATA: // Structure defined here: https://github.com/vedderb/bldc/blob/c8be115bb5be5a5558e3a50ba82e55931e3a45c4/comm/commands.c#L1111
 
 			data.imuRoll			= buffer_get_float32(message, 1.0, &index);
 			data.imuPitch			= buffer_get_float32(message, 1.0, &index);

From 60a8e2f147f7f9cc666c7ee3a570e07ed1c9ebcb Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Tue, 5 Mar 2024 21:10:08 -0500
Subject: [PATCH 07/13] Add skipping to end of message

---
 src/VescUart.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index c6988b5..3fdfc19 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -228,7 +228,9 @@ bool VescUart::processReadPacket(uint8_t * message) {
 			data.gyroX				= buffer_get_float32(message, 1.0, &index);
 			data.gyroY				= buffer_get_float32(message, 1.0, &index);
 			data.gyroZ				= buffer_get_float32(message, 1.0, &index);
+			index += 4*7;
 
+			data.id					= message[index++];								// 1 byte  - app_get_configuration()->controller_id	
 			return true;
 
 		break;
@@ -310,7 +312,7 @@ bool VescUart::getImuData(uint8_t canId) {
 	}
 
 	int32_t index = 0;
-	int payloadSize = (canId == 0 ? 1 : 3);
+	int payloadSize = (canId == 0 ? 1 : 3); // Not sure if correct. Needs additional testing
 	uint8_t payload[payloadSize];
 	if (canId != 0) {
 		payload[index++] = { COMM_FORWARD_CAN };

From a0f7e2cf6733fffe9e1c27ba838bdbedfa9142aa Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Tue, 5 Mar 2024 21:14:27 -0500
Subject: [PATCH 08/13] Separate imu id and real id for testing purposes

---
 src/VescUart.cpp | 2 +-
 src/VescUart.h   | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index 3fdfc19..8b56eac 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -230,7 +230,7 @@ bool VescUart::processReadPacket(uint8_t * message) {
 			data.gyroZ				= buffer_get_float32(message, 1.0, &index);
 			index += 4*7;
 
-			data.id					= message[index++];								// 1 byte  - app_get_configuration()->controller_id	
+			data.idimu					= message[index++];								// 1 byte  - app_get_configuration()->controller_id	
 			return true;
 
 		break;
diff --git a/src/VescUart.h b/src/VescUart.h
index 4bd80ec..5d53632 100644
--- a/src/VescUart.h
+++ b/src/VescUart.h
@@ -35,7 +35,8 @@ class VescUart
         float accZ;
         float gyroX;
         float gyroY;
-        float gyroZ; 
+        float gyroZ;
+        uint8_t idimu;
 	};
 
 	/** Struct to hold the nunchuck values to send over UART */

From cc9213c92579ad7ec02e5936efeaf12ba3d32ced Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Mon, 11 Mar 2024 19:48:22 -0400
Subject: [PATCH 09/13] Hardcode a mask to the payload

---
 src/VescUart.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index 8b56eac..09267e1 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -319,6 +319,7 @@ bool VescUart::getImuData(uint8_t canId) {
 		payload[index++] = canId;
 	}
 	payload[index++] = { COMM_GET_IMU_DATA};
+	payload[index++] = {1}; // Not sure if correct. Needs additional testing
 
 	packSendPayload(payload, payloadSize);
 

From 2b82169a6e931db3b91921e12330c6206de74853 Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Mon, 11 Mar 2024 19:51:26 -0400
Subject: [PATCH 10/13] Update Mask Value

---
 src/VescUart.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index 09267e1..b3aa8e5 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -319,7 +319,7 @@ bool VescUart::getImuData(uint8_t canId) {
 		payload[index++] = canId;
 	}
 	payload[index++] = { COMM_GET_IMU_DATA};
-	payload[index++] = {1}; // Not sure if correct. Needs additional testing
+	payload[index++] = 0xFFFF; // Not sure if correct. Needs additional testing
 
 	packSendPayload(payload, payloadSize);
 

From ea2ae27b16059a81205ff1f8b1d27514eb9e2bce Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Mon, 11 Mar 2024 20:04:45 -0400
Subject: [PATCH 11/13] Add Debug for IMU reading

---
 src/VescUart.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index b3aa8e5..9f96030 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -229,7 +229,9 @@ bool VescUart::processReadPacket(uint8_t * message) {
 			data.gyroY				= buffer_get_float32(message, 1.0, &index);
 			data.gyroZ				= buffer_get_float32(message, 1.0, &index);
 			index += 4*7;
-
+			if (debugPort!=NULL){
+				debugPort->println("IMU DATA READ:  "+String(canId));
+				}
 			data.idimu					= message[index++];								// 1 byte  - app_get_configuration()->controller_id	
 			return true;
 

From 78326b1b9a66ba9246eb43a27a67d2f097a879e0 Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Thu, 14 Mar 2024 21:17:45 -0400
Subject: [PATCH 12/13] Cleaned up code that seems to actually work

Had to basically reverse engineer the data from the VESC tool source code to figure out what was wrong
---
 src/VescUart.cpp | 42 ++++++++++++++++++++++++------------------
 src/VescUart.h   | 10 +++++++++-
 2 files changed, 33 insertions(+), 19 deletions(-)

diff --git a/src/VescUart.cpp b/src/VescUart.cpp
index 9f96030..563b8f8 100644
--- a/src/VescUart.cpp
+++ b/src/VescUart.cpp
@@ -218,21 +218,26 @@ bool VescUart::processReadPacket(uint8_t * message) {
 		break;
 
 		case COMM_GET_IMU_DATA: // Structure defined here: https://github.com/vedderb/bldc/blob/c8be115bb5be5a5558e3a50ba82e55931e3a45c4/comm/commands.c#L1111
-
-			data.imuRoll			= buffer_get_float32(message, 1.0, &index);
-			data.imuPitch			= buffer_get_float32(message, 1.0, &index);
-			data.imuYaw 			= buffer_get_float32(message, 1.0, &index);
-			data.accX				= buffer_get_float32(message, 1.0, &index);
-			data.accY				= buffer_get_float32(message, 1.0, &index);
-			data.accZ				= buffer_get_float32(message, 1.0, &index);
-			data.gyroX				= buffer_get_float32(message, 1.0, &index);
-			data.gyroY				= buffer_get_float32(message, 1.0, &index);
-			data.gyroZ				= buffer_get_float32(message, 1.0, &index);
-			index += 4*7;
-			if (debugPort!=NULL){
-				debugPort->println("IMU DATA READ:  "+String(canId));
-				}
-			data.idimu					= message[index++];								// 1 byte  - app_get_configuration()->controller_id	
+			
+			data.imuMask			= buffer_get_uint16(message, &index); // Skip 2 bytes for mask
+			data.imuRoll			= buffer_get_float32_auto(message, &index);
+			data.imuPitch			= buffer_get_float32_auto(message, &index);
+			data.imuYaw 			= buffer_get_float32_auto(message, &index);
+			data.accX				= buffer_get_float32_auto(message, &index);
+			data.accY				= buffer_get_float32_auto(message, &index);
+			data.accZ				= buffer_get_float32_auto(message, &index);
+			data.gyroX				= buffer_get_float32_auto(message, &index);
+			data.gyroY				= buffer_get_float32_auto(message, &index);
+			data.gyroZ				= buffer_get_float32_auto(message, &index);
+			data.magX				= buffer_get_float32_auto(message, &index);
+			data.magY				= buffer_get_float32_auto(message, &index);
+			data.magZ				= buffer_get_float32_auto(message, &index);
+			data.q0					= buffer_get_float32_auto(message, &index);
+			data.q1					= buffer_get_float32_auto(message, &index);
+			data.q2					= buffer_get_float32_auto(message, &index);
+			data.q3					= buffer_get_float32_auto(message, &index);
+			//data.imuID			= message[index] // Last byte should have the ID of the VESC but it seems like its 	
+	
 			return true;
 
 		break;
@@ -314,21 +319,22 @@ bool VescUart::getImuData(uint8_t canId) {
 	}
 
 	int32_t index = 0;
-	int payloadSize = (canId == 0 ? 1 : 3); // Not sure if correct. Needs additional testing
+	int payloadSize = (canId == 0 ? 3: 5);
 	uint8_t payload[payloadSize];
 	if (canId != 0) {
 		payload[index++] = { COMM_FORWARD_CAN };
 		payload[index++] = canId;
 	}
 	payload[index++] = { COMM_GET_IMU_DATA};
-	payload[index++] = 0xFFFF; // Not sure if correct. Needs additional testing
+	payload[index++] = 0xFF; // Add Mask
+	payload[index++] = 0xFF; // Add Mask
 
 	packSendPayload(payload, payloadSize);
 
 	uint8_t message[256];
 	int messageLength = receiveUartMessage(message);
 
-	if (messageLength > 55) {
+	if (messageLength > 65) { // Message length should be at least 67 (set to 65)
 		return processReadPacket(message); 
 	}
 	return false;
diff --git a/src/VescUart.h b/src/VescUart.h
index 5d53632..fdd2c94 100644
--- a/src/VescUart.h
+++ b/src/VescUart.h
@@ -27,6 +27,7 @@ class VescUart
         float pidPos;
         uint8_t id;
         mc_fault_code error; 
+        uint16_t imuMask;
         float imuRoll;
         float imuPitch;
         float imuYaw;
@@ -36,7 +37,14 @@ class VescUart
         float gyroX;
         float gyroY;
         float gyroZ;
-        uint8_t idimu;
+        float magX;
+        float magY;
+        float magZ;
+        float q0;
+        float q1;
+        float q2;
+        float q3;
+        // uint8_t imuID;
 	};
 
 	/** Struct to hold the nunchuck values to send over UART */

From 48cc6d98e7347e5d5551b81762765f0522f12737 Mon Sep 17 00:00:00 2001
From: Kevin Yacucci <kcy0511@gmail.com>
Date: Thu, 14 Mar 2024 21:58:05 -0400
Subject: [PATCH 13/13] Update Library Version

Updated Keywords and Version since this is new functionality
---
 README.md          | 2 +-
 keywords.txt       | 3 ++-
 library.properties | 2 +-
 3 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index b8280df..ef82090 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 # VescUart
 
-Arduino library for interfacing with a VESC over UART. This library is based upon the works of many. The library is tested on a Teensy 4, and is updated for VESC firmware FW5+. The library is not nessecary backwards compatible with older versions of VESC firmware, so please update to the newest firmware available to your VESC.
+Arduino library for interfacing with a VESC over UART. This library is based upon the works of many. The library is tested on a Teensy 4, and is updated for VESC firmware FW5+. The library is not necessarily backwards compatible with older versions of VESC firmware, so please update to the newest firmware available to your VESC.
 
 The library supports only a small amount of features available to the VESC. You are welcome to make a pull request if you integrate new functionality and I will do my best to merge. 
 
diff --git a/keywords.txt b/keywords.txt
index 5ec1bcb..534d309 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -20,4 +20,5 @@ printVescValues		KEYWORD2
 setCurrent			KEYWORD2
 setBrakeCurrent		KEYWORD2
 setRPM				KEYWORD2
-setDuty				KEYWORD2
\ No newline at end of file
+setDuty				KEYWORD2
+getImuData          KEYWORD2
\ No newline at end of file
diff --git a/library.properties b/library.properties
index 38fdc19..8cd309a 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=VescUart
-version=1.0.1
+version=1.0.2
 author=SolidGeek
 maintainer=SolidGeek
 sentence=Library offering UART communication for VESC