From d037709963487fe1993876d8324df229cde2d92d Mon Sep 17 00:00:00 2001 From: Paul Hollinsky Date: Thu, 18 Oct 2018 17:39:37 -0400 Subject: [PATCH] CAN and CAN FD transmit implemented --- api/icsneoc/icsneoc.cpp | 18 +++- api/icsneoc/include/icsneoc.h | 12 +++ communication/decoder.cpp | 75 +++++++------- communication/encoder.cpp | 110 +++++++++++++++++---- communication/message/include/neomessage.h | 4 +- communication/message/neomessage.cpp | 47 ++++++--- device/device.cpp | 26 +++-- device/include/device.h | 3 + 8 files changed, 213 insertions(+), 82 deletions(-) diff --git a/api/icsneoc/icsneoc.cpp b/api/icsneoc/icsneoc.cpp index c165c08..b4853a9 100644 --- a/api/icsneoc/icsneoc.cpp +++ b/api/icsneoc/icsneoc.cpp @@ -180,7 +180,7 @@ bool icsneo_getMessages(const neodevice_t* device, neomessage_t* messages, size_ for(size_t i = 0; i < *items; i++) { // For each message, copy into neomessage_t buffer given - messages[i] = CreateNeoMessage(*(storage[i])); + messages[i] = CreateNeoMessage(storage[i]); } // The user now has until the next call of icsneo_getMessages (for this device) to use the data, after which point it's freed @@ -270,4 +270,20 @@ bool icsneo_setBaudrate(const neodevice_t* device, uint16_t netid, uint32_t newB return false; return device->device->settings->setBaudrateFor(netid, newBaudrate); +} + +bool icsneo_transmit(const neodevice_t* device, const neomessage_t* message) { + if(!icsneo_isValidNeoDevice(device)) + return false; + + return device->device->transmit(CreateMessageFromNeoMessage(message)); +} + +bool icsneo_transmitMessages(const neodevice_t* device, const neomessage_t* messages, size_t count) { + // TODO This can be implemented faster + for(size_t i = 0; i < count; i++) { + if(!icsneo_transmit(device, messages + i)) + return false; + } + return true; } \ No newline at end of file diff --git a/api/icsneoc/include/icsneoc.h b/api/icsneoc/include/icsneoc.h index 971487c..993b43b 100644 --- a/api/icsneoc/include/icsneoc.h +++ b/api/icsneoc/include/icsneoc.h @@ -56,6 +56,10 @@ extern bool DLLExport icsneo_settingsApplyDefaultsTemporary(const neodevice_t* d extern bool DLLExport icsneo_setBaudrate(const neodevice_t* device, uint16_t netid, uint32_t newBaudrate); +extern bool DLLExport icsneo_transmit(const neodevice_t* device, const neomessage_t* message); + +extern bool DLLExport icsneo_transmitMessages(const neodevice_t* device, const neomessage_t* messages, size_t count); + #ifdef __cplusplus } // extern "C" #endif @@ -128,6 +132,12 @@ fn_icsneo_settingsApplyDefaultsTemporary icsneo_settingsApplyDefaultsTemporary; typedef bool(*fn_icsneo_setBaudrate)(const neodevice_t* device, uint16_t netid, uint32_t newBaudrate); fn_icsneo_setBaudrate icsneo_setBaudrate; +typedef bool(*fn_icsneo_transmit)(const neodevice_t* device, const neomessage_t* message); +fn_icsneo_transmit icsneo_transmit; + +typedef bool(*fn_icsneo_transmitMessages)(const neodevice_t* device, const neomessage_t* messages, size_t count); +fn_icsneo_transmitMessages icsneo_transmitMessages; + #define ICSNEO_IMPORT(func) func = (fn_##func)icsneo_dynamicLibraryGetFunction(icsneo_libraryHandle, #func) #define ICSNEO_IMPORTASSERT(func) if((ICSNEO_IMPORT(func)) == NULL) return 3 void* icsneo_libraryHandle = NULL; @@ -164,6 +174,8 @@ int icsneo_init() { ICSNEO_IMPORTASSERT(icsneo_settingsApplyDefaults); ICSNEO_IMPORTASSERT(icsneo_settingsApplyDefaultsTemporary); ICSNEO_IMPORTASSERT(icsneo_setBaudrate); + ICSNEO_IMPORTASSERT(icsneo_transmit); + ICSNEO_IMPORTASSERT(icsneo_transmitMessages); icsneo_initialized = true; return 0; diff --git a/communication/decoder.cpp b/communication/decoder.cpp index 0c4401a..8c756ad 100644 --- a/communication/decoder.cpp +++ b/communication/decoder.cpp @@ -43,40 +43,32 @@ bool Decoder::decode(std::shared_ptr& result, const std::shared_ptrheader.EDL && data->timestamp.IsExtended) { // CAN FD msg->isCANFD = true; msg->baudrateSwitch = data->header.BRS; // CAN FD Baudrate Switch - switch(length) { // CAN FD Length Decoding - case 0x0: - case 0x1: - case 0x2: - case 0x3: - case 0x4: - case 0x5: - case 0x6: - case 0x7: - case 0x8: - break; // The length is already correct - case 0x9: - length = 12; - break; - case 0xa: - length = 16; - break; - case 0xb: - length = 20; - break; - case 0xc: - length = 24; - break; - case 0xd: - length = 32; - break; - case 0xe: - length = 48; - break; - case 0xf: - length = 64; - break; - default: - return false; + if(length > 8) { + switch(length) { // CAN FD Length Decoding + case 0x9: + length = 12; + break; + case 0xa: + length = 16; + break; + case 0xb: + length = 20; + break; + case 0xc: + length = 24; + break; + case 0xd: + length = 32; + break; + case 0xe: + length = 48; + break; + case 0xf: + length = 64; + break; + default: + return false; + } } } @@ -128,7 +120,7 @@ bool Decoder::decode(std::shared_ptr& result, const std::shared_ptr& result, const std::shared_ptr(); - // msg->network = packet->network; - // msg->data = packet->data; - // result = msg; - // return true; + // For the moment other types of messages will automatically be decoded as raw messages + auto msg = std::make_shared(); + msg->network = packet->network; + msg->data = packet->data; + result = msg; + return true; } \ No newline at end of file diff --git a/communication/encoder.cpp b/communication/encoder.cpp index 0a320ad..2ccc5ee 100644 --- a/communication/encoder.cpp +++ b/communication/encoder.cpp @@ -4,26 +4,98 @@ using namespace icsneo; bool Encoder::encode(std::vector& result, const std::shared_ptr& 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(); - // msg->network = message->network; - // msg->arbid = data->header.SID; - // msg->data.reserve(data->dlc.DLC); + auto canmsg = std::dynamic_pointer_cast(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& result, const std::shared_ptrdata; + 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& result, const std::shared_ptrpacketWrap(message->data, shortFormat); + result = packetizer->packetWrap(buffer, shortFormat); return true; } diff --git a/communication/message/include/neomessage.h b/communication/message/include/neomessage.h index 9842c3b..d886449 100644 --- a/communication/message/include/neomessage.h +++ b/communication/message/include/neomessage.h @@ -119,12 +119,14 @@ typedef struct { #ifdef __cplusplus #include "communication/message/include/message.h" +#include static_assert(sizeof(neomessage_can_t) == sizeof(neomessage_t), "All types of neomessage_t must be the same size!"); namespace icsneo { -neomessage_t CreateNeoMessage(const Message& message); +neomessage_t CreateNeoMessage(const std::shared_ptr message); +std::shared_ptr CreateMessageFromNeoMessage(const neomessage_t* neomessage); } #endif diff --git a/communication/message/neomessage.cpp b/communication/message/neomessage.cpp index aad1d30..497212e 100644 --- a/communication/message/neomessage.cpp +++ b/communication/message/neomessage.cpp @@ -3,27 +3,27 @@ using namespace icsneo; -neomessage_t icsneo::CreateNeoMessage(const Message& message) { +neomessage_t icsneo::CreateNeoMessage(const std::shared_ptr message) { // This function is not responsible for storing the message! // Keep the shared_ptr around for the lifetime of the data access - const auto type = message.network.getType(); + const auto type = message->network.getType(); neomessage_t neomsg = {}; // Clear out the memory - neomsg.netid = (uint32_t)message.network.getNetID(); + neomsg.netid = (uint32_t)message->network.getNetID(); neomsg.type = (uint8_t)type; - neomsg.length = message.data.size(); - neomsg.data = message.data.data(); - neomsg.timestamp = message.timestamp; + neomsg.length = message->data.size(); + neomsg.data = message->data.data(); + neomsg.timestamp = message->timestamp; switch(type) { case Network::Type::CAN: { neomessage_can_t& can = *(neomessage_can_t*)&neomsg; - const CANMessage& canmsg = *(const CANMessage*)&message; - can.arbid = canmsg.arbid; - can.status.extendedFrame = canmsg.isExtended; - can.status.remoteFrame = canmsg.isRemote; - can.status.canfdRTR = canmsg.isRemote; - can.status.canfdFDF = canmsg.isCANFD; - can.status.canfdBRS = canmsg.baudrateSwitch; + auto canmsg = std::static_pointer_cast(message); + can.arbid = canmsg->arbid; + can.status.extendedFrame = canmsg->isExtended; + can.status.remoteFrame = canmsg->isRemote; + can.status.canfdRTR = canmsg->isRemote; + can.status.canfdFDF = canmsg->isCANFD; + can.status.canfdBRS = canmsg->baudrateSwitch; break; } default: @@ -32,4 +32,25 @@ neomessage_t icsneo::CreateNeoMessage(const Message& message) { } return neomsg; +} + +std::shared_ptr icsneo::CreateMessageFromNeoMessage(const neomessage_t* neomessage) { + const Network network = neomessage->netid; + switch(network.getType()) { + case Network::Type::CAN: { + neomessage_can_t& can = *(neomessage_can_t*)neomessage; + auto canmsg = std::make_shared(); + canmsg->network = network; + canmsg->data.insert(canmsg->data.end(), can.data, can.data + can.length); + canmsg->arbid = can.arbid; + canmsg->isExtended = can.status.extendedFrame; + canmsg->isRemote = can.status.remoteFrame | can.status.canfdRTR; + canmsg->isCANFD = can.status.canfdFDF; + canmsg->baudrateSwitch = can.status.canfdBRS; + return canmsg; + } + default: + // TODO Implement others + return std::shared_ptr(); + } } \ No newline at end of file diff --git a/device/device.cpp b/device/device.cpp index 967d94b..acfa870 100644 --- a/device/device.cpp +++ b/device/device.cpp @@ -191,6 +191,22 @@ bool Device::goOffline() { return true; } +bool Device::transmit(std::shared_ptr message) { + std::vector packet; + if(!com->encoder->encode(packet, message)) + return false; + + return com->sendPacket(packet); +} + +bool Device::transmit(std::vector> messages) { + for(auto& message : messages) { + if(!transmit(message)) + return false; + } + return true; +} + void Device::handleInternalMessage(std::shared_ptr message) { switch(message->network.getNetID()) { case Network::NetID::Reset_Status: @@ -202,17 +218,13 @@ void Device::handleInternalMessage(std::shared_ptr message) { } void Device::updateLEDState() { - auto msg = std::make_shared(); - msg->network = Network::NetID::Device; /* NetID::Device is a super old command type. * It has a leading 0x00 byte, a byte for command, and a byte for an argument. * In this case, command 0x06 is SetLEDState. * This old command type is not really used anywhere else. */ + auto msg = std::make_shared(); + msg->network = Network::NetID::Device; msg->data = {0x00, 0x06, uint8_t(ledState)}; - std::vector packet; - if(!com->encoder->encode(packet, msg)) - return; - - com->sendPacket(packet); + transmit(msg); } \ No newline at end of file diff --git a/device/include/device.h b/device/include/device.h index 89a8391..7a8d660 100644 --- a/device/include/device.h +++ b/device/include/device.h @@ -55,6 +55,9 @@ public: enforcePollingMessageLimit(); } + bool transmit(std::shared_ptr message); + bool transmit(std::vector> messages); + void handleInternalMessage(std::shared_ptr message); std::unique_ptr settings;