From 37b83e05064ea5afae1a6fa3c7624aec159e5bcc Mon Sep 17 00:00:00 2001
From: Paul Hollinsky <phollinsky@intrepidcs.com>
Date: Wed, 17 Apr 2019 16:50:11 -0400
Subject: [PATCH] Automatically pad CAN FD messages out

---
 communication/packet/canpacket.cpp | 53 +++++++++++++++++++++++++++++-
 1 file changed, 52 insertions(+), 1 deletion(-)

diff --git a/communication/packet/canpacket.cpp b/communication/packet/canpacket.cpp
index 8b29b63..5c61699 100644
--- a/communication/packet/canpacket.cpp
+++ b/communication/packet/canpacket.cpp
@@ -87,26 +87,76 @@ bool HardwareCANPacket::EncodeFromMessage(const CANMessage& message, std::vector
 		return false; // Too much data for the protocol
 
 	uint8_t lengthNibble = uint8_t(message.data.size());
+	uint8_t paddingBytes = 0;
 	if(lengthNibble > 8) {
 		switch(lengthNibble) {
+			case 9: paddingBytes++;
+			case 10: paddingBytes++;
+			case 11: paddingBytes++;
 			case 12:
 				lengthNibble = 0x9;
 				break;
+			case 13: paddingBytes++;
+			case 14: paddingBytes++;
+			case 15: paddingBytes++;
 			case 16:
 				lengthNibble = 0xA;
 				break;
+			case 17: paddingBytes++;
+			case 18: paddingBytes++;
+			case 19: paddingBytes++;
 			case 20:
 				lengthNibble = 0xB;
 				break;
+			case 21: paddingBytes++;
+			case 22: paddingBytes++;
+			case 23: paddingBytes++;
 			case 24:
 				lengthNibble = 0xC;
 				break;
+			case 25: paddingBytes++;
+			case 26: paddingBytes++;
+			case 27: paddingBytes++;
+			case 28: paddingBytes++;
+			case 29: paddingBytes++;
+			case 30: paddingBytes++;
+			case 31: paddingBytes++;
 			case 32:
 				lengthNibble = 0xD;
 				break;
+			case 33: paddingBytes++;
+			case 34: paddingBytes++;
+			case 35: paddingBytes++;
+			case 36: paddingBytes++;
+			case 37: paddingBytes++;
+			case 38: paddingBytes++;
+			case 39: paddingBytes++;
+			case 40: paddingBytes++;
+			case 41: paddingBytes++;
+			case 42: paddingBytes++;
+			case 43: paddingBytes++;
+			case 44: paddingBytes++;
+			case 45: paddingBytes++;
+			case 46: paddingBytes++;
+			case 47: paddingBytes++;
 			case 48:
 				lengthNibble = 0xE;
 				break;
+			case 49: paddingBytes++;
+			case 50: paddingBytes++;
+			case 51: paddingBytes++;
+			case 52: paddingBytes++;
+			case 53: paddingBytes++;
+			case 54: paddingBytes++;
+			case 55: paddingBytes++;
+			case 56: paddingBytes++;
+			case 57: paddingBytes++;
+			case 58: paddingBytes++;
+			case 59: paddingBytes++;
+			case 60: paddingBytes++;
+			case 61: paddingBytes++;
+			case 62: paddingBytes++;
+			case 63: paddingBytes++;
 			case 64:
 				lengthNibble = 0xF;
 				break;
@@ -116,7 +166,7 @@ bool HardwareCANPacket::EncodeFromMessage(const CANMessage& message, std::vector
 	}
 
 	// Pre-allocate as much memory as we will possibly need for speed
-	result.reserve(17 + dataSize);
+	result.reserve(17 + dataSize + paddingBytes);
 
 	result.push_back(0 /* byte count here later */ << 4 | (uint8_t(message.network.getNetID()) & 0xF));
 
@@ -159,6 +209,7 @@ bool HardwareCANPacket::EncodeFromMessage(const CANMessage& message, std::vector
 
 	// Now finally the payload
 	result.insert(result.end(), message.data.begin(), message.data.end());
+	result.resize(result.size() + paddingBytes);
 	result.push_back(0);
 
 	// Fill in the length byte from earlier