uprobeprotocol.cpp
159 lines
| 3.6 KiB
| text/x-c
|
CppLexer
/ src / uprobeprotocol.cpp
|
r2 | #include "uprobeprotocol.h" | ||
UProbeProtocol::UProbeProtocol(QObject *parent) : | ||||
QThread(parent),UComPort() | ||||
{ | ||||
moveToThread((QThread*)this); | ||||
} | ||||
UProbeProtocol::~UProbeProtocol() | ||||
{ | ||||
this->requestInterruption(); | ||||
while(isRunning()); | ||||
} | ||||
void UProbeProtocol::run() | ||||
{ | ||||
|
r3 | p_buffer = (char*)malloc(bufferSize); | ||
if(p_buffer==NULL)return; | ||||
|
r2 | while(!this->isInterruptionRequested()) | ||
{ | ||||
|
r3 | getMoreData(); | ||
while(p_index<p_datacount) | ||||
{ | ||||
if(p_buffer[p_index]==(char)0xA5) | ||||
{ | ||||
//got to Sync state | ||||
while (p_datacount<(p_index+6)) | ||||
{ | ||||
getMoreData(); | ||||
} | ||||
if(FSM_SyncState()==true) | ||||
{ | ||||
p_index+=currentPacket->packetSize + 6; | ||||
//forward DATA | ||||
|
r2 | |||
|
r3 | } | ||
} | ||||
p_index++; | ||||
} | ||||
|
r2 | } | ||
} | ||||
void UProbeProtocol::setPortName(const QString &name) | ||||
{ | ||||
UComPort::setPortName(name); | ||||
} | ||||
void UProbeProtocol::setSpeed(int speed) | ||||
{ | ||||
UComPort::setSpeed(speed); | ||||
} | ||||
|
r3 | |||
bool UProbeProtocol::open() | ||||
{ | ||||
return UComPort::open(); | ||||
} | ||||
bool UProbeProtocol::open(const QString &name, int speed) | ||||
{ | ||||
return UComPort::open(name,speed); | ||||
} | ||||
bool UProbeProtocol::close() | ||||
{ | ||||
return UComPort::close(); | ||||
} | ||||
void UProbeProtocol::getMoreData() | ||||
{ | ||||
int avail; | ||||
int maxCount=(bufferSize-32)-p_datacount; | ||||
if(p_index>(bufferSize/2)) | ||||
{ | ||||
memcpy(p_buffer,p_buffer+p_index,p_datacount-p_index); | ||||
p_datacount = p_datacount-p_index; | ||||
p_index = 0; | ||||
} | ||||
if(opened()) | ||||
{ | ||||
avail=availableBytes(); | ||||
if(avail) | ||||
{ | ||||
if(maxCount>=avail) | ||||
{ | ||||
p_datacount += readBytes(p_buffer+p_datacount,avail); | ||||
} | ||||
else | ||||
{ | ||||
p_datacount += readBytes(p_buffer+p_datacount,maxCount); | ||||
} | ||||
} | ||||
} | ||||
} | ||||
bool UProbeProtocol::FSM_SyncState() | ||||
{ | ||||
if(p_buffer[p_index+1]==(char)0x0F0) | ||||
{ | ||||
return FSM_ExtractHeaderState(); | ||||
} | ||||
else | ||||
{ | ||||
return false; | ||||
} | ||||
} | ||||
bool UProbeProtocol::FSM_ExtractHeaderState() | ||||
{ | ||||
currentPacket = new UProbeProtocolPacket(); | ||||
currentPacket->packetNumber = p_buffer[p_index+2]; | ||||
currentPacket->packetSize = ((unsigned int)p_buffer[p_index+3] + 1)*2; | ||||
currentPacket->channelCount = 0xF & (int)(p_buffer[p_index+4]>>4); | ||||
currentPacket->coding = (UProbeProtocol::UProbeProtocolPacket::PacketCoding)(0x3 & (int)(p_buffer[p_index+4]>>2)); | ||||
currentPacket->resolution = (UProbeProtocol::UProbeProtocolPacket::PacketResolution)(0x3 & (int)(p_buffer[p_index+4])); | ||||
currentPacket->ID = (unsigned int)p_buffer[p_index+5]; | ||||
while(p_datacount<(p_index+2+4+currentPacket->packetSize+1)) | ||||
{ | ||||
getMoreData(); | ||||
} | ||||
return FSM_ExtractPayloadState(); | ||||
} | ||||
bool UProbeProtocol::FSM_ExtractPayloadState() | ||||
{ | ||||
if(!currentPacket || currentPacket->packetSize < 2 || (currentPacket->packetSize>UProbeProtocolMaxPacketSize)) | ||||
return false; | ||||
currentPacket->data = (char*)malloc(currentPacket->packetSize); | ||||
if(currentPacket->data != NULL) | ||||
{ | ||||
memcpy(currentPacket->data,p_buffer+p_index+6,currentPacket->packetSize); | ||||
return FSM_CheckCRCState(); | ||||
} | ||||
return false; | ||||
} | ||||
bool UProbeProtocol::FSM_CheckCRCState() | ||||
{ | ||||
currentPacket->receivedCRC = p_buffer[p_index+6+currentPacket->packetSize]; | ||||
currentPacket->localCRC = spw_CRC(p_buffer+p_index,6+currentPacket->packetSize); | ||||
if(currentPacket->localCRC == currentPacket->receivedCRC) | ||||
{ | ||||
currentPacket->correct = true; | ||||
return true; | ||||
} | ||||
currentPacket->correct = false; | ||||
return false; | ||||
} | ||||