sven
/
libicsneo
mirror of https://github.com/intrepidcs/libicsneo.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

Automatically pad CAN FD messages out

pull/13/head
Paul Hollinsky 2019-04-17 16:50:11 -04:00
parent 313a98aa04
commit 37b83e0506
1 changed files with 52 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
communication/packet/canpacket.cpp
View File

@ -87,26 +87,76 @@ bool HardwareCANPacket::EncodeFromMessage(const CANMessage& message, std::vector
return false; // Too much data for the protocol return false; // Too much data for the protocol
uint8_t lengthNibble = uint8_t(message.data.size()); uint8_t lengthNibble = uint8_t(message.data.size());
uint8_t paddingBytes = 0;
if(lengthNibble > 8) { if(lengthNibble > 8) {
switch(lengthNibble) { switch(lengthNibble) {
case 9: paddingBytes++;
case 10: paddingBytes++;
case 11: paddingBytes++;
case 12: case 12:
lengthNibble = 0x9; lengthNibble = 0x9;
break; break;
case 13: paddingBytes++;
case 14: paddingBytes++;
case 15: paddingBytes++;
case 16: case 16:
lengthNibble = 0xA; lengthNibble = 0xA;
break; break;
case 17: paddingBytes++;
case 18: paddingBytes++;
case 19: paddingBytes++;
case 20: case 20:
lengthNibble = 0xB; lengthNibble = 0xB;
break; break;
case 21: paddingBytes++;
case 22: paddingBytes++;
case 23: paddingBytes++;
case 24: case 24:
lengthNibble = 0xC; lengthNibble = 0xC;
break; break;
case 25: paddingBytes++;
case 26: paddingBytes++;
case 27: paddingBytes++;
case 28: paddingBytes++;
case 29: paddingBytes++;
case 30: paddingBytes++;
case 31: paddingBytes++;
case 32: case 32:
lengthNibble = 0xD; lengthNibble = 0xD;
break; 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: case 48:
lengthNibble = 0xE; lengthNibble = 0xE;
break; 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: case 64:
lengthNibble = 0xF; lengthNibble = 0xF;
break; 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 // 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)); 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 // Now finally the payload
result.insert(result.end(), message.data.begin(), message.data.end()); result.insert(result.end(), message.data.begin(), message.data.end());
result.resize(result.size() + paddingBytes);
result.push_back(0); result.push_back(0);
// Fill in the length byte from earlier // Fill in the length byte from earlier