|
|
/*------------------------------------------------------------------------------
|
|
|
-- 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
|
|
|
--------------------------------------------------------------------------------*/
|
|
|
#include "uprobeprotocol.h"
|
|
|
|
|
|
UProbeProtocol::UProbeProtocol(QObject *parent) :
|
|
|
QThread(parent),UComPort(),p_datacount(0),p_index(0)
|
|
|
{
|
|
|
moveToThread((QThread*)this);
|
|
|
}
|
|
|
|
|
|
UProbeProtocol::~UProbeProtocol()
|
|
|
{
|
|
|
this->requestInterruption();
|
|
|
while(isRunning());
|
|
|
}
|
|
|
|
|
|
void UProbeProtocol::run()
|
|
|
{
|
|
|
p_buffer = (char*)malloc(bufferSize);
|
|
|
if(p_buffer==NULL)return;
|
|
|
while(!this->isInterruptionRequested())
|
|
|
{
|
|
|
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
|
|
|
emit packetReady(currentPacket);
|
|
|
}
|
|
|
}
|
|
|
p_index++;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
void UProbeProtocol::setPortName(const QString &name)
|
|
|
{
|
|
|
UComPort::setPortName(name);
|
|
|
}
|
|
|
|
|
|
void UProbeProtocol::setSpeed(int speed)
|
|
|
{
|
|
|
UComPort::setSpeed(speed);
|
|
|
}
|
|
|
|
|
|
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) +1);
|
|
|
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;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
