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

Separate MessageDecoder from Packetizer and optimize 

This will, in the future, allow overriding of MessageDecoder per device as necessary.
pull/4/head
Paul Hollinsky 2018-09-13 19:39:19 -04:00
parent e2e5017331
commit 9f43e9e39e
9 changed files with 440 additions and 374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -30,7 +30,9 @@ else()
endif()
set(COMMON_SRC
communication/message/neomessage.cpp
communication/messagedecoder.cpp
communication/packetizer.cpp
communication/multichannelcommunication.cpp
communication/communication.cpp
communication/icommunication.cpp

7
communication/communication.cpp
View File

@ -5,6 +5,7 @@
#include <iomanip>
#include <cstring>
#include "communication/include/messagedecoder.h"
#include "communication/include/packetizer.h"
using namespace icsneo;
@ -86,13 +87,15 @@ bool Communication::removeMessageCallback(int id) {
void Communication::readTask() {
std::vector<uint8_t> readBytes;
Packetizer packetizer;
MessageDecoder decoder;
while(!closing) {
readBytes.clear();
if(impl->readWait(readBytes)) {
if(decoder.input(readBytes)) {
for(auto& msg : decoder.output()) {
if(packetizer.input(readBytes)) {
for(auto& packet : packetizer.output()) {
auto msg = decoder.decodePacket(packet);
for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
if(!closing) {
cb.second.callIfMatch(msg);

6
communication/include/communication.h
View File

@ -40,6 +40,12 @@ public:
void setAlign16Bit(bool enable) { align16bit = enable; }
class Packet {
public:
Network network;
std::vector<uint8_t> data;
};
protected:
std::shared_ptr<ICommunication> impl;
static int messageCallbackIDCounter;

484
communication/include/messagedecoder.h
View File

@ -1,6 +1,7 @@
#ifndef __MESSAGEDECODER_H_
#define __MESSAGEDECODER_H_
#include "communication/include/communication.h"
#include "communication/message/include/message.h"
#include "communication/message/include/canmessage.h"
#include "communication/include/network.h"
@ -12,245 +13,258 @@ namespace icsneo {
class MessageDecoder {
public:
bool input(const std::vector<uint8_t>& bytes);
std::vector<std::shared_ptr<Message>> output();
std::shared_ptr<Message> decodePacket(const std::shared_ptr<Communication::Packet>& message);
private:
enum class ReadState {
SearchForHeader,
ParseHeader,
ParseLongStylePacketHeader,
GetData
};
ReadState state = ReadState::SearchForHeader;
int currentIndex = 0;
int messageLength = 0;
int headerSize = 0;
bool checksum = false;
bool gotGoodMessages = false; // Tracks whether we've ever gotten a good message
Message message;
std::deque<uint8_t> bytes;
void processMessage(const Message& message);
std::vector<std::shared_ptr<Message>> processedMessages;
typedef uint16_t icscm_bitfield;
struct CoreMiniMsg {
CANMessage toCANMessage(Network netid);
union {
uint16_t CxTRB0SID16;
struct
{
icscm_bitfield IDE : 1;
icscm_bitfield SRR : 1;
icscm_bitfield SID : 11;
icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS
} CxTRB0SID;
struct
{
icscm_bitfield : 13;
icscm_bitfield EDL : 1;
icscm_bitfield BRS : 1;
icscm_bitfield ESI : 1;
} CxTRB0FD;
struct
{
icscm_bitfield ErrRxOnlyBreak : 1;
icscm_bitfield ErrRxOnlyBreakSync : 1;
icscm_bitfield ID : 11;
icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS
} CxLIN3;
struct
{
uint8_t D8;
uint8_t options : 4;
uint8_t TXMSG : 1;
uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS
} C1xJ1850;
struct
{
uint8_t D8;
uint8_t options : 4;
uint8_t TXMSG : 1;
uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS
} C1xISO;
struct
{
uint8_t D8;
uint8_t options : 4;
uint8_t TXMSG : 1;
uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS
} C1xJ1708;
struct
{
icscm_bitfield FCS_AVAIL : 1;
icscm_bitfield RUNT_FRAME : 1;
icscm_bitfield DISABLE_PADDING : 1;
icscm_bitfield PREEMPTION_ENABLED : 1;
icscm_bitfield MPACKET_TYPE : 4;
icscm_bitfield MPACKET_FRAG_CNT : 2;
icscm_bitfield : 6;
} C1xETH;
struct
{
uint16_t ID : 11;
uint16_t STARTUP : 1;
uint16_t SYNC : 1;
uint16_t NULL_FRAME : 1;
uint16_t PAYLOAD_PREAMBLE : 1;
uint16_t RESERVED_0 : 1;
} C1xFlex;
struct
{
uint8_t daqType;
uint8_t ethDaqRes1;
} C1xETHDAQ;
};
union {
uint16_t CxTRB0EID16;
struct
{
icscm_bitfield EID : 12;
icscm_bitfield TXMSG : 1;
icscm_bitfield TXAborted : 1;
icscm_bitfield TXLostArb : 1;
icscm_bitfield TXError : 1;
} CxTRB0EID;
struct
{
uint8_t LINByte9;
uint8_t ErrTxRxMismatch : 1;
uint8_t TxChkSumEnhanced : 1;
uint8_t TXMaster : 1;
uint8_t TXSlave : 1;
uint8_t ErrRxBreakNot0 : 1;
uint8_t ErrRxBreakTooShort : 1;
uint8_t ErrRxSyncNot55 : 1;
uint8_t ErrRxDataGreater8 : 1;
} CxLIN;
struct
{
uint8_t D9;
uint8_t D10;
} C2xJ1850;
struct
{
uint8_t D9;
uint8_t D10;
} C2xISO;
struct
{
uint8_t D9;
uint8_t D10;
} C2xJ1708;
struct
{
uint16_t txlen : 12;
uint16_t TXMSG : 1;
uint16_t : 3;
} C2xETH;
struct
{
uint16_t HDR_CRC_10 : 1;
uint16_t PAYLOAD_LEN : 7;
uint16_t RESERVED_1 : 4;
uint16_t TXMSG : 1;
uint16_t RESERVED_2 : 3;
} C2xFlex;
};
union {
// For use by CAN
uint16_t CxTRB0DLC16;
struct
{
icscm_bitfield DLC : 4;
icscm_bitfield RB0 : 1;
icscm_bitfield IVRIF : 1;
icscm_bitfield HVEnable : 1;// must be cleared before passing into CAN driver
icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS
icscm_bitfield RB1 : 1;
icscm_bitfield RTR : 1;
icscm_bitfield EID : 6;
} CxTRB0DLC;
struct
{
icscm_bitfield len : 4;
icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
icscm_bitfield UpdateSlaveOnce : 1;
icscm_bitfield HasUpdatedSlaveOnce : 1;
icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS
icscm_bitfield BusRecovered : 1;
icscm_bitfield SyncFerr : 1;//!< We got framing error in our sync byte.
icscm_bitfield MidFerr : 1;//!< We got framing error in our message id.
icscm_bitfield SlaveByteFerr : 1;//!< We got framing error in one of our slave bytes.
icscm_bitfield TxAborted : 1;//!< This transmit was aborted.
icscm_bitfield breakOnly : 1;
icscm_bitfield : 2;
} CxLIN2;
// For use by JVPW
struct
{
icscm_bitfield len : 4;
icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
icscm_bitfield just_tx_timestamp : 1;
icscm_bitfield first_seg : 1;
icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit
icscm_bitfield D11 : 8;
} C3xJ1850;
// For use by the ISO/KEYWORD
struct
{
icscm_bitfield len : 4;
icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
icscm_bitfield FRM : 1;
icscm_bitfield INIT : 1;
icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit
icscm_bitfield D11 : 8;
} C3xISO;
struct
{
icscm_bitfield len : 4;
icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
icscm_bitfield FRM : 1;
icscm_bitfield : 1;
icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit
icscm_bitfield pri : 8;
} C3xJ1708;
struct
{
uint16_t rsvd;
} C3xETH;
struct
{
uint16_t CYCLE : 6;
uint16_t HDR_CRC_9_0 : 10;
} C3xFlex;
};
unsigned char CxTRB0Dall[8];
union {
uint16_t CxTRB0STAT;
uint16_t J1850_TX_ID;
};
union {
struct
{
uint32_t uiTimeStamp10uS;
union {
uint32_t uiTimeStamp10uSMSB;
struct
{
unsigned : 28;
unsigned res_0s : 3;// must be 0!!!
unsigned bIsExtended : 1;// always 1 for CoreMiniMsgExtended.
};
};
};
int64_t uiTimeStampLarge;
uint8_t uiTimeStampBytes[8];
};
struct HardwareCANPacket {
struct {
icscm_bitfield IDE : 1;
icscm_bitfield SRR : 1;
icscm_bitfield SID : 11;
icscm_bitfield EDL : 1;
icscm_bitfield BRS : 1;
icscm_bitfield ESI : 1;
} header;
struct {
icscm_bitfield EID : 12;
icscm_bitfield TXMSG : 1;
icscm_bitfield TXAborted : 1;
icscm_bitfield TXLostArb : 1;
icscm_bitfield TXError : 1;
} eid;
struct {
icscm_bitfield DLC : 4;
icscm_bitfield RB0 : 1;
icscm_bitfield IVRIF : 1;
icscm_bitfield HVEnable : 1;// must be cleared before passing into CAN driver
icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS
icscm_bitfield RB1 : 1;
icscm_bitfield RTR : 1;
icscm_bitfield EID : 6;
} dlc;
unsigned char data[8];
struct {
uint64_t TS : 60;
uint64_t : 3;
uint64_t IsExtended : 1;
} timestamp;
};
// struct CoreMiniMsg {
// CANMessage toCANMessage(Network netid);
// union {
// uint16_t CxTRB0SID16;
// struct
// {
// icscm_bitfield IDE : 1;
// icscm_bitfield SRR : 1;
// icscm_bitfield SID : 11;
// icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS
// } CxTRB0SID;
// struct
// {
// icscm_bitfield : 13;
// icscm_bitfield EDL : 1;
// icscm_bitfield BRS : 1;
// icscm_bitfield ESI : 1;
// } CxTRB0FD;
// struct
// {
// icscm_bitfield ErrRxOnlyBreak : 1;
// icscm_bitfield ErrRxOnlyBreakSync : 1;
// icscm_bitfield ID : 11;
// icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS
// } CxLIN3;
// struct
// {
// uint8_t D8;
// uint8_t options : 4;
// uint8_t TXMSG : 1;
// uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS
// } C1xJ1850;
// struct
// {
// uint8_t D8;
// uint8_t options : 4;
// uint8_t TXMSG : 1;
// uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS
// } C1xISO;
// struct
// {
// uint8_t D8;
// uint8_t options : 4;
// uint8_t TXMSG : 1;
// uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS
// } C1xJ1708;
// struct
// {
// icscm_bitfield FCS_AVAIL : 1;
// icscm_bitfield RUNT_FRAME : 1;
// icscm_bitfield DISABLE_PADDING : 1;
// icscm_bitfield PREEMPTION_ENABLED : 1;
// icscm_bitfield MPACKET_TYPE : 4;
// icscm_bitfield MPACKET_FRAG_CNT : 2;
// icscm_bitfield : 6;
// } C1xETH;
// struct
// {
// uint16_t ID : 11;
// uint16_t STARTUP : 1;
// uint16_t SYNC : 1;
// uint16_t NULL_FRAME : 1;
// uint16_t PAYLOAD_PREAMBLE : 1;
// uint16_t RESERVED_0 : 1;
// } C1xFlex;
// struct
// {
// uint8_t daqType;
// uint8_t ethDaqRes1;
// } C1xETHDAQ;
// };
// union {
// uint16_t CxTRB0EID16;
// struct
// {
// icscm_bitfield EID : 12;
// icscm_bitfield TXMSG : 1;
// icscm_bitfield TXAborted : 1;
// icscm_bitfield TXLostArb : 1;
// icscm_bitfield TXError : 1;
// } CxTRB0EID;
// struct
// {
// uint8_t LINByte9;
// uint8_t ErrTxRxMismatch : 1;
// uint8_t TxChkSumEnhanced : 1;
// uint8_t TXMaster : 1;
// uint8_t TXSlave : 1;
// uint8_t ErrRxBreakNot0 : 1;
// uint8_t ErrRxBreakTooShort : 1;
// uint8_t ErrRxSyncNot55 : 1;
// uint8_t ErrRxDataGreater8 : 1;
// } CxLIN;
// struct
// {
// uint8_t D9;
// uint8_t D10;
// } C2xJ1850;
// struct
// {
// uint8_t D9;
// uint8_t D10;
// } C2xISO;
// struct
// {
// uint8_t D9;
// uint8_t D10;
// } C2xJ1708;
// struct
// {
// uint16_t txlen : 12;
// uint16_t TXMSG : 1;
// uint16_t : 3;
// } C2xETH;
// struct
// {
// uint16_t HDR_CRC_10 : 1;
// uint16_t PAYLOAD_LEN : 7;
// uint16_t RESERVED_1 : 4;
// uint16_t TXMSG : 1;
// uint16_t RESERVED_2 : 3;
// } C2xFlex;
// };
// union {
// // For use by CAN
// uint16_t CxTRB0DLC16;
// struct
// {
// icscm_bitfield DLC : 4;
// icscm_bitfield RB0 : 1;
// icscm_bitfield IVRIF : 1;
// icscm_bitfield HVEnable : 1;// must be cleared before passing into CAN driver
// icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS
// icscm_bitfield RB1 : 1;
// icscm_bitfield RTR : 1;
// icscm_bitfield EID : 6;
// } CxTRB0DLC;
// struct
// {
// icscm_bitfield len : 4;
// icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
// icscm_bitfield UpdateSlaveOnce : 1;
// icscm_bitfield HasUpdatedSlaveOnce : 1;
// icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS
// icscm_bitfield BusRecovered : 1;
// icscm_bitfield SyncFerr : 1;//!< We got framing error in our sync byte.
// icscm_bitfield MidFerr : 1;//!< We got framing error in our message id.
// icscm_bitfield SlaveByteFerr : 1;//!< We got framing error in one of our slave bytes.
// icscm_bitfield TxAborted : 1;//!< This transmit was aborted.
// icscm_bitfield breakOnly : 1;
// icscm_bitfield : 2;
// } CxLIN2;
// // For use by JVPW
// struct
// {
// icscm_bitfield len : 4;
// icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
// icscm_bitfield just_tx_timestamp : 1;
// icscm_bitfield first_seg : 1;
// icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit
// icscm_bitfield D11 : 8;
// } C3xJ1850;
// // For use by the ISO/KEYWORD
// struct
// {
// icscm_bitfield len : 4;
// icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
// icscm_bitfield FRM : 1;
// icscm_bitfield INIT : 1;
// icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit
// icscm_bitfield D11 : 8;
// } C3xISO;
// struct
// {
// icscm_bitfield len : 4;
// icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
// icscm_bitfield FRM : 1;
// icscm_bitfield : 1;
// icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit
// icscm_bitfield pri : 8;
// } C3xJ1708;
// struct
// {
// uint16_t rsvd;
// } C3xETH;
// struct
// {
// uint16_t CYCLE : 6;
// uint16_t HDR_CRC_9_0 : 10;
// } C3xFlex;
// };
// unsigned char CxTRB0Dall[8];
// union {
// uint16_t CxTRB0STAT;
// uint16_t J1850_TX_ID;
// };
// union {
// struct
// {
// uint32_t uiTimeStamp10uS;
// union {
// uint32_t uiTimeStamp10uSMSB;
// struct
// {
// unsigned : 28;
// unsigned res_0s : 3;// must be 0!!!
// unsigned bIsExtended : 1;// always 1 for CoreMiniMsgExtended.
// };
// };
// };
// int64_t uiTimeStampLarge;
// uint8_t uiTimeStampBytes[8];
// };
// };
};
}

38
communication/include/packetizer.h 100644
View File

@ -0,0 +1,38 @@
#ifndef __PACKETIZER_H_
#define __PACKETIZER_H_
#include "communication/include/communication.h"
#include <queue>
#include <vector>
#include <memory>
namespace icsneo {
class Packetizer {
public:
bool input(const std::vector<uint8_t>& bytes);
std::vector<std::shared_ptr<Communication::Packet>> output();
private:
enum class ReadState {
SearchForHeader,
ParseHeader,
ParseLongStylePacketHeader,
GetData
};
ReadState state = ReadState::SearchForHeader;
int currentIndex = 0;
int packetLength = 0;
int headerSize = 0;
bool checksum = false;
bool gotGoodPackets = false; // Tracks whether we've ever gotten a good packet
Communication::Packet packet;
std::deque<uint8_t> bytes;
std::vector<std::shared_ptr<Communication::Packet>> processedPackets;
};
}
#endif

1
communication/message/include/message.h
View File

@ -11,6 +11,7 @@ public:
virtual ~Message() {}
Network network;
std::vector<uint8_t> data;
uint64_t timestamp;
};
};

145
communication/messagedecoder.cpp
View File

@ -4,136 +4,29 @@
using namespace icsneo;
CANMessage MessageDecoder::CoreMiniMsg::toCANMessage(Network network) {
CANMessage msg;
msg.network = network;
msg.arbid = CxTRB0SID.SID;
msg.data.reserve(CxTRB0DLC.DLC);
for(auto i = 0; i < CxTRB0DLC.DLC; i++)
msg.data.push_back(CxTRB0Dall[i]);
return msg;
}
std::shared_ptr<Message> MessageDecoder::decodePacket(const std::shared_ptr<Communication::Packet>& packet) {
switch(packet->network.getType()) {
case Network::Type::CAN: {
if(packet->data.size() < 24)
break; // We would read garbage when interpereting the data
bool MessageDecoder::input(const std::vector<uint8_t>& inputBytes) {
bool haveEnoughData = true;
bytes.insert(bytes.end(), inputBytes.begin(), inputBytes.end());
HardwareCANPacket* data = (HardwareCANPacket*)packet->data.data();
auto msg = std::make_shared<CANMessage>();
msg->network = packet->network;
msg->arbid = data->header.SID;
msg->data.reserve(data->dlc.DLC);
while(haveEnoughData) {
switch(state) {
case ReadState::SearchForHeader:
if(bytes.size() < 1) {
haveEnoughData = false;
break;
}
// Timestamp calculation
msg->timestamp = data->timestamp.TS * 25; // Timestamps are in 25ns increments since 1/1/2007 GMT 00:00:00.0000
if(bytes[0] == 0xAA) { // 0xAA denotes the beginning of a packet
state = ReadState::ParseHeader;
currentIndex = 1;
} else {
//std::cout << (int)bytes[0] << " ";
bytes.pop_front(); // Discard
}
break;
case ReadState::ParseHeader:
if(bytes.size() < 2) {
haveEnoughData = false;
break;
}
messageLength = bytes[1] >> 4 & 0xf; // Upper nibble of the second byte denotes the message length
message.network = Network(bytes[1] & 0xf); // Lower nibble of the second byte is the network ID
if(messageLength == 0) { // A length of zero denotes a long style packet
state = ReadState::ParseLongStylePacketHeader;
checksum = false;
headerSize = 6;
} else {
state = ReadState::GetData;
checksum = true;
headerSize = 2;
messageLength += 2; // The message length given in short messages does not include header
}
currentIndex++;
break;
case ReadState::ParseLongStylePacketHeader:
if(bytes.size() < 6) {
haveEnoughData = false;
break;
}
messageLength = bytes[2]; // Long messages have a little endian length on bytes 3 and 4
messageLength |= bytes[3] << 8;
message.network = Network((bytes[5] << 8) | bytes[4]); // Long messages have their netid stored as little endian on bytes 5 and 6
currentIndex += 4;
/* Long messages can't have a length less than 4, because that would indicate a negative payload size.
* Unlike the short message length, the long message length encompasses everything from the 0xAA to the
* end of the payload. The short message length, for reference, only encompasses the length of the actual
* payload, and not the header or checksum.
*/
if(messageLength < 4 || messageLength > 4000) {
bytes.pop_front();
//std::cout << "skipping long message with length " << messageLength << std::endl;
state = ReadState::SearchForHeader;
} else {
state = ReadState::GetData;
}
break;
case ReadState::GetData:
// We do not include the checksum in messageLength so it doesn't get copied into the payload buffer
if(bytes.size() < messageLength + (checksum ? 1 : 0)) { // Read until we have the rest of the message
haveEnoughData = false;
break;
}
message.data.clear();
if(messageLength > 0)
message.data.reserve(messageLength - headerSize);
while(currentIndex < messageLength)
message.data.push_back(bytes[currentIndex++]);
if(!checksum || bytes[currentIndex] == Communication::ICSChecksum(message.data)) {
// Got a good packet
gotGoodMessages = true;
processMessage(message);
for (auto i = 0; i < messageLength; i++)
bytes.pop_front();
} else {
if(gotGoodMessages) // Don't complain unless we've already gotten a good message, in case we started in the middle of a stream
std::cout << "Dropping message due to bad checksum" << std::endl;
bytes.pop_front(); // Drop the first byte so it doesn't get picked up again
}
// Reset for the next packet
currentIndex = 0;
state = ReadState::SearchForHeader;
break;
for(auto i = 0; i < data->dlc.DLC; i++)
msg->data.push_back(data->data[i]);
return msg;
}
}
return processedMessages.size() > 0;
}
std::vector<std::shared_ptr<Message>> MessageDecoder::output() {
auto ret = std::move(processedMessages);
processedMessages = std::vector<std::shared_ptr<Message>>(); // Reset the vector
return ret;
}
void MessageDecoder::processMessage(const Message& msg) {
switch(msg.network.getType()) {
case Network::Type::CAN:
if(msg.data.size() >= 24) {
CoreMiniMsg* cmsg = (CoreMiniMsg*)msg.data.data();
processedMessages.push_back(std::make_shared<CANMessage>(cmsg->toCANMessage(msg.network)));
} else {
//std::cout << "bad CAN frame " << msg.data.size() << std::endl;
}
break;
default:
// if(msg.network.getNetID() != Network::NetID::Device)
// std::cout << "Message: " << msg.network << " with data length " << msg.data.size() << std::endl;
processedMessages.push_back(std::make_shared<Message>(msg));
}
auto msg = std::make_shared<Message>();
msg->network = packet->network;
msg->data = packet->data;
return msg;
}

20
communication/multichannelcommunication.cpp
View File

@ -1,5 +1,6 @@
#include "communication/include/multichannelcommunication.h"
#include "communication/include/messagedecoder.h"
#include "communication/include/packetizer.h"
#include <iostream>
#include <iomanip>
@ -30,6 +31,7 @@ void MultiChannelCommunication::readTask() {
std::deque<uint8_t> usbReadFifo;
std::vector<uint8_t> readBytes;
std::vector<uint8_t> payloadBytes;
Packetizer packetizer;
MessageDecoder decoder;
while(!closing) {
@ -95,25 +97,21 @@ void MultiChannelCommunication::readTask() {
continue;
}
//std::cout << std::dec << "Got a multichannel message! Size: " << currentCommandLength << std::hex << std::setfill('0') << std::setw(2) << " Cmd: 0x" << (int)currentCommandType << std::endl;
for(auto i = 0; i < currentReadIndex; i++)
usbReadFifo.pop_front();
payloadBytes.clear();
payloadBytes.reserve(currentCommandLength);
payloadBytes.resize(currentCommandLength);
for(auto i = 0; i < currentCommandLength; i++) {
//std::cout << (int)usbReadFifo[0] << ' ';
payloadBytes.push_back(usbReadFifo[0]);
// if(i % 16 == 15)
// std::cout << std::endl;
payloadBytes[i] = usbReadFifo[0];
usbReadFifo.pop_front();
}
//std::cout << std::dec << std::endl;
if(decoder.input(payloadBytes)) {
for(auto& msg : decoder.output()) {
for(auto& cb : messageCallbacks) {
if(!closing) { // We might have closed while reading or processing
if(packetizer.input(payloadBytes)) {
for(auto& packet : packetizer.output()) {
auto msg = decoder.decodePacket(packet);
for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
if(!closing) {
cb.second.callIfMatch(msg);
}
}

111
communication/packetizer.cpp 100644
View File

@ -0,0 +1,111 @@
#include "communication/include/packetizer.h"
#include <iostream>
using namespace icsneo;
bool Packetizer::input(const std::vector<uint8_t>& inputBytes) {
bool haveEnoughData = true;
bytes.insert(bytes.end(), inputBytes.begin(), inputBytes.end());
while(haveEnoughData) {
switch(state) {
case ReadState::SearchForHeader:
if(bytes.size() < 1) {
haveEnoughData = false;
break;
}
if(bytes[0] == 0xAA) { // 0xAA denotes the beginning of a packet
state = ReadState::ParseHeader;
currentIndex = 1;
} else {
bytes.pop_front(); // Discard
}
break;
case ReadState::ParseHeader:
if(bytes.size() < 2) {
haveEnoughData = false;
break;
}
packetLength = bytes[1] >> 4 & 0xf; // Upper nibble of the second byte denotes the packet length
packet.network = Network(bytes[1] & 0xf); // Lower nibble of the second byte is the network ID
if(packetLength == 0) { // A length of zero denotes a long style packet
state = ReadState::ParseLongStylePacketHeader;
checksum = false;
headerSize = 6;
} else {
state = ReadState::GetData;
checksum = true;
headerSize = 2;
packetLength += 2; // The packet length given in short packets does not include header
}
currentIndex++;
break;
case ReadState::ParseLongStylePacketHeader:
if(bytes.size() < 6) {
haveEnoughData = false;
break;
}
packetLength = bytes[2]; // Long packets have a little endian length on bytes 3 and 4
packetLength |= bytes[3] << 8;
packet.network = Network((bytes[5] << 8) | bytes[4]); // Long packets have their netid stored as little endian on bytes 5 and 6
currentIndex += 4;
/* Long packets can't have a length less than 4, because that would indicate a negative payload size.
* Unlike the short packet length, the long packet length encompasses everything from the 0xAA to the
* end of the payload. The short packet length, for reference, only encompasses the length of the actual
* payload, and not the header or checksum.
*/
if(packetLength < 4 || packetLength > 4000) {
bytes.pop_front();
//std::cout << "skipping long packet with length " << packetLength << std::endl;
state = ReadState::SearchForHeader;
} else {
state = ReadState::GetData;
}
break;
case ReadState::GetData:
// We do not include the checksum in packetLength so it doesn't get copied into the payload buffer
if(bytes.size() < packetLength + (checksum ? 1 : 0)) { // Read until we have the rest of the packet
haveEnoughData = false;
break;
}
packet.data.clear();
if(packetLength > 0)
packet.data.resize(packetLength - headerSize);
auto i = 0;
while(currentIndex < packetLength)
packet.data[i++] = bytes[currentIndex++];
if(!checksum || bytes[currentIndex] == Communication::ICSChecksum(packet.data)) {
// Got a good packet
gotGoodPackets = true;
processedPackets.push_back(std::make_shared<Communication::Packet>(packet));
for (auto i = 0; i < packetLength; i++)
bytes.pop_front();
} else {
if(gotGoodPackets) // Don't complain unless we've already gotten a good packet, in case we started in the middle of a stream
std::cout << "Dropping packet due to bad checksum" << std::endl;
bytes.pop_front(); // Drop the first byte so it doesn't get picked up again
}
// Reset for the next packet
currentIndex = 0;
state = ReadState::SearchForHeader;
break;
}
}
return processedPackets.size() > 0;
}
std::vector<std::shared_ptr<Communication::Packet>> Packetizer::output() {
auto ret = std::move(processedPackets);
processedPackets = std::vector<std::shared_ptr<Communication::Packet>>(); // Reset the vector
return ret;
}