mirror of
https://github.com/intrepidcs/libicsneo.git
synced 2026-08-05 01:18:36 +02:00
Initial commit
This commit is contained in:
Paul Hollinsky
@@ -0,0 +1,105 @@
|
||||
#include "communication/include/communication.h"
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <queue>
|
||||
#include <iomanip>
|
||||
#include <cstring>
|
||||
#include "communication/include/messagedecoder.h"
|
||||
|
||||
using namespace icsneo;
|
||||
|
||||
int Communication::messageCallbackIDCounter = 1;
|
||||
|
||||
uint8_t Communication::ICSChecksum(const std::vector<uint8_t>& data) {
|
||||
uint32_t checksum = 0;
|
||||
for(auto i = 0; i < data.size(); i++)
|
||||
checksum += data[i];
|
||||
checksum = ~checksum;
|
||||
checksum++;
|
||||
return (uint8_t)checksum;
|
||||
}
|
||||
|
||||
std::vector<uint8_t>& Communication::packetWrap(std::vector<uint8_t>& data, bool addChecksum) {
|
||||
if(addChecksum)
|
||||
data.push_back(ICSChecksum(data));
|
||||
data.insert(data.begin(), 0xAA);
|
||||
if(align16bit && data.size() % 2 == 1)
|
||||
data.push_back('A');
|
||||
return data;
|
||||
}
|
||||
|
||||
bool Communication::open() {
|
||||
if(isOpen)
|
||||
return true;
|
||||
|
||||
spawnThreads();
|
||||
isOpen = true;
|
||||
return impl->open();
|
||||
}
|
||||
|
||||
void Communication::spawnThreads() {
|
||||
readTaskThread = std::thread(&Communication::readTask, this);
|
||||
}
|
||||
|
||||
void Communication::joinThreads() {
|
||||
if(readTaskThread.joinable())
|
||||
readTaskThread.join();
|
||||
}
|
||||
|
||||
bool Communication::close() {
|
||||
if(!isOpen)
|
||||
return false;
|
||||
|
||||
isOpen = false;
|
||||
closing = true;
|
||||
joinThreads();
|
||||
|
||||
return impl->close();
|
||||
}
|
||||
|
||||
bool Communication::sendPacket(std::vector<uint8_t>& bytes) {
|
||||
return impl->write(Communication::packetWrap(bytes));
|
||||
}
|
||||
|
||||
bool Communication::sendCommand(Communication::Command cmd, std::vector<uint8_t> arguments) {
|
||||
std::vector<uint8_t> bytes;
|
||||
bytes.push_back((uint8_t)cmd);
|
||||
for(auto& b : arguments)
|
||||
bytes.push_back(b);
|
||||
bytes.insert(bytes.begin(), 0xB | ((uint8_t)bytes.size() << 4));
|
||||
return sendPacket(bytes);
|
||||
}
|
||||
|
||||
int Communication::addMessageCallback(const MessageCallback& cb) {
|
||||
messageCallbacks.insert(std::make_pair(messageCallbackIDCounter, cb));
|
||||
return messageCallbackIDCounter++;
|
||||
}
|
||||
|
||||
bool Communication::removeMessageCallback(int id) {
|
||||
try {
|
||||
messageCallbacks.erase(id);
|
||||
return true;
|
||||
} catch(...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void Communication::readTask() {
|
||||
std::vector<uint8_t> readBytes;
|
||||
MessageDecoder decoder;
|
||||
|
||||
while(!closing) {
|
||||
readBytes.clear();
|
||||
if(impl->readWait(readBytes)) {
|
||||
if(decoder.input(readBytes)) {
|
||||
for(auto& msg : decoder.output()) {
|
||||
for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
|
||||
if(!closing) {
|
||||
cb.second.callIfMatch(msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
#include "communication/include/icommunication.h"
|
||||
|
||||
using namespace icsneo;
|
||||
|
||||
bool ICommunication::read(std::vector<uint8_t>& bytes, size_t limit) {
|
||||
// A limit of zero indicates no limit
|
||||
if(limit == 0)
|
||||
limit = (size_t)-1;
|
||||
|
||||
if(limit > (readQueue.size_approx() + 4))
|
||||
limit = (readQueue.size_approx() + 4);
|
||||
|
||||
if(bytes.capacity() < limit)
|
||||
bytes.resize(limit);
|
||||
|
||||
size_t actuallyRead = readQueue.try_dequeue_bulk(bytes.data(), limit);
|
||||
|
||||
bytes.resize(actuallyRead);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ICommunication::readWait(std::vector<uint8_t>& bytes, std::chrono::milliseconds timeout, size_t limit) {
|
||||
// A limit of zero indicates no limit
|
||||
if(limit == 0)
|
||||
limit = (size_t)-1;
|
||||
|
||||
if(limit > (readQueue.size_approx() + 4))
|
||||
limit = (readQueue.size_approx() + 4);
|
||||
|
||||
bytes.resize(limit);
|
||||
|
||||
size_t actuallyRead = readQueue.wait_dequeue_bulk_timed(bytes.data(), limit, timeout);
|
||||
|
||||
bytes.resize(actuallyRead);
|
||||
|
||||
return actuallyRead > 0;
|
||||
}
|
||||
|
||||
bool ICommunication::write(const std::vector<uint8_t>& bytes) {
|
||||
return writeQueue.enqueue(WriteOperation(bytes));
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
#ifndef __COMMUNICATION_H_
|
||||
#define __COMMUNICATION_H_
|
||||
|
||||
#include "communication/include/icommunication.h"
|
||||
#include "communication/include/network.h"
|
||||
#include "communication/include/messagecallback.h"
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
#include <thread>
|
||||
#include <queue>
|
||||
#include <map>
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class Communication {
|
||||
public:
|
||||
static uint8_t ICSChecksum(const std::vector<uint8_t>& data);
|
||||
|
||||
Communication(std::shared_ptr<ICommunication> com) : impl(com) {}
|
||||
virtual ~Communication() { close(); }
|
||||
|
||||
bool open();
|
||||
bool close();
|
||||
virtual void spawnThreads();
|
||||
virtual void joinThreads();
|
||||
bool rawWrite(const std::vector<uint8_t>& bytes) { return impl->write(bytes); }
|
||||
std::vector<uint8_t>& packetWrap(std::vector<uint8_t>& data, bool addChecksum = true);
|
||||
bool sendPacket(std::vector<uint8_t>& bytes);
|
||||
|
||||
enum class Command : uint8_t {
|
||||
EnableNetworkCommunication = 0x07,
|
||||
RequestSerialNumber = 0xA1
|
||||
};
|
||||
virtual bool sendCommand(Command cmd, bool boolean) { return sendCommand(cmd, std::vector<uint8_t>({ (uint8_t)boolean })); }
|
||||
virtual bool sendCommand(Command cmd, std::vector<uint8_t> arguments = {});
|
||||
|
||||
int addMessageCallback(const MessageCallback& cb);
|
||||
bool removeMessageCallback(int id);
|
||||
|
||||
void setAlign16Bit(bool enable) { align16bit = enable; }
|
||||
|
||||
protected:
|
||||
std::shared_ptr<ICommunication> impl;
|
||||
static int messageCallbackIDCounter;
|
||||
std::map<int, MessageCallback> messageCallbacks;
|
||||
std::atomic<bool> closing{false};
|
||||
|
||||
private:
|
||||
bool isOpen = false;
|
||||
bool align16bit = true; // Not needed for Gigalog, Galaxy, etc and newer
|
||||
|
||||
std::thread readTaskThread;
|
||||
void readTask();
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,43 @@
|
||||
#ifndef __ICOMMUNICATION_H_
|
||||
#define __ICOMMUNICATION_H_
|
||||
|
||||
#include <vector>
|
||||
#include <chrono>
|
||||
#include <atomic>
|
||||
#include <thread>
|
||||
#include "third-party/concurrentqueue/blockingconcurrentqueue.h"
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class ICommunication {
|
||||
public:
|
||||
virtual ~ICommunication() {}
|
||||
virtual bool open() = 0;
|
||||
virtual bool isOpen() = 0;
|
||||
virtual bool close() = 0;
|
||||
virtual bool read(std::vector<uint8_t>& bytes, size_t limit = 0);
|
||||
virtual bool readWait(std::vector<uint8_t>& bytes, std::chrono::milliseconds timeout = std::chrono::milliseconds(100), size_t limit = 0);
|
||||
virtual bool write(const std::vector<uint8_t>& bytes);
|
||||
|
||||
protected:
|
||||
class WriteOperation {
|
||||
public:
|
||||
WriteOperation() {}
|
||||
WriteOperation(std::vector<uint8_t> b) { bytes = b; }
|
||||
std::vector<uint8_t> bytes;
|
||||
};
|
||||
enum IOTaskState {
|
||||
LAUNCH,
|
||||
WAIT
|
||||
};
|
||||
virtual void readTask() = 0;
|
||||
virtual void writeTask() = 0;
|
||||
moodycamel::BlockingConcurrentQueue<uint8_t> readQueue;
|
||||
moodycamel::BlockingConcurrentQueue<WriteOperation> writeQueue;
|
||||
std::thread readThread, writeThread;
|
||||
std::atomic<bool> closing{false};
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,46 @@
|
||||
#ifndef __MESSAGECALLBACK_H_
|
||||
#define __MESSAGECALLBACK_H_
|
||||
|
||||
#include "communication/message/include/message.h"
|
||||
#include "communication/include/messagefilter.h"
|
||||
#include <memory>
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class MessageCallback {
|
||||
public:
|
||||
typedef std::function< void( std::shared_ptr<Message> ) > fn_messageCallback;
|
||||
|
||||
MessageCallback(fn_messageCallback cb, std::shared_ptr<MessageFilter> f) : callback(cb), filter(f) {}
|
||||
MessageCallback(fn_messageCallback cb, MessageFilter f = MessageFilter()) : callback(cb), filter(std::make_shared<MessageFilter>(f)) {}
|
||||
|
||||
// Allow the filter to be placed first if the user wants (maybe in the case of a lambda)
|
||||
MessageCallback(MessageFilter f, fn_messageCallback cb) { MessageCallback(cb, f); }
|
||||
|
||||
virtual bool callIfMatch(const std::shared_ptr<Message>& message) const {
|
||||
bool ret = filter->match(message);
|
||||
if(ret)
|
||||
callback(message);
|
||||
return ret;
|
||||
}
|
||||
const MessageFilter& getFilter() const { return *filter; }
|
||||
const fn_messageCallback& getCallback() const { return callback; }
|
||||
|
||||
protected:
|
||||
fn_messageCallback callback;
|
||||
std::shared_ptr<MessageFilter> filter;
|
||||
};
|
||||
|
||||
class CANMessageCallback : public MessageCallback {
|
||||
public:
|
||||
CANMessageCallback(fn_messageCallback cb, CANMessageFilter f = CANMessageFilter()) : MessageCallback(cb, std::make_shared<CANMessageFilter>(f)) {}
|
||||
|
||||
// Allow the filter to be placed first if the user wants (maybe in the case of a lambda)
|
||||
CANMessageCallback(CANMessageFilter f, fn_messageCallback cb) : MessageCallback(cb, std::make_shared<CANMessageFilter>(f)) {}
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,258 @@
|
||||
#ifndef __MESSAGEDECODER_H_
|
||||
#define __MESSAGEDECODER_H_
|
||||
|
||||
#include "communication/message/include/message.h"
|
||||
#include "communication/message/include/canmessage.h"
|
||||
#include "communication/include/network.h"
|
||||
#include <queue>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class MessageDecoder {
|
||||
public:
|
||||
bool input(const std::vector<uint8_t>& bytes);
|
||||
std::vector<std::shared_ptr<Message>> output();
|
||||
|
||||
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];
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,72 @@
|
||||
#ifndef __MESSAGEFILTER_H_
|
||||
#define __MESSAGEFILTER_H_
|
||||
|
||||
#include "communication/include/network.h"
|
||||
#include "communication/message/include/message.h"
|
||||
#include "communication/message/include/canmessage.h"
|
||||
#include <memory>
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class MessageFilter {
|
||||
public:
|
||||
MessageFilter() : matchAny(true) {}
|
||||
MessageFilter(Network::Type type) : type(type) {}
|
||||
MessageFilter(Network::NetID netid) : netid(netid) {}
|
||||
virtual ~MessageFilter() {}
|
||||
|
||||
virtual bool match(const std::shared_ptr<Message>& message) const {
|
||||
if(matchAny)
|
||||
return true;
|
||||
if(!matchType(message->network.getType()))
|
||||
return false;
|
||||
if(!matchNetID(message->network.getNetID()))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
bool matchAny = false;
|
||||
|
||||
Network::Type type = Network::Type::Invalid; // Matching a type of invalid will match any
|
||||
bool matchType(Network::Type mtype) const {
|
||||
if(type == Network::Type::Invalid)
|
||||
return true;
|
||||
return type == mtype;
|
||||
}
|
||||
|
||||
Network::NetID netid = Network::NetID::Invalid; // Matching a netid of invalid will match any
|
||||
bool matchNetID(Network::NetID mnetid) const {
|
||||
if(netid == Network::NetID::Invalid)
|
||||
return true;
|
||||
return netid == mnetid;
|
||||
}
|
||||
};
|
||||
|
||||
class CANMessageFilter : public MessageFilter {
|
||||
public:
|
||||
CANMessageFilter() : MessageFilter(Network::Type::CAN), arbid(INVALID_ARBID) {}
|
||||
CANMessageFilter(uint32_t arbid) : MessageFilter(Network::Type::CAN), arbid(arbid) {}
|
||||
|
||||
bool match(const std::shared_ptr<Message>& message) const {
|
||||
if(!MessageFilter::match(message))
|
||||
return false;
|
||||
const auto canMessage = std::dynamic_pointer_cast<CANMessage>(message);
|
||||
if(canMessage == nullptr || !matchArbID(canMessage->arbid))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
static constexpr uint32_t INVALID_ARBID = 0xffffffff;
|
||||
uint32_t arbid;
|
||||
bool matchArbID(uint32_t marbid) const {
|
||||
if(arbid == INVALID_ARBID)
|
||||
return true;
|
||||
return arbid == marbid;
|
||||
}
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,102 @@
|
||||
#ifndef __MULTICHANNELCOMMUNICATION_H_
|
||||
#define __MULTICHANNELCOMMUNICATION_H_
|
||||
|
||||
#include "communication/include/communication.h"
|
||||
#include "communication/include/icommunication.h"
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class MultiChannelCommunication : public Communication {
|
||||
public:
|
||||
MultiChannelCommunication(std::shared_ptr<ICommunication> com) : Communication(com) {}
|
||||
void spawnThreads();
|
||||
void joinThreads();
|
||||
bool sendCommand(Communication::Command cmd, std::vector<uint8_t> arguments);
|
||||
|
||||
protected:
|
||||
bool preprocessPacket(std::deque<uint8_t>& usbReadFifo);
|
||||
|
||||
private:
|
||||
enum class CommandType : uint8_t {
|
||||
PlasmaReadRequest = 0x10, // Status read request to HSC
|
||||
PlasmaStatusResponse = 0x11, // Status response by HSC
|
||||
HostPC_to_Vnet1 = 0x20, // Host PC data to Vnet module-1
|
||||
Vnet1_to_HostPC = 0x21, // Vnet module-1 data to host PC
|
||||
HostPC_to_Vnet2 = 0x30, // Host PC data to Vnet module-2
|
||||
Vnet2_to_HostPC = 0x31, // Vnet module-2 data to host PC
|
||||
HostPC_to_Vnet3 = 0x40, // Host PC data to Vnet module-3
|
||||
Vnet3_to_HostPC = 0x41, // Vnet module-3 data to host PC
|
||||
HostPC_to_SDCC1 = 0x50, // Host PC data to write to SDCC-1
|
||||
HostPC_from_SDCC1 = 0x51, // Host PC wants data read from SDCC-1
|
||||
SDCC1_to_HostPC = 0x52, // SDCC-1 data to host PC
|
||||
HostPC_to_SDCC2 = 0x60, // Host PC data to write to SDCC-2
|
||||
HostPC_from_SDCC2 = 0x61, // Host PC wants data read from SDCC-2
|
||||
SDCC2_to_HostPC = 0x62, // SDCC-2 data to host PC
|
||||
PC_to_LSOC = 0x70, // Host PC data to LSOCC
|
||||
LSOCC_to_PC = 0x71, // LSOCC data to host PC
|
||||
HostPC_to_Microblaze = 0x80, // Host PC data to microblaze processor
|
||||
Microblaze_to_HostPC = 0x81 // Microblaze processor data to host PC
|
||||
};
|
||||
static bool CommandTypeIsValid(CommandType cmd) {
|
||||
switch(cmd) {
|
||||
case CommandType::PlasmaReadRequest:
|
||||
case CommandType::PlasmaStatusResponse:
|
||||
case CommandType::HostPC_to_Vnet1:
|
||||
case CommandType::Vnet1_to_HostPC:
|
||||
case CommandType::HostPC_to_Vnet2:
|
||||
case CommandType::Vnet2_to_HostPC:
|
||||
case CommandType::HostPC_to_Vnet3:
|
||||
case CommandType::Vnet3_to_HostPC:
|
||||
case CommandType::HostPC_to_SDCC1:
|
||||
case CommandType::HostPC_from_SDCC1:
|
||||
case CommandType::SDCC1_to_HostPC:
|
||||
case CommandType::HostPC_to_SDCC2:
|
||||
case CommandType::HostPC_from_SDCC2:
|
||||
case CommandType::SDCC2_to_HostPC:
|
||||
case CommandType::PC_to_LSOC:
|
||||
case CommandType::LSOCC_to_PC:
|
||||
case CommandType::HostPC_to_Microblaze:
|
||||
case CommandType::Microblaze_to_HostPC:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
static bool CommandTypeHasAddress(CommandType cmd) {
|
||||
// Check CommandTypeIsValid before this, you will get false on an invalid command
|
||||
switch(cmd) {
|
||||
case CommandType::SDCC1_to_HostPC:
|
||||
case CommandType::SDCC2_to_HostPC:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
static uint16_t CommandTypeDefinesLength(CommandType cmd) {
|
||||
// Check CommandTypeIsValid before this, you will get 0 on an invalid command
|
||||
switch(cmd) {
|
||||
case CommandType::PlasmaStatusResponse:
|
||||
return 2;
|
||||
default:
|
||||
return 0; // Length is defined by following bytes in message
|
||||
}
|
||||
}
|
||||
|
||||
enum class PreprocessState {
|
||||
SearchForCommand,
|
||||
ParseAddress,
|
||||
ParseLength,
|
||||
GetData
|
||||
};
|
||||
PreprocessState state = PreprocessState::SearchForCommand;
|
||||
uint16_t currentCommandLength;
|
||||
CommandType currentCommandType;
|
||||
size_t currentReadIndex = 0;
|
||||
|
||||
std::thread mainChannelReadThread;
|
||||
void readTask();
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,340 @@
|
||||
#ifndef __NETWORKID_H_
|
||||
#define __NETWORKID_H_
|
||||
|
||||
#include <cstdint>
|
||||
#include <ostream>
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class Network {
|
||||
public:
|
||||
enum class NetID : uint16_t {
|
||||
Device = 0,
|
||||
HSCAN = 1,
|
||||
MSCAN = 2,
|
||||
SWCAN = 3,
|
||||
LSFTCAN = 4,
|
||||
FordSCP = 5,
|
||||
J1708 = 6,
|
||||
Aux = 7,
|
||||
J1850VPW = 8,
|
||||
ISO = 9,
|
||||
ISOPIC = 10,
|
||||
Main51 = 11,
|
||||
RED = 12,
|
||||
SCI = 13,
|
||||
ISO2 = 14,
|
||||
ISO14230 = 15,
|
||||
LIN = 16,
|
||||
OP_Ethernet1 = 17,
|
||||
OP_Ethernet2 = 18,
|
||||
OP_Ethernet3 = 19,
|
||||
ISO3 = 41,
|
||||
HSCAN2 = 42,
|
||||
HSCAN3 = 44,
|
||||
OP_Ethernet4 = 45,
|
||||
OP_Ethernet5 = 46,
|
||||
ISO4 = 47,
|
||||
LIN2 = 48,
|
||||
LIN3 = 49,
|
||||
LIN4 = 50,
|
||||
MOST = 51,
|
||||
Red_App_Error = 52,
|
||||
CGI = 53,
|
||||
Reset_Status = 54,
|
||||
FB_Status = 55,
|
||||
App_Signal_Status = 56,
|
||||
Read_Datalink_Cm_Tx_Msg = 57,
|
||||
Read_Datalink_Cm_Rx_Msg = 58,
|
||||
Logging_Overflow = 59,
|
||||
Read_Settings_Ex = 60,
|
||||
HSCAN4 = 61,
|
||||
HSCAN5 = 62,
|
||||
RS232 = 63,
|
||||
UART = 64,
|
||||
UART2 = 65,
|
||||
UART3 = 66,
|
||||
UART4 = 67,
|
||||
SWCAN2 = 68,
|
||||
Ethernet_DAQ = 69,
|
||||
Data_To_Host = 70,
|
||||
TextAPI_To_Host = 71,
|
||||
OP_Ethernet6 = 73,
|
||||
Red_VBat = 74,
|
||||
OP_Ethernet7 = 75,
|
||||
OP_Ethernet8 = 76,
|
||||
OP_Ethernet9 = 77,
|
||||
OP_Ethernet10 = 78,
|
||||
OP_Ethernet11 = 79,
|
||||
FlexRay1a = 80,
|
||||
FlexRay1b = 81,
|
||||
FlexRay2a = 82,
|
||||
FlexRay2b = 83,
|
||||
LIN5 = 84,
|
||||
FlexRay = 85,
|
||||
FlexRay2 = 86,
|
||||
OP_Ethernet12 = 87,
|
||||
MOST25 = 90,
|
||||
MOST50 = 91,
|
||||
MOST150 = 92,
|
||||
Ethernet = 93,
|
||||
GMFSA = 94,
|
||||
TCP = 95,
|
||||
HSCAN6 = 96,
|
||||
HSCAN7 = 97,
|
||||
LIN6 = 98,
|
||||
LSFTCAN2 = 99,
|
||||
HW_COM_Latency_Test = 512,
|
||||
Device_Status = 513,
|
||||
Invalid = 0xffff
|
||||
};
|
||||
enum class Type {
|
||||
CAN,
|
||||
LIN,
|
||||
FlexRay,
|
||||
MOST,
|
||||
Ethernet,
|
||||
Other,
|
||||
Invalid
|
||||
};
|
||||
static constexpr const char* GetTypeString(Type type) {
|
||||
switch(type) {
|
||||
case Type::CAN:
|
||||
return "CAN";
|
||||
case Type::LIN:
|
||||
return "LIN";
|
||||
case Type::FlexRay:
|
||||
return "FlexRay";
|
||||
case Type::MOST:
|
||||
return "MOST";
|
||||
case Type::Other:
|
||||
return "Other";
|
||||
case Type::Invalid:
|
||||
default:
|
||||
return "Invalid Type";
|
||||
}
|
||||
}
|
||||
static constexpr Type GetTypeOfNetID(NetID netid) {
|
||||
switch(netid) {
|
||||
case NetID::HSCAN:
|
||||
case NetID::MSCAN:
|
||||
case NetID::SWCAN:
|
||||
case NetID::LSFTCAN:
|
||||
case NetID::HSCAN2:
|
||||
case NetID::HSCAN3:
|
||||
case NetID::HSCAN4:
|
||||
case NetID::HSCAN5:
|
||||
case NetID::SWCAN2:
|
||||
case NetID::HSCAN6:
|
||||
case NetID::HSCAN7:
|
||||
case NetID::LSFTCAN2:
|
||||
return Type::CAN;
|
||||
case NetID::LIN:
|
||||
case NetID::LIN2:
|
||||
case NetID::LIN3:
|
||||
case NetID::LIN4:
|
||||
case NetID::LIN5:
|
||||
case NetID::LIN6:
|
||||
return Type::LIN;
|
||||
case NetID::FlexRay:
|
||||
case NetID::FlexRay1a:
|
||||
case NetID::FlexRay1b:
|
||||
case NetID::FlexRay2:
|
||||
case NetID::FlexRay2a:
|
||||
case NetID::FlexRay2b:
|
||||
return Type::FlexRay;
|
||||
case NetID::MOST:
|
||||
case NetID::MOST25:
|
||||
case NetID::MOST50:
|
||||
case NetID::MOST150:
|
||||
return Type::MOST;
|
||||
case NetID::Invalid:
|
||||
return Type::Invalid;
|
||||
default:
|
||||
return Type::Other;
|
||||
}
|
||||
}
|
||||
static constexpr const char* GetNetIDString(NetID netid) {
|
||||
switch(netid) {
|
||||
case NetID::Device:
|
||||
return "Device";
|
||||
case NetID::HSCAN:
|
||||
return "HSCAN";
|
||||
case NetID::MSCAN:
|
||||
return "MSCAN";
|
||||
case NetID::SWCAN:
|
||||
return "SWCAN";
|
||||
case NetID::LSFTCAN:
|
||||
return "LSFTCAN";
|
||||
case NetID::FordSCP:
|
||||
return "FordSCP";
|
||||
case NetID::J1708:
|
||||
return "J1708";
|
||||
case NetID::Aux:
|
||||
return "Aux";
|
||||
case NetID::J1850VPW:
|
||||
return "J1850 VPW";
|
||||
case NetID::ISO:
|
||||
return "ISO";
|
||||
case NetID::ISOPIC:
|
||||
return "ISOPIC";
|
||||
case NetID::Main51:
|
||||
return "Main51";
|
||||
case NetID::RED:
|
||||
return "RED";
|
||||
case NetID::SCI:
|
||||
return "SCI";
|
||||
case NetID::ISO2:
|
||||
return "ISO 2";
|
||||
case NetID::ISO14230:
|
||||
return "ISO 14230";
|
||||
case NetID::LIN:
|
||||
return "LIN";
|
||||
case NetID::OP_Ethernet1:
|
||||
return "Ethernet 1";
|
||||
case NetID::OP_Ethernet2:
|
||||
return "Ethernet 2";
|
||||
case NetID::OP_Ethernet3:
|
||||
return "Ethernet 3";
|
||||
case NetID::ISO3:
|
||||
return "ISO 3";
|
||||
case NetID::HSCAN2:
|
||||
return "HSCAN 2";
|
||||
case NetID::HSCAN3:
|
||||
return "HSCAN 3";
|
||||
case NetID::OP_Ethernet4:
|
||||
return "Ethernet 4";
|
||||
case NetID::OP_Ethernet5:
|
||||
return "Ethernet 5";
|
||||
case NetID::ISO4:
|
||||
return "ISO 4";
|
||||
case NetID::LIN2:
|
||||
return "LIN 2";
|
||||
case NetID::LIN3:
|
||||
return "LIN 3";
|
||||
case NetID::LIN4:
|
||||
return "LIN 4";
|
||||
case NetID::MOST:
|
||||
return "MOST";
|
||||
case NetID::Red_App_Error:
|
||||
return "Red App Error";
|
||||
case NetID::CGI:
|
||||
return "CGI";
|
||||
case NetID::Reset_Status:
|
||||
return "Reset Status";
|
||||
case NetID::FB_Status:
|
||||
return "FB Status";
|
||||
case NetID::App_Signal_Status:
|
||||
return "App Signal Status";
|
||||
case NetID::Read_Datalink_Cm_Tx_Msg:
|
||||
return "Read Datalink Cm Tx Msg";
|
||||
case NetID::Read_Datalink_Cm_Rx_Msg:
|
||||
return "Read Datalink Cm Rx Msg";
|
||||
case NetID::Logging_Overflow:
|
||||
return "Logging Overflow";
|
||||
case NetID::Read_Settings_Ex:
|
||||
return "Read Settings Ex";
|
||||
case NetID::HSCAN4:
|
||||
return "HSCAN 4";
|
||||
case NetID::HSCAN5:
|
||||
return "HSCAN 5";
|
||||
case NetID::RS232:
|
||||
return "RS232";
|
||||
case NetID::UART:
|
||||
return "UART";
|
||||
case NetID::UART2:
|
||||
return "UART 2";
|
||||
case NetID::UART3:
|
||||
return "UART 3";
|
||||
case NetID::UART4:
|
||||
return "UART 4";
|
||||
case NetID::SWCAN2:
|
||||
return "SWCAN 2";
|
||||
case NetID::Ethernet_DAQ:
|
||||
return "Ethernet DAQ";
|
||||
case NetID::Data_To_Host:
|
||||
return "Data To Host";
|
||||
case NetID::TextAPI_To_Host:
|
||||
return "TextAPI To Host";
|
||||
case NetID::OP_Ethernet6:
|
||||
return "Ethernet 6";
|
||||
case NetID::Red_VBat:
|
||||
return "Red VBat";
|
||||
case NetID::OP_Ethernet7:
|
||||
return "Ethernet 7";
|
||||
case NetID::OP_Ethernet8:
|
||||
return "Ethernet 8";
|
||||
case NetID::OP_Ethernet9:
|
||||
return "Ethernet 9";
|
||||
case NetID::OP_Ethernet10:
|
||||
return "Ethernet 10";
|
||||
case NetID::OP_Ethernet11:
|
||||
return "Ethernet 11";
|
||||
case NetID::FlexRay1a:
|
||||
return "FlexRay 1a";
|
||||
case NetID::FlexRay1b:
|
||||
return "FlexRay 1b";
|
||||
case NetID::FlexRay2a:
|
||||
return "FlexRay 2a";
|
||||
case NetID::FlexRay2b:
|
||||
return "FlexRay 2b";
|
||||
case NetID::LIN5:
|
||||
return "LIN 5";
|
||||
case NetID::FlexRay:
|
||||
return "FlexRay";
|
||||
case NetID::FlexRay2:
|
||||
return "FlexRay 2";
|
||||
case NetID::OP_Ethernet12:
|
||||
return "Ethernet 12";
|
||||
case NetID::MOST25:
|
||||
return "MOST25";
|
||||
case NetID::MOST50:
|
||||
return "MOST50";
|
||||
case NetID::MOST150:
|
||||
return "MOST150";
|
||||
case NetID::Ethernet:
|
||||
return "Ethernet";
|
||||
case NetID::GMFSA:
|
||||
return "GMFSA";
|
||||
case NetID::TCP:
|
||||
return "TCP";
|
||||
case NetID::HSCAN6:
|
||||
return "HSCAN 6";
|
||||
case NetID::HSCAN7:
|
||||
return "HSCAN 7";
|
||||
case NetID::LIN6:
|
||||
return "LIN 6";
|
||||
case NetID::LSFTCAN2:
|
||||
return "LSFTCAN 2";
|
||||
case NetID::HW_COM_Latency_Test:
|
||||
return "HW COM Latency Test";
|
||||
case NetID::Device_Status:
|
||||
return "Device Status";
|
||||
case NetID::Invalid:
|
||||
default:
|
||||
return "Invalid Network";
|
||||
}
|
||||
}
|
||||
|
||||
Network() { setValue(NetID::Invalid); }
|
||||
Network(uint16_t netid) { setValue((NetID)netid); }
|
||||
Network(NetID netid) { setValue(netid); }
|
||||
NetID getNetID() const { return value; }
|
||||
Type getType() const { return type; }
|
||||
friend std::ostream& operator<<(std::ostream& os, const Network& network) {
|
||||
os << GetNetIDString(network.getNetID());
|
||||
return os;
|
||||
}
|
||||
|
||||
private:
|
||||
NetID value; // Always use setValue so that value and type stay in sync
|
||||
Type type;
|
||||
void setValue(NetID id) {
|
||||
value = id;
|
||||
type = GetTypeOfNetID(value);
|
||||
}
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,15 @@
|
||||
#ifndef __CANMESSAGE_H_
|
||||
#define __CANMESSAGE_H_
|
||||
|
||||
#include "communication/message/include/message.h"
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class CANMessage : public Message {
|
||||
public:
|
||||
uint32_t arbid;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,18 @@
|
||||
#ifndef __MESSAGE_H_
|
||||
#define __MESSAGE_H_
|
||||
|
||||
#include "communication/include/network.h"
|
||||
#include <vector>
|
||||
|
||||
namespace icsneo {
|
||||
|
||||
class Message {
|
||||
public:
|
||||
virtual ~Message() {}
|
||||
Network network;
|
||||
std::vector<uint8_t> data;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,139 @@
|
||||
#include "communication/include/messagedecoder.h"
|
||||
#include "communication/include/communication.h"
|
||||
#include <iostream>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,128 @@
|
||||
#include "communication/include/multichannelcommunication.h"
|
||||
#include "communication/include/messagedecoder.h"
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
|
||||
using namespace icsneo;
|
||||
|
||||
void MultiChannelCommunication::spawnThreads() {
|
||||
mainChannelReadThread = std::thread(&MultiChannelCommunication::readTask, this);
|
||||
}
|
||||
|
||||
void MultiChannelCommunication::joinThreads() {
|
||||
if(mainChannelReadThread.joinable())
|
||||
mainChannelReadThread.join();
|
||||
}
|
||||
|
||||
bool MultiChannelCommunication::sendCommand(Communication::Command cmd, std::vector<uint8_t> arguments) {
|
||||
std::vector<uint8_t> bytes;
|
||||
bytes.push_back((uint8_t)cmd);
|
||||
for(auto& b : arguments)
|
||||
bytes.push_back(b);
|
||||
bytes.insert(bytes.begin(), 0xB | ((uint8_t)bytes.size() << 4));
|
||||
bytes = Communication::packetWrap(bytes);
|
||||
bytes.insert(bytes.begin(), {(uint8_t)CommandType::HostPC_to_Vnet1, (uint8_t)bytes.size(), (uint8_t)(bytes.size() >> 8)});
|
||||
return rawWrite(bytes);
|
||||
}
|
||||
|
||||
void MultiChannelCommunication::readTask() {
|
||||
bool readMore = true;
|
||||
std::deque<uint8_t> usbReadFifo;
|
||||
std::vector<uint8_t> readBytes;
|
||||
std::vector<uint8_t> payloadBytes;
|
||||
MessageDecoder decoder;
|
||||
|
||||
while(!closing) {
|
||||
if(readMore) {
|
||||
readBytes.clear();
|
||||
if(impl->readWait(readBytes)) {
|
||||
readMore = false;
|
||||
usbReadFifo.insert(usbReadFifo.end(), std::make_move_iterator(readBytes.begin()), std::make_move_iterator(readBytes.end()));
|
||||
}
|
||||
} else {
|
||||
switch(state) {
|
||||
case PreprocessState::SearchForCommand:
|
||||
if(usbReadFifo.size() < 1) {
|
||||
readMore = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
currentCommandType = (CommandType)usbReadFifo[0];
|
||||
|
||||
if(!CommandTypeIsValid(currentCommandType)) {
|
||||
std::cout << "cnv" << std::hex << (int)currentCommandType << ' ' << std::dec;
|
||||
usbReadFifo.pop_front();
|
||||
continue;
|
||||
}
|
||||
|
||||
currentReadIndex = 1;
|
||||
|
||||
if(CommandTypeHasAddress(currentCommandType)) {
|
||||
state = PreprocessState::ParseAddress;
|
||||
continue; // No commands which define an address also define a length, so we can just continue from there
|
||||
}
|
||||
|
||||
currentCommandLength = CommandTypeDefinesLength(currentCommandType);
|
||||
if(currentCommandLength == 0) {
|
||||
state = PreprocessState::ParseLength;
|
||||
continue;
|
||||
}
|
||||
|
||||
state = PreprocessState::GetData;
|
||||
continue;
|
||||
case PreprocessState::ParseAddress:
|
||||
// The address is represented by a 4 byte little endian
|
||||
// Don't care about it yet
|
||||
currentReadIndex += 4;
|
||||
// Intentionally fall through
|
||||
case PreprocessState::ParseLength:
|
||||
state = PreprocessState::ParseLength; // Set state in case we've fallen through, but later need to go around again
|
||||
|
||||
if(usbReadFifo.size() < currentReadIndex + 2) { // Come back we have more data
|
||||
readMore = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
// The length is represented by a 2 byte little endian
|
||||
currentCommandLength = usbReadFifo[currentReadIndex++];
|
||||
currentCommandLength |= usbReadFifo[currentReadIndex++] << 8;
|
||||
// Intentionally fall through
|
||||
case PreprocessState::GetData:
|
||||
state = PreprocessState::GetData; // Set state in case we've fallen through, but later need to go around again
|
||||
|
||||
if(usbReadFifo.size() <= currentReadIndex + currentCommandLength) { // Come back we have more data
|
||||
readMore = true;
|
||||
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);
|
||||
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;
|
||||
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
|
||||
cb.second.callIfMatch(msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
state = PreprocessState::SearchForCommand;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user