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; }