void SerialRXThread::run()
{
//qint64 currms = QDateTime::currentMSecsSinceEpoch();
QString byteoutofpacket;
QByteArray qbuffer;
unsigned char buffer[10240];
bool m_inpacket = false;
bool m_inescape = false;
int m_packetErrorCount=0;
int readlen=0;
while (!m_terminate)
{
readlen = m_serialPort->readBytes(buffer,1024);
if (readlen < 0)
{
//Nothing on the port
qDebug() << "Timeout";
//msleep(10);
}
else
{
//emit dataRead(QByteArray((const char*)buffer,readlen));
/*if (m_logsEnabled)
{
m_logInFile->write((const char*)buffer,readlen);
m_logInFile->flush();
}*/
emit dataRead(QByteArray((const char*)buffer,readlen));
}
if (readlen == 0)
{
//This should be an error
//qDebug() << "Nothing to read";
//perror("Error:");
//printf("\n");
//return; //Disable this, now that we are using timeouts.
//msleep(10);
continue;
}
for (int i=0;i<readlen;i++)
{
if (buffer[i] == 0xAA)
{
if (m_inpacket)
{
//Start byte in the middle of a packet
//Clear out the buffer and start fresh
m_inescape = false;
QByteArray bufToEmit;
bufToEmit.append(0xAA);
QByteArray nbuffer(qbuffer);
nbuffer.replace(0xBB,QByteArray().append(0xBB).append(0x44));
nbuffer.replace(0xAA,QByteArray().append(0xBB).append(0x55));
nbuffer.replace(0xCC,QByteArray().append(0xBB).append(0x33));
bufToEmit.append(nbuffer);
bufToEmit.append(0xCC);
emit dataRead(bufToEmit);
qbuffer.clear();
qDebug() << "Buffer error";
m_packetErrorCount++;
}
//qbuffer.append(buffer[i]);
//qDebug() << "Start of packet";
//Start of packet
m_inpacket = true;
}
else if (buffer[i] == 0xCC && m_inpacket)
{
//qDebug() << "End of packet. Size:" << qbuffer.size();
//End of packet
m_inpacket = false;
//qbuffer.append(buffer[i]);
//m_logFile->flush();
//emit parseBuffer(qbuffer);
//New Location of checksum
unsigned char sum = 0;
for (int i=0;i<qbuffer.size()-1;i++)
{
sum += qbuffer[i];
}
QByteArray bufToEmit;
bufToEmit.append(0xAA);
QByteArray nbuffer(qbuffer);
nbuffer.replace(0xBB,QByteArray().append(0xBB).append(0x44));
nbuffer.replace(0xAA,QByteArray().append(0xBB).append(0x55));
nbuffer.replace(0xCC,QByteArray().append(0xBB).append(0x33));
bufToEmit.append(nbuffer);
bufToEmit.append(0xCC);
emit dataRead(bufToEmit);
/*if (m_logsEnabled)
{
m_logWriteMutex->lock();
m_logInOutFile->write(QByteArray().append(0xAA));
QByteArray nbuffer(qbuffer);
nbuffer.replace(0xBB,QByteArray().append(0xBB).append(0x44));
nbuffer.replace(0xAA,QByteArray().append(0xBB).append(0x55));
nbuffer.replace(0xCC,QByteArray().append(0xBB).append(0x33));
m_logInOutFile->write((const char*)nbuffer.data(),nbuffer.size());
m_logInOutFile->write(QByteArray().append(0xCC));
m_logInOutFile->flush();
m_logWriteMutex->unlock();
}*/
//qDebug() << "Payload sum:" << QString::number(sum);
//qDebug() << "Checksum sum:" << QString::number((unsigned char)currPacket[currPacket.length()-1]);
if (sum != (unsigned char)qbuffer[qbuffer.size()-1])
{
qDebug() << "BAD CHECKSUM!";
m_packetErrorCount++;
//return QPair<QByteArray,QByteArray>();
}
else
{
m_packetErrorCount=0;
//m_queuedMessages.append(qbuffer.mid(0,qbuffer.length()-1));
emit incomingPacket(qbuffer.mid(0,qbuffer.length()-1));
}
//return qbuffer;
QString output;
for (int i=0;i<qbuffer.size();i++)
{
int num = (unsigned char)qbuffer[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
//qDebug() << "Full packet:";
//qDebug() << output;
qbuffer.clear();
}
else
{
if (m_inpacket && !m_inescape)
{
if (buffer[i] == 0xBB)
{
//Need to escape the next byte
//retval = logfile.read(1);
m_inescape = true;
}
else
{
qbuffer.append(buffer[i]);
}
}
else if (m_inpacket && m_inescape)
{
if (buffer[i] == 0x55)
{
qbuffer.append((char)0xAA);
}
else if (buffer[i] == 0x44)
{
qbuffer.append((char)0xBB);
}
else if (buffer[i] == 0x33)
{
qbuffer.append((char)0xCC);
}
else
{
qDebug() << "Error, escaped character is not valid!:" << QString::number(buffer[i],16);
m_packetErrorCount++;
}
m_inescape = false;
}
else
{
//qDebug() << "Byte out of a packet:" << QString::number(buffer[i],16);
byteoutofpacket += QString::number(buffer[i],16) + " ";
}
}
}
//qDebug() << "Bytes out of a packet:" << byteoutofpacket;
}
//m_buffer.append(qbuffer);
if (readlen > 0)
{
//m_buffer.write(buffer,readlen);
// m_buffer.append((const char*)buffer,readlen);
}
//return m_packetErrorCount;
return;
}
int SerialThread::readSerial(int timeout)
{
if (!m_logInFile || !m_logInOutFile)
{
if (m_logsEnabled)
{
openLogs();
}
}
qint64 currms = QDateTime::currentMSecsSinceEpoch();
int readlen = m_buffer.size();
QByteArray qbuffer;
if (m_buffer.size() > 10240)
{
//Error here somehow;
Q_ASSERT(m_buffer.size() < 10240);
}
unsigned char buffer[10240];
qbuffer.append(m_buffer);
/*for (int i=0;i<m_buffer.size();i++)
{
buffer[i] = m_buffer[i];
qbuffer.append(m_buffer[i]);
}*/
m_buffer.clear();
//bool inpacket = false;
//bool inescape = false;
QString byteoutofpacket;
while (currms + timeout > QDateTime::currentMSecsSinceEpoch())
{
#ifdef Q_OS_WIN32
if (!ReadFile(m_portHandle,(LPVOID)buffer,1024,(LPDWORD)&readlen,NULL))
{
//Serial error here
qDebug() << "Serial Read error";
}
#else
readlen = read(m_portHandle,buffer,1024);
#endif //Q_OS_WIN32
if (readlen < 0)
{
//Nothing on the port
msleep(10);
}
else
{
if (m_logsEnabled)
{
m_logInFile->write((const char*)buffer,readlen);
m_logInFile->flush();
}
}
if (readlen == 0)
{
msleep(10);
}
for (int i=0;i<readlen;i++)
{
if (buffer[i] == 0xAA)
{
if (m_inpacket)
{
//Start byte in the middle of a packet
//Clear out the buffer and start fresh
m_inescape = false;
if (m_logsEnabled)
{
m_logWriteMutex.lock();
m_logInOutFile->write(QByteArray().append(0xAA));
QByteArray nbuffer(qbuffer);
nbuffer.replace(0xBB,QByteArray().append(0xBB).append(0x44));
nbuffer.replace(0xAA,QByteArray().append(0xBB).append(0x55));
nbuffer.replace(0xCC,QByteArray().append(0xBB).append(0x33));
m_logInOutFile->write((const char*)nbuffer.data(),nbuffer.size());
m_logInOutFile->write(QByteArray().append(0xCC));
m_logInOutFile->flush();
m_logWriteMutex.unlock();
}
qbuffer.clear();
qDebug() << "Buffer error";
m_packetErrorCount++;
}
//qbuffer.append(buffer[i]);
//qDebug() << "Start of packet";
//Start of packet
m_inpacket = true;
}
else if (buffer[i] == 0xCC && m_inpacket)
{
//qDebug() << "End of packet. Size:" << qbuffer.size();
//End of packet
m_inpacket = false;
//qbuffer.append(buffer[i]);
//m_logFile->flush();
//emit parseBuffer(qbuffer);
//New Location of checksum
unsigned char sum = 0;
for (int i=0;i<qbuffer.size()-1;i++)
{
sum += qbuffer[i];
}
if (m_logsEnabled)
{
m_logWriteMutex.lock();
m_logInOutFile->write(QByteArray().append(0xAA));
QByteArray nbuffer(qbuffer);
nbuffer.replace(0xBB,QByteArray().append(0xBB).append(0x44));
nbuffer.replace(0xAA,QByteArray().append(0xBB).append(0x55));
nbuffer.replace(0xCC,QByteArray().append(0xBB).append(0x33));
m_logInOutFile->write((const char*)nbuffer.data(),nbuffer.size());
m_logInOutFile->write(QByteArray().append(0xCC));
m_logInOutFile->flush();
m_logWriteMutex.unlock();
}
//qDebug() << "Payload sum:" << QString::number(sum);
//qDebug() << "Checksum sum:" << QString::number((unsigned char)currPacket[currPacket.length()-1]);
if (sum != (unsigned char)qbuffer[qbuffer.size()-1])
{
qDebug() << "BAD CHECKSUM!";
m_packetErrorCount++;
//return QPair<QByteArray,QByteArray>();
}
else
{
m_packetErrorCount=0;
m_queuedMessages.append(qbuffer.mid(0,qbuffer.length()-1));
}
//return qbuffer;
QString output;
for (int i=0;i<qbuffer.size();i++)
{
int num = (unsigned char)qbuffer[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
//qDebug() << "Full packet:";
//qDebug() << output;
qbuffer.clear();
}
else
{
if (m_inpacket && !m_inescape)
{
if (buffer[i] == 0xBB)
{
//Need to escape the next byte
//retval = logfile.read(1);
m_inescape = true;
}
else
{
qbuffer.append(buffer[i]);
}
}
else if (m_inpacket && m_inescape)
{
if (buffer[i] == 0x55)
{
qbuffer.append((char)0xAA);
}
else if (buffer[i] == 0x44)
{
qbuffer.append((char)0xBB);
}
else if (buffer[i] == 0x33)
{
qbuffer.append((char)0xCC);
}
else
{
qDebug() << "Error, escaped character is not valid!:" << QString::number(buffer[i],16);
m_packetErrorCount++;
}
m_inescape = false;
}
else
{
//qDebug() << "Byte out of a packet:" << QString::number(buffer[i],16);
byteoutofpacket += QString::number(buffer[i],16) + " ";
}
}
}
//qDebug() << "Bytes out of a packet:" << byteoutofpacket;
}
m_buffer.append(qbuffer);
if (readlen > 0)
{
//m_buffer.write(buffer,readlen);
// m_buffer.append((const char*)buffer,readlen);
}
return m_packetErrorCount;
}
