CAN and CAN FD transmit implemented

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
@@ -271,3 +271,19 @@ bool icsneo_setBaudrate(const neodevice_t* device, uint16_t netid, uint32_t newB
 
 	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;
+}

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;

communication/decoder.cpp

@@ -43,17 +43,8 @@ bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Pac
 			if(data->header.EDL && data->timestamp.IsExtended) { // CAN FD
 				msg->isCANFD = true;
 				msg->baudrateSwitch = data->header.BRS; // CAN FD Baudrate Switch
+				if(length > 8) {
 					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;
@@ -79,6 +70,7 @@ bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Pac
 							return false;
 					}
 				}
+			}
 			
 			// Data
 			// The first 8 bytes are always in the standard place
@@ -128,7 +120,7 @@ bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Pac
 					return true;
 				}
 				default:
-					return false;
+					break;//return false;
 			}
 		}
 		default:
@@ -177,11 +169,10 @@ bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Pac
 			}
 	}
 
-	return false;
+	// For the moment other types of messages will automatically be decoded as raw messages
-
+	auto msg = std::make_shared<Message>();
-	// auto msg = std::make_shared<Message>();
+	msg->network = packet->network;
-	// msg->network = packet->network;
+	msg->data = packet->data;
-	// msg->data = packet->data;
+	result = msg;
-	// result = msg;
+	return true;
-	// return true;
 }

communication/encoder.cpp

@@ -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: {
+		case Network::Type::CAN: {
-		// 	if(message->data.size() < 24)
+			useResultAsBuffer = true;
-		// 		break; // We would read garbage when interpereting the data
 
-		// 	HardwareCANPacket* data = (HardwareCANPacket*)message->data.data();
+			auto canmsg = std::dynamic_pointer_cast<CANMessage>(message);
-		// 	auto msg = std::make_shared<CANMessage>();
+			if(!canmsg)
-		// 	msg->network = message->network;
+				return false; // The message was not a properly formed CANMessage
-		// 	msg->arbid = data->header.SID;
-		// 	msg->data.reserve(data->dlc.DLC);
 
-		// 	// Timestamp calculation
+			if(canmsg->isCANFD && canmsg->isRemote)
-		// 	msg->timestamp = data->timestamp.TS * 25; // Timestamps are in 25ns increments since 1/1/2007 GMT 00:00:00.0000
+				return false; // RTR frames can not be used with CAN FD
 
-		// 	for(auto i = 0; i < data->dlc.DLC; i++)
+			const size_t dataSize = canmsg->data.size();
-		// 		msg->data.push_back(data->data[i]);
+			if(dataSize > 64 || (dataSize > 8 && !canmsg->isCANFD))
-		// 	return msg;
+				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;
+		uint16_t size = uint16_t(buffer.size()) + 1 + 1 + 2 + 2;
-		message->data.insert(message->data.begin(), {
+		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;
 }
 

communication/message/include/neomessage.h

@@ -119,12 +119,14 @@ typedef struct {
 
 #ifdef __cplusplus
 #include "communication/message/include/message.h"
+#include <memory>
 
 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> message);
+std::shared_ptr<Message> CreateMessageFromNeoMessage(const neomessage_t* neomessage);
 
 }
 #endif

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> 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.length = message->data.size();
-	neomsg.data = message.data.data();
+	neomsg.data = message->data.data();
-	neomsg.timestamp = message.timestamp;
+	neomsg.timestamp = message->timestamp;
 
 	switch(type) {
 		case Network::Type::CAN: {
 			neomessage_can_t& can = *(neomessage_can_t*)&neomsg;
-			const CANMessage& canmsg = *(const CANMessage*)&message;
+			auto canmsg = std::static_pointer_cast<CANMessage>(message);
-			can.arbid = canmsg.arbid;
+			can.arbid = canmsg->arbid;
-			can.status.extendedFrame = canmsg.isExtended;
+			can.status.extendedFrame = canmsg->isExtended;
-			can.status.remoteFrame = canmsg.isRemote;
+			can.status.remoteFrame = canmsg->isRemote;
-			can.status.canfdRTR = canmsg.isRemote;
+			can.status.canfdRTR = canmsg->isRemote;
-			can.status.canfdFDF = canmsg.isCANFD;
+			can.status.canfdFDF = canmsg->isCANFD;
-			can.status.canfdBRS = canmsg.baudrateSwitch;
+			can.status.canfdBRS = canmsg->baudrateSwitch;
 			break;
 		}
 		default:
@@ -33,3 +33,24 @@ neomessage_t icsneo::CreateNeoMessage(const Message& message) {
 
 	return neomsg;
 }
+
+std::shared_ptr<Message> 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<CANMessage>();
+			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<Message>();
+	}
+}

device/device.cpp

@@ -191,6 +191,22 @@ bool Device::goOffline() {
 	return true;
 }
 
+bool Device::transmit(std::shared_ptr<Message> message) {
+	std::vector<uint8_t> packet;
+	if(!com->encoder->encode(packet, message))
+		return false;
+	
+	return com->sendPacket(packet);
+}
+
+bool Device::transmit(std::vector<std::shared_ptr<Message>> messages) {
+	for(auto& message : messages) {
+		if(!transmit(message))
+			return false;
+	}
+	return true;
+}
+
 void Device::handleInternalMessage(std::shared_ptr<Message> message) {
 	switch(message->network.getNetID()) {
 		case Network::NetID::Reset_Status:
@@ -202,17 +218,13 @@ void Device::handleInternalMessage(std::shared_ptr<Message> message) {
 }
 
 void Device::updateLEDState() {
-	auto msg = std::make_shared<Message>();
-	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<Message>();
+	msg->network = Network::NetID::Device;
 	msg->data = {0x00, 0x06, uint8_t(ledState)};
-	std::vector<uint8_t> packet;
+	transmit(msg);
-	if(!com->encoder->encode(packet, msg))
-		return;
-		
-	com->sendPacket(packet);
 }

device/include/device.h

@@ -55,6 +55,9 @@ public:
 		enforcePollingMessageLimit();
 	}
 
+	bool transmit(std::shared_ptr<Message> message);
+	bool transmit(std::vector<std::shared_ptr<Message>> messages);
+
 	void handleInternalMessage(std::shared_ptr<Message> message);
 
 	std::unique_ptr<IDeviceSettings> settings;