376 lines
7.7 KiB
C++
376 lines
7.7 KiB
C++
|
|
|
|
#include "NetworkProtocol.h"
|
|
|
|
|
|
CNetworkProtocol::CNetworkProtocol()
|
|
{
|
|
mIsResetManual = false;
|
|
ResetRxStateMachine();
|
|
|
|
}
|
|
|
|
CNetworkProtocol::~CNetworkProtocol()
|
|
{
|
|
|
|
}
|
|
|
|
//void CNetworkProtocol::BindUpperLayer(CMasterCtrl *UpperLayerPtr)
|
|
//{
|
|
// mMasterCtrlHandle = UpperLayerPtr;
|
|
//}
|
|
|
|
|
|
unsigned char CNetworkProtocol::CalcCRC(char *Buffer, int Size)
|
|
{
|
|
unsigned char CRC = 0x00;
|
|
|
|
for( int i = 0; i < Size; i++)
|
|
{
|
|
CRC ^= Buffer[i];
|
|
}
|
|
return CRC;
|
|
}
|
|
|
|
QByteArray CNetworkProtocol::GetTxPacket(unsigned char MessageID, unsigned char Flags, const char *Data, int Size, unsigned char Address, unsigned char ID, unsigned char SenderDevice, unsigned char SenderAddress)
|
|
{
|
|
#ifdef USE_BYTEARRAY_IN_SEND
|
|
|
|
QByteArray Frame;
|
|
Frame.clear();
|
|
int FrameSize = Size + 11; //Add header data...
|
|
|
|
Frame.append(FRAME_HEADER); //header
|
|
Frame.append(SenderAddress); //Device address (default = MasterCtrl)
|
|
Frame.append(SenderDevice); //Device ID (default = MasterCtrl
|
|
Frame.append(ID); //destination ID
|
|
Frame.append(Address); //Destination Address
|
|
Frame.append(Flags);
|
|
Frame.append(MessageID); //Cmd
|
|
Frame.append((Size & 0xFF000000) >> 24);
|
|
Frame.append((Size & 0x00FF0000) >> 16);
|
|
Frame.append((Size & 0x0000FF00) >> 8);
|
|
Frame.append(Size & 0x000000FF);
|
|
Frame.append(Data,Size);
|
|
char CRC = CalcCRC(Frame.data(),FrameSize);
|
|
Frame.append(CRC);
|
|
|
|
return Frame;
|
|
|
|
|
|
#else
|
|
int toto;
|
|
char temp;
|
|
char OutBuffer[MAX_MESSAGE_SIZE+10];
|
|
|
|
int FrameSize = Size + 9; //Add header data...
|
|
|
|
OutBuffer[0] = (char)FRAME_HEADER; //header
|
|
OutBuffer[1] = SenderAddress; //Device address (default = MasterCtrl)
|
|
OutBuffer[2] = SenderDevice; //Device ID (default = MasterCtrl
|
|
OutBuffer[3] = (char)ID; //destination ID
|
|
OutBuffer[4] = (char)Address; //Destination Address
|
|
OutBuffer[5] = (char)Flags;
|
|
OutBuffer[6] = (char)MessageID; //Cmd
|
|
// toto = FrameSize & 0xFF00;
|
|
temp = FrameSize >> 8;
|
|
OutBuffer[7] = ((FrameSize - 9) & 0xFF00) >> 8;
|
|
OutBuffer[8] = (FrameSize - 9) & 0x00FF;
|
|
|
|
for(int i = 0; i < Size; i++)
|
|
{
|
|
temp = Data[i];
|
|
OutBuffer[i+9] = temp;
|
|
}
|
|
|
|
OutBuffer[FrameSize] = CalcCRC(OutBuffer,FrameSize);
|
|
|
|
|
|
// CSerialComm::instance()->WriteData(&OutBuffer[0],FrameSize+1);
|
|
|
|
return QByteArray(OutBuffer,FrameSize);
|
|
#endif
|
|
}
|
|
|
|
//int CNetworkProtocol::TxData(unsigned char MessageID,unsigned char Flags,unsigned char *Data,int Size, unsigned char Address,unsigned char ID)
|
|
//{
|
|
|
|
// int toto;
|
|
// char temp;
|
|
// char OutBuffer[MAX_MESSAGE_SIZE+10];
|
|
|
|
// int FrameSize = Size + 9; //Add header data...
|
|
|
|
// OutBuffer[0] = (char)FRAME_HEADER; //header
|
|
// OutBuffer[1] = 1; //MasterCtrl address
|
|
// OutBuffer[2] = ID_MASTER; //MasterCtrl ID
|
|
// OutBuffer[3] = (char)ID; //destination ID
|
|
// OutBuffer[4] = (char)Address; //Destination Address
|
|
// OutBuffer[5] = (char)Flags;
|
|
// OutBuffer[6] = (char)MessageID; //Cmd
|
|
// toto = FrameSize & 0xFF00;
|
|
// temp = FrameSize >> 8;
|
|
// OutBuffer[7] = ((FrameSize - 9) & 0xFF00) >> 8;
|
|
// OutBuffer[8] = (FrameSize - 9) & 0x00FF;
|
|
|
|
// for(int i = 0; i < Size; i++)
|
|
// {
|
|
// temp = Data[i];
|
|
// OutBuffer[i+9] = temp;
|
|
// }
|
|
|
|
// OutBuffer[FrameSize] = CalcCRC(OutBuffer,FrameSize);
|
|
|
|
|
|
//// CSerialComm::instance()->WriteData(&OutBuffer[0],FrameSize+1);
|
|
|
|
// return 0;
|
|
//}
|
|
|
|
//void CNetworkProtocol::ReadPort(void)
|
|
//{
|
|
// int Size = 0;
|
|
// RxBuff = CSerialComm::instance()->ReceiveData(Size);
|
|
// if(Size)
|
|
// {
|
|
// if(Size > 50)
|
|
// {
|
|
// int toto = 5;
|
|
// }
|
|
//// TRACE("Received %d bytes\n",Size);
|
|
// AnalyseNewData(RxBuff,Size);
|
|
|
|
// }
|
|
//}
|
|
|
|
//void CNetworkProtocol::AnalyseNewData(char *Data, int size)
|
|
//{
|
|
// if(size != 0)
|
|
// {
|
|
// for(int i(0); i < size; i++)
|
|
// {
|
|
// StateMachine((unsigned char)Data[i]);
|
|
// }
|
|
// }
|
|
//}
|
|
|
|
int CNetworkProtocol::RxStateMachine(unsigned char Data)
|
|
{
|
|
int ret = PROTOCOL_RET_OK_PACKET_INCOMPLETE;
|
|
|
|
switch(RxState)
|
|
{
|
|
case RxHeader: //Wait for data header...
|
|
{
|
|
if((unsigned char)Data == FRAME_HEADER)
|
|
{
|
|
mRxData[BufPtr++] = Data;
|
|
RxState = RxAdd;
|
|
CRC ^= Data;
|
|
}
|
|
else
|
|
{
|
|
ResetRxStateMachine();
|
|
ret = PROTOCOL_RET_OK_BAD_HEADER;
|
|
}
|
|
|
|
break;
|
|
}
|
|
case RxAdd: //Sender Address.
|
|
{
|
|
SenderAddress = Data;
|
|
mRxData[BufPtr++] = Data;
|
|
RxState = RxID;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxID: //Sender ID
|
|
{
|
|
mRxData[BufPtr++] = Data;
|
|
RxState = RxMyID;
|
|
SenderID = Data;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxMyID:
|
|
{
|
|
|
|
if(Data != ID_MASTER && Data != 0xFF) //Message is not for Master and it's not a broadcast
|
|
{
|
|
ResetRxStateMachine();
|
|
ret = PROTOCOL_RET_ERROR_INVALID_TARGET_DEVICE;
|
|
|
|
break;
|
|
}
|
|
mRxData[BufPtr++] = Data;
|
|
RxState = RxMyAddress;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxMyAddress:
|
|
{
|
|
if(Data != 1 && Data != 0xFF) //Message is not for us and it's not a broadcast
|
|
{
|
|
ResetRxStateMachine();
|
|
ret = PROTOCOL_RET_ERROR_INVALID_TARGET_ADDRESS;
|
|
|
|
break;
|
|
}
|
|
mRxData[BufPtr++] = Data;
|
|
RxState = RxFlags;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxFlags:
|
|
{
|
|
Flags = Data;
|
|
mRxData[BufPtr++] = Data;
|
|
RxState = RxCMD;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxCMD:
|
|
{
|
|
RxCmd = Data;
|
|
mRxData[BufPtr++] = Data;
|
|
RxState = RxSize1;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxSize1: //Data size MSB
|
|
{
|
|
RxSize = 0;
|
|
RxSize = (unsigned int)Data;
|
|
RxSize <<= 8;
|
|
mRxData[BufPtr++] = Data;
|
|
|
|
|
|
RxState = RxSize2;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxSize2: //Data size MSB
|
|
{
|
|
RxSize |= (unsigned int)Data;
|
|
RxSize <<= 8;
|
|
mRxData[BufPtr++] = Data;
|
|
|
|
|
|
RxState = RxSize3;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxSize3: //Data size MSB
|
|
{
|
|
RxSize |= (unsigned int)Data;
|
|
RxSize <<= 8;
|
|
mRxData[BufPtr++] = Data;
|
|
|
|
|
|
RxState = RxSize4;
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
case RxSize4: //Data size LSB
|
|
{
|
|
RxSize |= (unsigned int)Data;
|
|
mRxData[BufPtr++] = Data;
|
|
|
|
if(RxSize == 0)
|
|
RxState = RxCRC;
|
|
else
|
|
RxState = RxData;
|
|
|
|
CRC ^= Data;
|
|
break;
|
|
}
|
|
|
|
case RxData:
|
|
{
|
|
//mRxData[BufPtr++] = Data;
|
|
mDataBuffer.append(Data);
|
|
CRC ^= Data;
|
|
DataCnt++;
|
|
|
|
if(DataCnt == RxSize)
|
|
RxState = RxCRC;
|
|
|
|
break;
|
|
}
|
|
case RxCRC:
|
|
{
|
|
if(Data != CRC) //Data corruption
|
|
{
|
|
ResetRxStateMachine();
|
|
ret = PROTOCOL_RET_ERROR_BAD_CRC;
|
|
|
|
break;
|
|
}
|
|
|
|
// mDataBuffer = QByteArray(&mRxData[DATA_START],RxSize);
|
|
NewFrameReceived(SenderID,SenderAddress,RxCmd,RxSize,mDataBuffer);
|
|
ret = PROTOCOL_RET_OK_PACKET_COMPLETE;
|
|
if(mIsResetManual == false)
|
|
{
|
|
ResetRxStateMachine();
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
ret = PROTOCOL_RET_ERROR_SM_LOGIC;
|
|
ResetRxStateMachine();
|
|
break;
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
void CNetworkProtocol::ResetRxStateMachine(void)
|
|
{
|
|
RxState = RxHeader;
|
|
RxSize = 0;
|
|
SenderID = 0;
|
|
SenderAddress = 0;
|
|
RxCmd = 0;
|
|
memset(mRxData,0,sizeof(mRxData));
|
|
DataCnt = 0;
|
|
BufPtr = 0;
|
|
CRC = 0;
|
|
Flags = 0;
|
|
mDataBuffer.clear();
|
|
}
|
|
|
|
void CNetworkProtocol::PrepareForNewPacket()
|
|
{
|
|
ResetRxStateMachine();
|
|
}
|
|
|
|
int CNetworkProtocol::AnalyzeRxBuffer(QByteArray Buffer)
|
|
{
|
|
int ret = PROTOCOL_RET_ERROR_EMPTY_BUFFER;
|
|
for(int i = 0; i < Buffer.size(); i++)
|
|
{
|
|
ret = RxStateMachine(Buffer.at(i));
|
|
if(ret != PROTOCOL_RET_OK_PACKET_INCOMPLETE)
|
|
{
|
|
if(ret == PROTOCOL_RET_OK_BAD_HEADER)
|
|
{
|
|
qDebug("Protocol Bad header");
|
|
}
|
|
else
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void CNetworkProtocol::SetManualPacketReset(bool Manual)
|
|
{
|
|
mIsResetManual = Manual;
|
|
}
|