Separate MessageDecoder from Packetizer and optimize
This will, in the future, allow overriding of MessageDecoder per device as necessary.pull/4/head
Paul Hollinsky
parent
e2e5017331
commit
9f43e9e39e
|
|
@ -30,7 +30,9 @@ else()
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
set(COMMON_SRC
|
set(COMMON_SRC
|
||||||
|
communication/message/neomessage.cpp
|
||||||
communication/messagedecoder.cpp
|
communication/messagedecoder.cpp
|
||||||
|
communication/packetizer.cpp
|
||||||
communication/multichannelcommunication.cpp
|
communication/multichannelcommunication.cpp
|
||||||
communication/communication.cpp
|
communication/communication.cpp
|
||||||
communication/icommunication.cpp
|
communication/icommunication.cpp
|
||||||
|
|
|
||||||
|
|
@ -5,6 +5,7 @@
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include "communication/include/messagedecoder.h"
|
#include "communication/include/messagedecoder.h"
|
||||||
|
#include "communication/include/packetizer.h"
|
||||||
|
|
||||||
using namespace icsneo;
|
using namespace icsneo;
|
||||||
|
|
||||||
|
|
@ -86,13 +87,15 @@ bool Communication::removeMessageCallback(int id) {
|
||||||
|
|
||||||
void Communication::readTask() {
|
void Communication::readTask() {
|
||||||
std::vector<uint8_t> readBytes;
|
std::vector<uint8_t> readBytes;
|
||||||
|
Packetizer packetizer;
|
||||||
MessageDecoder decoder;
|
MessageDecoder decoder;
|
||||||
|
|
||||||
while(!closing) {
|
while(!closing) {
|
||||||
readBytes.clear();
|
readBytes.clear();
|
||||||
if(impl->readWait(readBytes)) {
|
if(impl->readWait(readBytes)) {
|
||||||
if(decoder.input(readBytes)) {
|
if(packetizer.input(readBytes)) {
|
||||||
for(auto& msg : decoder.output()) {
|
for(auto& packet : packetizer.output()) {
|
||||||
|
auto msg = decoder.decodePacket(packet);
|
||||||
for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
|
for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
|
||||||
if(!closing) {
|
if(!closing) {
|
||||||
cb.second.callIfMatch(msg);
|
cb.second.callIfMatch(msg);
|
||||||
|
|
|
||||||
|
|
@ -40,6 +40,12 @@ public:
|
||||||
|
|
||||||
void setAlign16Bit(bool enable) { align16bit = enable; }
|
void setAlign16Bit(bool enable) { align16bit = enable; }
|
||||||
|
|
||||||
|
class Packet {
|
||||||
|
public:
|
||||||
|
Network network;
|
||||||
|
std::vector<uint8_t> data;
|
||||||
|
};
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::shared_ptr<ICommunication> impl;
|
std::shared_ptr<ICommunication> impl;
|
||||||
static int messageCallbackIDCounter;
|
static int messageCallbackIDCounter;
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,7 @@
|
||||||
#ifndef __MESSAGEDECODER_H_
|
#ifndef __MESSAGEDECODER_H_
|
||||||
#define __MESSAGEDECODER_H_
|
#define __MESSAGEDECODER_H_
|
||||||
|
|
||||||
|
#include "communication/include/communication.h"
|
||||||
#include "communication/message/include/message.h"
|
#include "communication/message/include/message.h"
|
||||||
#include "communication/message/include/canmessage.h"
|
#include "communication/message/include/canmessage.h"
|
||||||
#include "communication/include/network.h"
|
#include "communication/include/network.h"
|
||||||
|
|
@ -12,159 +13,27 @@ namespace icsneo {
|
||||||
|
|
||||||
class MessageDecoder {
|
class MessageDecoder {
|
||||||
public:
|
public:
|
||||||
bool input(const std::vector<uint8_t>& bytes);
|
std::shared_ptr<Message> decodePacket(const std::shared_ptr<Communication::Packet>& message);
|
||||||
std::vector<std::shared_ptr<Message>> output();
|
|
||||||
|
|
||||||
private:
|
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;
|
typedef uint16_t icscm_bitfield;
|
||||||
struct CoreMiniMsg {
|
struct HardwareCANPacket {
|
||||||
CANMessage toCANMessage(Network netid);
|
struct {
|
||||||
union {
|
|
||||||
uint16_t CxTRB0SID16;
|
|
||||||
struct
|
|
||||||
{
|
|
||||||
icscm_bitfield IDE : 1;
|
icscm_bitfield IDE : 1;
|
||||||
icscm_bitfield SRR : 1;
|
icscm_bitfield SRR : 1;
|
||||||
icscm_bitfield SID : 11;
|
icscm_bitfield SID : 11;
|
||||||
icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS
|
|
||||||
} CxTRB0SID;
|
|
||||||
struct
|
|
||||||
{
|
|
||||||
icscm_bitfield : 13;
|
|
||||||
icscm_bitfield EDL : 1;
|
icscm_bitfield EDL : 1;
|
||||||
icscm_bitfield BRS : 1;
|
icscm_bitfield BRS : 1;
|
||||||
icscm_bitfield ESI : 1;
|
icscm_bitfield ESI : 1;
|
||||||
} CxTRB0FD;
|
} header;
|
||||||
struct
|
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 EID : 12;
|
||||||
icscm_bitfield TXMSG : 1;
|
icscm_bitfield TXMSG : 1;
|
||||||
icscm_bitfield TXAborted : 1;
|
icscm_bitfield TXAborted : 1;
|
||||||
icscm_bitfield TXLostArb : 1;
|
icscm_bitfield TXLostArb : 1;
|
||||||
icscm_bitfield TXError : 1;
|
icscm_bitfield TXError : 1;
|
||||||
} CxTRB0EID;
|
} eid;
|
||||||
struct
|
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 DLC : 4;
|
||||||
icscm_bitfield RB0 : 1;
|
icscm_bitfield RB0 : 1;
|
||||||
icscm_bitfield IVRIF : 1;
|
icscm_bitfield IVRIF : 1;
|
||||||
|
|
@ -173,84 +42,229 @@ private:
|
||||||
icscm_bitfield RB1 : 1;
|
icscm_bitfield RB1 : 1;
|
||||||
icscm_bitfield RTR : 1;
|
icscm_bitfield RTR : 1;
|
||||||
icscm_bitfield EID : 6;
|
icscm_bitfield EID : 6;
|
||||||
} CxTRB0DLC;
|
} dlc;
|
||||||
struct
|
unsigned char data[8];
|
||||||
{
|
struct {
|
||||||
icscm_bitfield len : 4;
|
uint64_t TS : 60;
|
||||||
icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS
|
uint64_t : 3;
|
||||||
icscm_bitfield UpdateSlaveOnce : 1;
|
uint64_t IsExtended : 1;
|
||||||
icscm_bitfield HasUpdatedSlaveOnce : 1;
|
} timestamp;
|
||||||
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 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];
|
||||||
|
// };
|
||||||
|
// };
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -11,6 +11,7 @@ public:
|
||||||
virtual ~Message() {}
|
virtual ~Message() {}
|
||||||
Network network;
|
Network network;
|
||||||
std::vector<uint8_t> data;
|
std::vector<uint8_t> data;
|
||||||
|
uint64_t timestamp;
|
||||||
};
|
};
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -4,136 +4,29 @@
|
||||||
|
|
||||||
using namespace icsneo;
|
using namespace icsneo;
|
||||||
|
|
||||||
CANMessage MessageDecoder::CoreMiniMsg::toCANMessage(Network network) {
|
std::shared_ptr<Message> MessageDecoder::decodePacket(const std::shared_ptr<Communication::Packet>& packet) {
|
||||||
CANMessage msg;
|
switch(packet->network.getType()) {
|
||||||
msg.network = network;
|
case Network::Type::CAN: {
|
||||||
msg.arbid = CxTRB0SID.SID;
|
if(packet->data.size() < 24)
|
||||||
msg.data.reserve(CxTRB0DLC.DLC);
|
break; // We would read garbage when interpereting the data
|
||||||
for(auto i = 0; i < CxTRB0DLC.DLC; i++)
|
|
||||||
msg.data.push_back(CxTRB0Dall[i]);
|
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);
|
||||||
|
|
||||||
|
// Timestamp calculation
|
||||||
|
msg->timestamp = data->timestamp.TS * 25; // Timestamps are in 25ns increments since 1/1/2007 GMT 00:00:00.0000
|
||||||
|
|
||||||
|
for(auto i = 0; i < data->dlc.DLC; i++)
|
||||||
|
msg->data.push_back(data->data[i]);
|
||||||
return msg;
|
return msg;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MessageDecoder::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
|
auto msg = std::make_shared<Message>();
|
||||||
state = ReadState::ParseHeader;
|
msg->network = packet->network;
|
||||||
currentIndex = 1;
|
msg->data = packet->data;
|
||||||
} else {
|
return msg;
|
||||||
//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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
@ -1,5 +1,6 @@
|
||||||
#include "communication/include/multichannelcommunication.h"
|
#include "communication/include/multichannelcommunication.h"
|
||||||
#include "communication/include/messagedecoder.h"
|
#include "communication/include/messagedecoder.h"
|
||||||
|
#include "communication/include/packetizer.h"
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
|
|
||||||
|
|
@ -30,6 +31,7 @@ void MultiChannelCommunication::readTask() {
|
||||||
std::deque<uint8_t> usbReadFifo;
|
std::deque<uint8_t> usbReadFifo;
|
||||||
std::vector<uint8_t> readBytes;
|
std::vector<uint8_t> readBytes;
|
||||||
std::vector<uint8_t> payloadBytes;
|
std::vector<uint8_t> payloadBytes;
|
||||||
|
Packetizer packetizer;
|
||||||
MessageDecoder decoder;
|
MessageDecoder decoder;
|
||||||
|
|
||||||
while(!closing) {
|
while(!closing) {
|
||||||
|
|
@ -95,25 +97,21 @@ void MultiChannelCommunication::readTask() {
|
||||||
continue;
|
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++)
|
for(auto i = 0; i < currentReadIndex; i++)
|
||||||
usbReadFifo.pop_front();
|
usbReadFifo.pop_front();
|
||||||
|
|
||||||
payloadBytes.clear();
|
payloadBytes.clear();
|
||||||
payloadBytes.reserve(currentCommandLength);
|
payloadBytes.resize(currentCommandLength);
|
||||||
for(auto i = 0; i < currentCommandLength; i++) {
|
for(auto i = 0; i < currentCommandLength; i++) {
|
||||||
//std::cout << (int)usbReadFifo[0] << ' ';
|
payloadBytes[i] = usbReadFifo[0];
|
||||||
payloadBytes.push_back(usbReadFifo[0]);
|
|
||||||
// if(i % 16 == 15)
|
|
||||||
// std::cout << std::endl;
|
|
||||||
usbReadFifo.pop_front();
|
usbReadFifo.pop_front();
|
||||||
}
|
}
|
||||||
//std::cout << std::dec << std::endl;
|
|
||||||
|
|
||||||
if(decoder.input(payloadBytes)) {
|
if(packetizer.input(payloadBytes)) {
|
||||||
for(auto& msg : decoder.output()) {
|
for(auto& packet : packetizer.output()) {
|
||||||
for(auto& cb : messageCallbacks) {
|
auto msg = decoder.decodePacket(packet);
|
||||||
if(!closing) { // We might have closed while reading or processing
|
for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
|
||||||
|
if(!closing) {
|
||||||
cb.second.callIfMatch(msg);
|
cb.second.callIfMatch(msg);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -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;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue