mirror of
https://github.com/intrepidcs/libicsneo.git
synced 2026-08-05 01:18:36 +02:00
CAN and CAN FD transmit implemented
This commit is contained in:
Paul Hollinsky
+92
-18
@@ -4,26 +4,98 @@ using namespace icsneo;
|
||||
|
||||
bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message>& message) {
|
||||
bool shortFormat = false;
|
||||
bool useResultAsBuffer = false; // Otherwise it's expected that we use message->data
|
||||
result.clear();
|
||||
|
||||
switch(message->network.getType()) {
|
||||
// case Network::Type::CAN: {
|
||||
// if(message->data.size() < 24)
|
||||
// break; // We would read garbage when interpereting the data
|
||||
case Network::Type::CAN: {
|
||||
useResultAsBuffer = true;
|
||||
|
||||
// HardwareCANPacket* data = (HardwareCANPacket*)message->data.data();
|
||||
// auto msg = std::make_shared<CANMessage>();
|
||||
// msg->network = message->network;
|
||||
// msg->arbid = data->header.SID;
|
||||
// msg->data.reserve(data->dlc.DLC);
|
||||
auto canmsg = std::dynamic_pointer_cast<CANMessage>(message);
|
||||
if(!canmsg)
|
||||
return false; // The message was not a properly formed CANMessage
|
||||
|
||||
// // Timestamp calculation
|
||||
// msg->timestamp = data->timestamp.TS * 25; // Timestamps are in 25ns increments since 1/1/2007 GMT 00:00:00.0000
|
||||
if(canmsg->isCANFD && canmsg->isRemote)
|
||||
return false; // RTR frames can not be used with CAN FD
|
||||
|
||||
// for(auto i = 0; i < data->dlc.DLC; i++)
|
||||
// msg->data.push_back(data->data[i]);
|
||||
// return msg;
|
||||
// }
|
||||
const size_t dataSize = canmsg->data.size();
|
||||
if(dataSize > 64 || (dataSize > 8 && !canmsg->isCANFD))
|
||||
return false; // Too much data for the protocol
|
||||
|
||||
uint8_t lengthNibble = uint8_t(canmsg->data.size());
|
||||
if(lengthNibble > 8) {
|
||||
switch(lengthNibble) {
|
||||
case 12:
|
||||
lengthNibble = 0x9;
|
||||
break;
|
||||
case 16:
|
||||
lengthNibble = 0xA;
|
||||
break;
|
||||
case 20:
|
||||
lengthNibble = 0xB;
|
||||
break;
|
||||
case 24:
|
||||
lengthNibble = 0xC;
|
||||
break;
|
||||
case 32:
|
||||
lengthNibble = 0xD;
|
||||
break;
|
||||
case 48:
|
||||
lengthNibble = 0xE;
|
||||
break;
|
||||
case 64:
|
||||
lengthNibble = 0xF;
|
||||
break;
|
||||
default:
|
||||
return false; // CAN FD frame may have had an incorrect byte count
|
||||
}
|
||||
}
|
||||
|
||||
result.push_back(0 /* byte count here later */ << 4 | (uint8_t(canmsg->network.getNetID()) & 0xF));
|
||||
result.insert(result.end(), {0,0}); // Two bytes for Description ID, big endian, not used in API currently
|
||||
|
||||
// Next 2-4 bytes are ArbID
|
||||
if(canmsg->isExtended) {
|
||||
if(canmsg->arbid >= 0x20000000) // Extended messages use 29-bit arb IDs
|
||||
return false;
|
||||
|
||||
result.insert(result.end(), {
|
||||
(uint8_t)(canmsg->arbid >> 21),
|
||||
(uint8_t)((((canmsg->arbid & 0x001C0000) >> 13) & 0xFF) + (((canmsg->arbid & 0x00030000) >> 16) & 0xFF) | 8),
|
||||
(uint8_t)(canmsg->arbid >> 8),
|
||||
(uint8_t)canmsg->arbid
|
||||
});
|
||||
} else {
|
||||
if(canmsg->arbid >= 0x800) // Standard messages use 11-bit arb IDs
|
||||
return false;
|
||||
|
||||
result.insert(result.end(), {
|
||||
(uint8_t)(canmsg->arbid >> 3),
|
||||
(uint8_t)((canmsg->arbid & 0x7) << 5)
|
||||
});
|
||||
}
|
||||
|
||||
// Status and DLC bits
|
||||
if(canmsg->isCANFD) {
|
||||
result.push_back(0x0F); // FD Frame
|
||||
uint8_t fdStatusByte = lengthNibble;
|
||||
if(canmsg->baudrateSwitch)
|
||||
fdStatusByte |= 0x80; // BRS status bit
|
||||
result.push_back(fdStatusByte);
|
||||
} else {
|
||||
// TODO Support high voltage wakeup, bitwise-or in 0x8 here to enable
|
||||
uint8_t statusNibble = canmsg->isRemote ? 0x4 : 0x0;
|
||||
result.push_back((statusNibble << 4) | lengthNibble);
|
||||
}
|
||||
|
||||
// Now finally the payload
|
||||
result.insert(result.end(), canmsg->data.begin(), canmsg->data.end());
|
||||
result.push_back(0);
|
||||
|
||||
// Fill in the length byte from earlier
|
||||
result[0] |= result.size() << 4;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
switch(message->network.getNetID()) {
|
||||
case Network::NetID::Device:
|
||||
@@ -59,13 +131,15 @@ bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message
|
||||
}
|
||||
}
|
||||
|
||||
auto& buffer = useResultAsBuffer ? result : message->data;
|
||||
|
||||
if(shortFormat) {
|
||||
message->data.insert(message->data.begin(), (uint8_t(message->data.size()) << 4) | uint8_t(message->network.getNetID()));
|
||||
buffer.insert(buffer.begin(), (uint8_t(buffer.size()) << 4) | uint8_t(message->network.getNetID()));
|
||||
} else {
|
||||
// Size in long format is the size of the entire packet
|
||||
// So +1 for AA header, +1 for short format header, +2 for long format size, and +2 for long format NetID
|
||||
uint16_t size = uint16_t(message->data.size()) + 1 + 1 + 2 + 2;
|
||||
message->data.insert(message->data.begin(), {
|
||||
uint16_t size = uint16_t(buffer.size()) + 1 + 1 + 2 + 2;
|
||||
buffer.insert(buffer.begin(), {
|
||||
(uint8_t)Network::NetID::RED, // 0x0C for long message
|
||||
(uint8_t)size, // Size, little endian 16-bit
|
||||
(uint8_t)(size >> 8),
|
||||
@@ -74,7 +148,7 @@ bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message
|
||||
});
|
||||
}
|
||||
|
||||
result = packetizer->packetWrap(message->data, shortFormat);
|
||||
result = packetizer->packetWrap(buffer, shortFormat);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user