uprobeprotocol.cpp
182 lines
| 5.0 KiB
| text/x-c
|
CppLexer
/ src / uprobeprotocol.cpp
kaveh
|
r4 | /*------------------------------------------------------------------------------ | ||
-- This file is a part of the Uprobe Software | ||||
-- Copyright (C) 2014, Plasma Physics Laboratory - CNRS | ||||
-- | ||||
-- This program is free software; you can redistribute it and/or modify | ||||
-- it under the terms of the GNU General Public License as published by | ||||
-- the Free Software Foundation; either version 2 of the License, or | ||||
-- (at your option) any later version. | ||||
-- | ||||
-- This program is distributed in the hope that it will be useful, | ||||
-- but WITHOUT ANY WARRANTY; without even the implied warranty of | ||||
-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||||
-- GNU General Public License for more details. | ||||
-- | ||||
-- You should have received a copy of the GNU General Public License | ||||
-- along with this program; if not, write to the Free Software | ||||
-- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA | ||||
-------------------------------------------------------------------------------*/ | ||||
/*-- Author : Alexis Jeandet | ||||
-- Mail : alexis.jeandet@member.fsf.org | ||||
-- Author : Kaveh Mohamadabadi | ||||
-- Mail : kaveh.mohamadabadi@lpp.polytechnique.fr | ||||
--------------------------------------------------------------------------------*/ | ||||
kaveh
|
r2 | #include "uprobeprotocol.h" | ||
UProbeProtocol::UProbeProtocol(QObject *parent) : | ||||
kaveh
|
r4 | QThread(parent),UComPort(),p_datacount(0),p_index(0) | ||
kaveh
|
r2 | { | ||
moveToThread((QThread*)this); | ||||
} | ||||
UProbeProtocol::~UProbeProtocol() | ||||
{ | ||||
this->requestInterruption(); | ||||
while(isRunning()); | ||||
} | ||||
void UProbeProtocol::run() | ||||
{ | ||||
kaveh
|
r3 | p_buffer = (char*)malloc(bufferSize); | ||
if(p_buffer==NULL)return; | ||||
kaveh
|
r2 | while(!this->isInterruptionRequested()) | ||
{ | ||||
kaveh
|
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 | ||||
kaveh
|
r4 | emit packetReady(currentPacket); | ||
kaveh
|
r3 | } | ||
} | ||||
p_index++; | ||||
} | ||||
kaveh
|
r2 | } | ||
} | ||||
void UProbeProtocol::setPortName(const QString &name) | ||||
{ | ||||
UComPort::setPortName(name); | ||||
} | ||||
void UProbeProtocol::setSpeed(int speed) | ||||
{ | ||||
UComPort::setSpeed(speed); | ||||
} | ||||
kaveh
|
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; | ||||
kaveh
|
r4 | currentPacket->channelCount = 0xF & ((int)(p_buffer[p_index+4]>>4) +1); | ||
kaveh
|
r3 | 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; | ||||
} | ||||