379 lines
11 KiB
C++
379 lines
11 KiB
C++
/*******************************************************************************
|
|
* *
|
|
* Société de Transports de Montréal. *
|
|
* 2012 *
|
|
* *
|
|
* Projet Zones Tests *
|
|
* *
|
|
* *
|
|
* *
|
|
*******************************************************************************/
|
|
/*
|
|
Description:
|
|
Lecture et décodage des sondes Laser physiques dans la voie. La classe lit
|
|
le port série lorsque des données sont disponibles, décode les données et les
|
|
rend disponibles via la fonction GetLastData().
|
|
|
|
*/
|
|
|
|
/* ************************************************************************** */
|
|
/* Revision:
|
|
### 20121221 JFM
|
|
Verision d'origine.
|
|
|
|
### YYYYMMDD Description du besoin ou du bug
|
|
Description du changement.
|
|
20141118 JFM
|
|
Reset the mProbeAlive flag to "true" when signal comes back.
|
|
*/
|
|
|
|
/* ************************************************************************** */
|
|
|
|
#include "LazerProbe.h"
|
|
#include <QtDebug>
|
|
#include "LazerProbesMgr.h"
|
|
#include <sys/io.h>
|
|
#include <sys/ioctl.h>
|
|
#include <linux/serial.h>
|
|
#include <asm/ioctls.h>
|
|
#include "ZTLog.h"
|
|
|
|
CLazerProbe::CLazerProbe(unsigned int ProbeID, unsigned int ProbeType):
|
|
mIsAcquisitioning(false),
|
|
mSerialPort(NULL)
|
|
|
|
{
|
|
mLazerProbeRxState = LP_WAIT_HEADER1_STATE;
|
|
mHeader1 = mDataNibble = mData1 = mData2 = mHeader2 = 0;
|
|
mProbeType = ProbeType;
|
|
mProbeID = ProbeID;
|
|
mLPCurData = 0;
|
|
mIsLazerProbeAlive = true;
|
|
mRunThread = true;
|
|
|
|
}
|
|
|
|
CLazerProbe::~CLazerProbe()
|
|
{
|
|
close(fd);
|
|
}
|
|
|
|
unsigned int CLazerProbe::OpenPort(QString PortName)
|
|
{
|
|
////////////////////////////////////////////////////////////////////////////////////
|
|
///NOTE SUR CE GESTIONNAIRE DE PORT SÉRIE:
|
|
///
|
|
///Linux étant ce qu'il est, il est nécessaire de faire une petite passe-passe afin d'exploiter le port série efficacement.
|
|
///Les sondes lazer envoient 3 bytes par message, afin d'avoir une bonne synchronisation entre le train et les données des
|
|
///sondes, il est impératif d'analyser les nouvelles données au moment même où elles arrivent.
|
|
///Le problème est que le chip UART 16550A possède une FIFO interne qui est automatiquement configurée à 8 bytes par le kernel
|
|
///lorsqu'on ouvre le port. Ceci empèche le traîtement des données en temps réel car nous avons toujours au moins 2 échantillons
|
|
///qui sont perdus pour chaque lecture du port série. Seule la désactivation de la FIFO permet de lire 1 byte à la fois par des
|
|
///moyens "traditionnels"
|
|
///Il n'y a aucun moyen de désactiver la FIFO interne autrement qu'en écrivant directement dans le registre de la carte avec
|
|
///la fonction outb() (cette méthode est aussi implémentée, voir le #define USE_UART_HARDWARE_CTRL). Par contre, cette méthode
|
|
///nécessite les privilèges ROOT (utilisation de la fonction setuid() ). Ceci rend le debugging impossible autrement qu'en passant
|
|
///par un remote debugger...
|
|
///
|
|
///En regardant le code source du kernel driver, j'ai remarqué que si le "device" était un 16550 plutôt qu'un 16550A, la fifo était désactivée
|
|
///lors du changement de baudrate, ceci pour des raisons historiques de compatibilité. On peut donc exploiter ce "bug" en forcant
|
|
///le device à un 16550 avant d'ajuster le baudrate. Comme par magie, la FIFO est désactivée et tout fonctionne bien.
|
|
///
|
|
///J-F Martel. 2014/01/27
|
|
/////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
QString PortPath = "/dev/";
|
|
PortPath += PortName;
|
|
fd = open(PortPath.toLatin1().data(), O_RDONLY | O_NOCTTY | O_NDELAY | O_NONBLOCK);
|
|
if (fd < 0)
|
|
{
|
|
CEngLog::instance()->AddLogString(QString().sprintf("Erreur d'ouverture de la sonde EXT #%d sur le port %s : %s",mProbeID,PortName.toUtf8().constData(),strerror (errno)));
|
|
return RET_ERROR;
|
|
}
|
|
CEngLog::instance()->AddLogString(QString().sprintf("Ouverture de la sonde laser INT #%d réussie sur le port %s",mProbeID,PortName.toUtf8().constData()),1);
|
|
|
|
//Faisons un peu de magie !
|
|
//On accède directement au driver avec ioctl (privilèges ROOT non requis).
|
|
//On lit les paramètres du port série détectés au démarrage par le driver.
|
|
struct serial_struct serinfo;
|
|
if(ioctl(fd,TIOCGSERIAL,&serinfo) != 0)
|
|
{
|
|
CEngLog::instance()->AddLogString(QString().sprintf("Impossible d'accéder au driver du port série %s",PortName.toUtf8().constData()),1);
|
|
close(fd);
|
|
return RET_ERROR;
|
|
}
|
|
|
|
#ifndef CUSTOM_KERNEL_INSTALLED
|
|
//C'est ici que l'on force le port à être un 16550 plutôt qu'un 16550A.
|
|
//Le port fonctionne de la même manière mais le driver force la désactivation du FIFO interne.
|
|
//Aussi, on active le flag LOW_LATENCY pour que le kernel remonte les données au fur et à
|
|
//mesure qu'elles arrivent du port.
|
|
//Il est important de faire cette opération avant de modifier le baudrate du port
|
|
|
|
|
|
serinfo.type = PORT_16550;
|
|
#endif
|
|
|
|
serinfo.flags |= ASYNCB_LOW_LATENCY;
|
|
if(ioctl(fd,TIOCSSERIAL,&serinfo) != 0)
|
|
{
|
|
CEngLog::instance()->AddLogString(QString().sprintf("Impossible de fixer le type du port série %s",PortName.toUtf8().constData()),1);
|
|
close(fd);
|
|
return RET_ERROR;
|
|
}
|
|
|
|
|
|
//De retour à la configuration "traditionnelle" du port série.
|
|
struct termios tty;
|
|
memset (&tty, 0, sizeof tty);
|
|
if (tcgetattr (fd, &tty) != 0)
|
|
{
|
|
CEngLog::instance()->AddLogString(QString().sprintf("Impossible de configurer le port série %s",PortName.toUtf8().constData()),1);
|
|
close(fd);
|
|
return RET_ERROR;
|
|
}
|
|
|
|
cfsetospeed (&tty, B38400);
|
|
cfsetispeed (&tty, B38400);
|
|
|
|
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
|
|
tty.c_iflag &= ~IGNBRK; // ignore break signal
|
|
tty.c_lflag = 0; // no signaling chars, no echo,
|
|
tty.c_oflag = 0; // no remapping, no delays
|
|
tty.c_cc[VMIN] = 0; // read doesn't block
|
|
tty.c_cc[VTIME] = 0; // no read timeout
|
|
|
|
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl
|
|
|
|
tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
|
|
// enable reading
|
|
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
|
|
tty.c_cflag |= CSTOPB; //2 STOP BITS JFM 20140210
|
|
tty.c_cflag &= ~CRTSCTS;
|
|
|
|
tcflush(fd,TCIOFLUSH);
|
|
|
|
if (tcsetattr (fd, TCSANOW, &tty) != 0)
|
|
{
|
|
CEngLog::instance()->AddLogString(QString().sprintf("Impossible de configurer le port série %s",PortName.toUtf8().constData()),1);
|
|
close(fd);
|
|
return RET_ERROR;
|
|
}
|
|
|
|
return RET_OK;
|
|
|
|
}
|
|
|
|
unsigned int CLazerProbe::SetUID()
|
|
{
|
|
|
|
return 0;
|
|
}
|
|
|
|
unsigned int CLazerProbe::ResetUID()
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
void CLazerProbe::ProbeDataAvailable()
|
|
{
|
|
|
|
qDebug("Probe thread started...");
|
|
mDataIntervalTimer.start();
|
|
|
|
char buf[1024];
|
|
int size;
|
|
|
|
bool run = true;
|
|
|
|
while(run)
|
|
{
|
|
size = read(fd,buf,1024);
|
|
if(size > 0)
|
|
{
|
|
for(int i = 0; i < size; i++)
|
|
LPRxStateMachine(buf[i]);
|
|
|
|
mDataIntervalTimer.start();
|
|
if(mIsLazerProbeAlive == false)
|
|
{
|
|
mRdWrLock.lockForWrite();
|
|
mIsLazerProbeAlive = true;
|
|
mRdWrLock.unlock();
|
|
CZTLog::instance()->AddLogString(QString().sprintf("Fin panne sonde lazer ID %d ",mProbeID),true);
|
|
}
|
|
}
|
|
|
|
if(mDataIntervalTimer.elapsed() > 1000) //If we didn't receive anything for more than 1 second
|
|
{
|
|
if(mIsLazerProbeAlive == true)
|
|
{
|
|
mRdWrLock.lockForWrite();
|
|
mIsLazerProbeAlive = false;
|
|
mRdWrLock.unlock();
|
|
|
|
CZTLog::instance()->AddLogString(QString().sprintf("Panne équipement sonde lazer ID %d",mProbeID),true);
|
|
}
|
|
}
|
|
mRdWrLock.lockForRead();
|
|
run = mRunThread;
|
|
mRdWrLock.unlock();
|
|
}
|
|
|
|
// qDebug("Probe thread quit : %d",mProbeID);
|
|
}
|
|
|
|
unsigned int CLazerProbe::LPRxStateMachine(unsigned char newbyte)
|
|
{
|
|
QString mRxString;
|
|
static int datacount = 0;
|
|
|
|
switch(mLazerProbeRxState)
|
|
{
|
|
case LP_IGNORE_DATA_STATE:
|
|
{
|
|
//The system is not acquisitioning so simply ignore and discard byte
|
|
break;
|
|
}
|
|
case LP_WAIT_HEADER1_STATE:
|
|
{
|
|
mHeader1 = newbyte & LAZER_PROBE_HEADER_MASK;
|
|
if(mHeader1 == 0x54 || mHeader1 == 0xA8)
|
|
{
|
|
mDataNibble = newbyte & LAZER_PROBE_DATA_NIBBLE_MASK;
|
|
mLazerProbeRxState = LP_GET_DATA1_STATE;
|
|
}
|
|
else
|
|
{
|
|
mHeader1 = 0;
|
|
qDebug("Bad header 0x%x",newbyte);
|
|
}
|
|
|
|
break;
|
|
}
|
|
case LP_GET_DATA1_STATE:
|
|
{
|
|
mData1 = newbyte;
|
|
mLazerProbeRxState = LP_GET_DATA2_STATE;
|
|
break;
|
|
}
|
|
case LP_GET_DATA2_STATE:
|
|
{
|
|
mData2 = newbyte;
|
|
mLazerProbeRxState = LP_CHECK_HEADER2_STATE;
|
|
break;
|
|
}
|
|
case LP_CHECK_HEADER2_STATE:
|
|
{
|
|
mHeader2 = newbyte & LAZER_PROBE_HEADER_MASK;
|
|
|
|
|
|
//We do this verification to ensure that we are still in sync with the frame.
|
|
//If Header1 is not 1's complement of Header2, reset the state machine and
|
|
//wait for a new header...
|
|
if(mHeader1 == (~mHeader2 & 0xFC))
|
|
{
|
|
unsigned int mTempData = 0;
|
|
|
|
mTempData = mData2;
|
|
mTempData <<= 8;
|
|
|
|
mTempData += (unsigned int)mData1;
|
|
|
|
|
|
mTempData <<= 2;
|
|
|
|
|
|
if(mTempData >= 100000 && mTempData <= 200000) //filter results out of the sensor's range
|
|
{
|
|
|
|
mRdWrLock.lockForWrite();
|
|
mLPCurData = mTempData;
|
|
mRdWrLock.unlock();
|
|
}
|
|
|
|
|
|
|
|
//This is used for the Engineering page only so we can display
|
|
//the value in semi-realtime each 500ms (1 sample / 500)
|
|
if(datacount++ >= 500)
|
|
{
|
|
emit NewProbeData(mTempData,mProbeType);
|
|
datacount = 0;
|
|
}
|
|
|
|
//The Header2 is now the Header1 of the next frame.
|
|
//So we need to continue to LP_GET_DATA1_STATE
|
|
mHeader1 = mHeader2;
|
|
mDataNibble = newbyte & LAZER_PROBE_DATA_NIBBLE_MASK;
|
|
mLazerProbeRxState = LP_GET_DATA1_STATE;
|
|
}
|
|
else
|
|
{
|
|
//We are out of sync or there was a transmission error. Ignore this frame and reset.
|
|
mHeader1 = 0;
|
|
mDataNibble = 0;
|
|
mLazerProbeRxState = LP_WAIT_HEADER1_STATE;
|
|
qDebug("Bad header 0x%x",newbyte);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
return LAZER_PROBE_DATA_INVALID;
|
|
}
|
|
|
|
unsigned int CLazerProbe::LPAnalyseNewData()
|
|
{
|
|
return RET_OK;
|
|
}
|
|
|
|
unsigned int CLazerProbe::StartAcquisition()
|
|
{
|
|
mLazerProbeRxState = LP_WAIT_HEADER1_STATE;
|
|
mHeader1 = mDataNibble = mData1 = mData2 = mHeader2 = 0;
|
|
mIsAcquisitioning = true;
|
|
return RET_OK;
|
|
}
|
|
unsigned int CLazerProbe::StopAcquisition()
|
|
{
|
|
mIsAcquisitioning = false;
|
|
mLazerProbeRxState = LP_IGNORE_DATA_STATE;
|
|
return RET_OK;
|
|
}
|
|
|
|
unsigned int CLazerProbe::GetLastData()
|
|
{
|
|
static unsigned int Last = 0;
|
|
|
|
mRdWrLock.lockForRead();
|
|
Last = mLPCurData;
|
|
mRdWrLock.unlock();
|
|
|
|
|
|
return Last;
|
|
}
|
|
|
|
unsigned int CLazerProbe::FlushProbeData()
|
|
{
|
|
mSerialPort->flush();
|
|
return RET_OK;
|
|
}
|
|
|
|
bool CLazerProbe::IsProbeAlive()
|
|
{
|
|
bool temp;
|
|
mRdWrLock.lockForRead();
|
|
temp = mIsLazerProbeAlive;
|
|
mRdWrLock.unlock();
|
|
|
|
return temp;
|
|
}
|
|
|
|
void CLazerProbe::QuitProbeThread()
|
|
{
|
|
mRdWrLock.lockForWrite();
|
|
mRunThread = false;
|
|
mRdWrLock.unlock();
|
|
}
|