Testing the encoder for sending more complex messages

CMakeLists.txt

@@ -42,6 +42,7 @@ endif()
 set(COMMON_SRC
 	communication/message/neomessage.cpp
 	communication/decoder.cpp
+	communication/encoder.cpp
 	communication/packetizer.cpp
 	communication/multichannelcommunication.cpp
 	communication/communication.cpp

communication/communication.cpp

@@ -46,16 +46,20 @@ bool Communication::close() {
 }
 
 bool Communication::sendPacket(std::vector<uint8_t>& bytes) {
-	return rawWrite(packetizer->packetWrap(bytes));
+	std::cout << "\nWriting " << bytes.size() << " bytes\n" << std::hex;
+	for(size_t i = 0; i < bytes.size(); i++)
+		std::cout << std::setw(2) << std::setfill('0') << (int)bytes[i] << (i % 16 == 15 ? '\n' : ' ');
+	std::cout << '\n' << std::endl << std::dec;
+	return rawWrite(bytes);
 }
 
 bool Communication::sendCommand(Command cmd, std::vector<uint8_t> arguments) {
-	std::vector<uint8_t> bytes;
+	auto msg = std::make_shared<Message>();
-	bytes.push_back((uint8_t)cmd);
+	msg->network = Network::NetID::Main51;
-	for(auto& b : arguments)
+	msg->data = std::move(arguments);
-		bytes.push_back(b);
+	msg->data.insert(msg->data.begin(), (uint8_t)cmd);
-	bytes.insert(bytes.begin(), (uint8_t)Network::NetID::Main51 | ((uint8_t)bytes.size() << 4));
+	auto packet = encoder->encode(msg);
-	return sendPacket(bytes);
+	return sendPacket(packet);
 }
 
 bool Communication::getSettingsSync(std::vector<uint8_t>& data, std::chrono::milliseconds timeout) {
@@ -130,6 +134,7 @@ void Communication::readTask() {
 			if(packetizer->input(readBytes)) {
 				for(auto& packet : packetizer->output()) {
 					auto msg = decoder->decodePacket(packet);
+					std::cout << "Got packet for " << msg->network << ", calling " << messageCallbacks.size() << " callbacks" << std::endl;
 					for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
 						if(!closing) {
 							cb.second.callIfMatch(msg);

communication/decoder.cpp

@@ -64,6 +64,7 @@ std::shared_ptr<Message> Decoder::decodePacket(const std::shared_ptr<Packet>& pa
 					 */
 					uint16_t length = packet->data[0] | (packet->data[1] << 8);
 					packet->network = Network(packet->data[2] & 0xF);
+					std::cout << "Got an old format packet, decoding against " << packet->network << std::endl;
 					packet->data.erase(packet->data.begin(), packet->data.begin() + 3);
 					if(packet->data.size() != length)
 						packet->data.resize(length);

communication/encoder.cpp

@@ -2,7 +2,8 @@
 
 using namespace icsneo;
 
-std::shared_ptr<Message> Encoder::encode(const std::shared_ptr<Message>& message) {
+std::vector<uint8_t> Encoder::encode(const std::shared_ptr<Message>& message) {
+	bool shortFormat = false;
 	switch(message->network.getType()) {
 		// case Network::Type::CAN: {
 		// 	if(message->data.size() < 24)
@@ -24,34 +25,40 @@ std::shared_ptr<Message> Encoder::encode(const std::shared_ptr<Message>& message
 		default:
 			switch(message->network.getNetID()) {
 				case Network::NetID::Main51:
-					if(message->data.size() > 0xF) {
+					if(message->data.size() <= 0xF)
-						message->network = Network::NetID::RED_OLDFORMAT;
+						shortFormat = true;
-						message->data.insert(message->data.begin(), 0x10 | (uint8_t)Network::NetID::Main51);
-						return encode(message);
-					}
-					message->data.insert(message->data.begin(), (message->data.size() << 4) | (uint8_t)Network::NetID::Main51);
-					data.in
 					break;
 				case Network::NetID::RED_OLDFORMAT: {
 					// See the decoder for an explanation
-					uint16_t length = message->data[0] | (message->data[1] << 8);
+					// We expect the network byte to be populated already in data, but not the length
-					message->network = Network(message->data[2] & 0xF);
+					uint16_t length = message->data.size() - 1;
-					message->data.erase(message->data.begin(), message->data.begin() + 3);
+					message->data.insert(message->data.begin(), {(uint8_t)length, (uint8_t)(length >> 8)});
-					if(message->data.size() != length)
-						message->data.resize(length);
-					return decodePacket(message);
 				}
+				default:
+					break;
 			}
 			break;
 	}
 
-	auto msg = std::make_shared<Message>();
+	if(shortFormat) {
-	msg->network = message->network;
+		message->data.insert(message->data.begin(), (message->data.size() << 4) | (uint8_t)message->network.getNetID());
-	msg->data = message->data;
+	} else {
-	return msg;
+		// 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 = message->data.size() + 1 + 1 + 2 + 2;
+		message->data.insert(message->data.begin(), {
+			(uint8_t)Network::NetID::RED, // 0x0C for long message
+			(uint8_t)size, // Size, little endian 16-bit
+			(uint8_t)(size >> 8),
+			(uint8_t)message->network.getNetID(), // NetID, little endian 16-bit
+			(uint8_t)(uint16_t(message->network.getNetID()) >> 8)
+		});
 	}
 
-std::vector<uint8_t> Encoder::encode(Command cmd, std::vector<uint8_t> arguments = {}) {
+	return packetizer->packetWrap(message->data, shortFormat);
+}
+
+std::vector<uint8_t> Encoder::encode(Command cmd, std::vector<uint8_t> arguments) {
 	auto msg = std::make_shared<Message>();
 	msg->network = Network::NetID::Main51;
 	msg->data.resize(arguments.size() + 1);

communication/include/communication.h

@@ -8,6 +8,7 @@
 #include "communication/message/callback/include/messagecallback.h"
 #include "communication/message/include/serialnumbermessage.h"
 #include "communication/include/packetizer.h"
+#include "communication/include/encoder.h"
 #include "communication/include/decoder.h"
 #include <memory>
 #include <vector>
@@ -20,7 +21,11 @@ namespace icsneo {
 
 class Communication {
 public:
-	Communication(std::shared_ptr<ICommunication> com, std::shared_ptr<Packetizer> p, std::shared_ptr<Decoder> md) : packetizer(p), decoder(md), impl(com) {}
+	Communication(
+		std::shared_ptr<ICommunication> com,
+		std::shared_ptr<Packetizer> p,
+		std::shared_ptr<Encoder> e,
+		std::shared_ptr<Decoder> md) : packetizer(p), encoder(e), decoder(md), impl(com) {}
 	virtual ~Communication() { close(); }
 
 	bool open();
@@ -43,6 +48,7 @@ public:
 	std::shared_ptr<Message> waitForMessageSync(std::shared_ptr<MessageFilter> f, std::chrono::milliseconds timeout = std::chrono::milliseconds(50));
 
 	std::shared_ptr<Packetizer> packetizer;
+	std::shared_ptr<Encoder> encoder;
 	std::shared_ptr<Decoder> decoder;
 
 protected:

communication/include/encoder.h

@@ -4,7 +4,9 @@
 #include "communication/message/include/message.h"
 #include "communication/message/include/canmessage.h"
 #include "communication/include/packet.h"
+#include "communication/include/command.h"
 #include "communication/include/network.h"
+#include "communication/include/packetizer.h"
 #include <queue>
 #include <vector>
 #include <memory>
@@ -15,11 +17,14 @@ namespace icsneo {
 
 class Encoder {
 public:
+	Encoder(std::shared_ptr<Packetizer> packetizerInstance) : packetizer(packetizerInstance) {}
 	std::vector<uint8_t> encode(const std::shared_ptr<Message>& message);
 	std::vector<uint8_t> encode(Command cmd, bool boolean) { return encode(cmd, std::vector<uint8_t>({ (uint8_t)boolean })); }
 	std::vector<uint8_t> encode(Command cmd, std::vector<uint8_t> arguments = {});
 	
 private:
+	std::shared_ptr<Packetizer> packetizer;
+
 	typedef uint16_t icscm_bitfield;
 	struct HardwareCANPacket {
 		struct {

communication/include/multichannelcommunication.h

@@ -4,12 +4,17 @@
 #include "communication/include/communication.h"
 #include "communication/include/icommunication.h"
 #include "communication/include/command.h"
+#include "communication/include/encoder.h"
 
 namespace icsneo {
 
 class MultiChannelCommunication : public Communication {
 public:
-	MultiChannelCommunication(std::shared_ptr<ICommunication> com, std::shared_ptr<Packetizer> p, std::shared_ptr<Decoder> md) : Communication(com, p, md) {}
+	MultiChannelCommunication(
+		std::shared_ptr<ICommunication> com,
+		std::shared_ptr<Packetizer> p,
+		std::shared_ptr<Encoder> e,
+		std::shared_ptr<Decoder> md) : Communication(com, p, e, md) {}
 	void spawnThreads() override;
 	void joinThreads() override;
 	bool sendPacket(std::vector<uint8_t>& bytes) override;

communication/include/packetizer.h

@@ -11,7 +11,7 @@ namespace icsneo {
 class Packetizer {
 public:
 	static uint8_t ICSChecksum(const std::vector<uint8_t>& data);
-	std::vector<uint8_t>& packetWrap(std::vector<uint8_t>& data, bool checksum = true);
+	std::vector<uint8_t>& packetWrap(std::vector<uint8_t>& data, bool shortFormat);
 
 	bool input(const std::vector<uint8_t>& bytes);
 	std::vector<std::shared_ptr<Packet>> output();

communication/packetizer.cpp

@@ -1,5 +1,6 @@
 #include "communication/include/packetizer.h"
 #include <iostream>
+#include <iomanip>
 
 using namespace icsneo;
 
@@ -12,12 +13,15 @@ uint8_t Packetizer::ICSChecksum(const std::vector<uint8_t>& data) {
 	return (uint8_t)checksum;
 }
 
-std::vector<uint8_t>& Packetizer::packetWrap(std::vector<uint8_t>& data, bool checksum) {
+std::vector<uint8_t>& Packetizer::packetWrap(std::vector<uint8_t>& data, bool shortFormat) {
-	if(!disableChecksum && checksum)
+	if(shortFormat) {
-		data.push_back(ICSChecksum(data));
+		// Some devices don't use the checksum, so might as well not calculate it if that's the case
+		// Either way the byte is still expected to be present in the bytestream for short messages
+		data.push_back(disableChecksum ? 0x00 : ICSChecksum(data)); 
+	}
 	data.insert(data.begin(), 0xAA);
-	if(align16bit && data.size() % 2 == 1)
+	if(align16bit && data.size() % 2 == 1) // Some devices always expect 16-bit aligned data
-		data.push_back('A');
+		data.push_back('A'); // Used as padding character, ignored by the firmware
 	return data;
 }
 

device/idevicesettings.cpp

@@ -95,7 +95,11 @@ bool IDeviceSettings::send() {
 	bytestream[5] = gs_checksum >> 8;
 	memcpy(bytestream.data() + 6, getRawStructurePointer(), structSize);
 	com->sendCommand(Command::SetSettings, bytestream);
-	std::shared_ptr<Message> msg = com->waitForMessageSync(Main51MessageFilter(Command::SetSettings), std::chrono::milliseconds(500));
+	std::shared_ptr<Message> msg = com->waitForMessageSync(std::make_shared<Main51MessageFilter>(), std::chrono::milliseconds(3000));
+	if(!msg)
+		std::cout << "No response from device for set settings" << std::endl;
+	if(msg && msg->data[1] != 1)
+		std::cout << "Response was incorrect for set settings: " << (int)msg->data[1] << std::endl;
 	if(!msg || msg->data[1] != 1) { // The second byte of the response carries the result, 1 being success
 		refresh(); // Refresh our buffer with what the device has
 		return false;

device/include/device.h

@@ -9,6 +9,7 @@
 #include "device/include/devicetype.h"
 #include "communication/include/communication.h"
 #include "communication/include/packetizer.h"
+#include "communication/include/encoder.h"
 #include "communication/include/decoder.h"
 #include "third-party/concurrentqueue/concurrentqueue.h"
 

device/neovifire2/include/neovifire2eth.h

@@ -14,8 +14,9 @@ public:
 	NeoVIFIRE2ETH(neodevice_t neodevice) : NeoVIFIRE2(neodevice) {
 		auto transport = std::make_shared<PCAP>(getWritableNeoDevice());
 		auto packetizer = std::make_shared<Packetizer>();
+		auto encoder = std::make_shared<Encoder>(packetizer);
 		auto decoder = std::make_shared<Decoder>();
-		com = std::make_shared<Communication>(transport, packetizer, decoder);
+		com = std::make_shared<Communication>(transport, packetizer, encoder, decoder);
 		settings = std::make_shared<NeoVIFIRE2Settings>(com);
 		productId = PRODUCT_ID;
 	}

device/neovifire2/include/neovifire2usb.h

@@ -13,8 +13,9 @@ public:
 	NeoVIFIRE2USB(neodevice_t neodevice) : NeoVIFIRE2(neodevice) {
 		auto transport = std::make_shared<FTDI>(getWritableNeoDevice());
 		auto packetizer = std::make_shared<Packetizer>();
+		auto encoder = std::make_shared<Encoder>(packetizer);
 		auto decoder = std::make_shared<Decoder>();
-		com = std::make_shared<Communication>(transport, packetizer, decoder);
+		com = std::make_shared<Communication>(transport, packetizer, encoder, decoder);
 		settings = std::make_shared<NeoVIFIRE2Settings>(com);
 		productId = PRODUCT_ID;
 	}