From aedaa844a860195d4056c071d214c38f0276db7c Mon Sep 17 00:00:00 2001 From: Bill Bonney Date: Mon, 1 Jul 2013 11:26:16 -0700 Subject: [PATCH] Stability improvements and error fixes --- libs/serialport/apmserial.pri | 1 - qgroundcontrol.pro | 3 +- src/comm/SerialLink.cc | 182 +++++++++++++++++++++++++++++++--- src/comm/SerialLink.h | 1 + 4 files changed, 172 insertions(+), 15 deletions(-) diff --git a/libs/serialport/apmserial.pri b/libs/serialport/apmserial.pri index 738c48294..9de6a7ab0 100644 --- a/libs/serialport/apmserial.pri +++ b/libs/serialport/apmserial.pri @@ -1,4 +1,3 @@ - INCLUDEPATH += $$PWD unix { diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index ca1222745..0c3dfb754 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -20,7 +20,8 @@ # Qt configuration CONFIG += qt \ - thread \ + thread +# serialport QT += network \ opengl \ diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index a0e52ae98..3b1039aa1 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -137,6 +137,114 @@ void SerialLink::writeSettings() * **/ void SerialLink::run() +{ + // Initialize the connection + if (!hardwareConnect()) + { + //Need to error out here. + emit communicationError(getName(),"Error connecting: " + m_port->errorString()); + return; + } + + // Qt way to make clear what a while(1) loop does + qint64 msecs = QDateTime::currentMSecsSinceEpoch(); + qint64 initialmsecs = QDateTime::currentMSecsSinceEpoch(); + quint64 bytes = 0; + bool triedreset = false; + bool triedDTR = false; + qint64 timeout = 5000; + + forever + { + if(m_stopp) + { + m_stopp = false; + break; // exit the thread + } + + if (m_reqReset) + { + m_reqReset = false; + communicationUpdate(getName(),"Reset requested via DTR signal"); + m_port->setDataTerminalReady(true); + msleep(250); + m_port->setDataTerminalReady(false); + } + bool error = m_port->waitForReadyRead(500); + + if(error) { // Waits for 1/2 second [TODO][BB] lower to SerialLink::poll_interval? + QByteArray readData = m_port->readAll(); + while (m_port->waitForReadyRead(10)) + readData += m_port->readAll(); + if (readData.length() > 0) { + emit bytesReceived(this, readData); +// qDebug() << "rx of length " << QString::number(readData.length()); + + m_bytesRead += readData.length(); + m_bitsReceivedTotal += readData.length() * 8; + } + } else { + qDebug() << "readyReadTime #"<< __LINE__; + + } + + if (bytes != m_bytesRead) // i.e things are good and data is being read. + { + bytes = m_bytesRead; + msecs = QDateTime::currentMSecsSinceEpoch(); + } + else + { + if (QDateTime::currentMSecsSinceEpoch() - msecs > timeout) + { + //It's been 10 seconds since the last data came in. Reset and try again + msecs = QDateTime::currentMSecsSinceEpoch(); + if (msecs - initialmsecs > 25000) + { + //After initial 25 seconds, timeouts are increased to 30 seconds. + //This prevents temporary silences from things like calibration commands + //from screwing things up. In all reality, timeouts should be enabled/disabled + //for events like that on a case by case basis. + //TODO ^^ + timeout = 30000; + } + if (!triedDTR && triedreset) + { + triedDTR = true; + communicationUpdate(getName(),"No data to receive on COM port. Attempting to reset via DTR signal"); + qDebug() << "No data!!! Attempting reset via DTR."; + m_port->setDataTerminalReady(true); + msleep(250); + m_port->setDataTerminalReady(false); + } + else if (!triedreset) + { + qDebug() << "No data!!! Attempting reset via reboot command."; + communicationUpdate(getName(),"No data to receive on COM port. Assuming possible terminal mode, attempting to reset via \"reboot\" command"); + m_port->write("reboot\r\n",8); + triedreset = true; + } + else + { + communicationUpdate(getName(),"No data to receive on COM port...."); + qDebug() << "No data!!!"; + } + } + } + MG::SLEEP::msleep(SerialLink::poll_interval); + } // end of forever + + if (m_port) { // [TODO][BB] Not sure we need to close the port here + qDebug() << "Closing Port #"<< __LINE__ << m_port->portName(); + m_port->close(); + delete m_port; + m_port = NULL; + + emit disconnected(); + emit connected(false); + } +} +void SerialLink::run2() { // Initialize the connection if (!hardwareConnect()) @@ -174,7 +282,60 @@ void SerialLink::run() } } // Check if new bytes have arrived, if yes, emit the notification signal - checkForBytes(); + /* Check if bytes are available */ +// checkForBytes(); + { + if(m_port && m_port->isOpen() && m_port->isWritable()) + { + m_dataMutex.lock(); + qint64 available = m_port->bytesAvailable(); + m_dataMutex.unlock(); + + if(available > 0) + { + //readBytes(); + { + m_dataMutex.lock(); + if(m_port && m_port->isOpen()) { + const qint64 maxLength = 2048; + char data[maxLength]; + qint64 numBytes = m_port->bytesAvailable(); + //qDebug() << "numBytes: " << numBytes; + + if(numBytes > 0) { + /* Read as much data in buffer as possible without overflow */ + if(maxLength < numBytes) numBytes = maxLength; + + m_port->read(data, numBytes); + QByteArray b(data, numBytes); + emit bytesReceived(this, b); + + //qDebug() << "SerialLink::readBytes()" << std::hex << data; + // int i; + // for (i=0; iclose(); + emit disconnected(); + emit connected(false); + emit communicationError(this->getName(), tr("Could not read data - link %1 is disconnected!").arg(this->getName())); + } + }; + } + if (bytes != m_bytesRead) { bytes = m_bytesRead; @@ -254,31 +415,26 @@ void SerialLink::checkForBytes() emit communicationError(this->getName(), tr("Could not read data - link %1 is disconnected!").arg(this->getName())); } } -// else -// { -// emit disconnected(); -// emit connected(false); -// } - } void SerialLink::writeBytes(const char* data, qint64 size) { if(m_port && m_port->isOpen()) { - qDebug() << "writeBytes" << m_portName << "attempting to tx " << size << "bytes."; +// qDebug() << "writeBytes" << m_portName << "attempting to tx " << size << "bytes."; int b = m_port->write(data, size); if (b > 0) { Q_ASSERT_X(b = size, "writeBytes", "failed to write all bytes"); - qDebug() << "writeBytes " << m_portName << "tx'd" << b << "bytes:"; +// qDebug() << "writeBytes " << m_portName << "tx'd" << b << "bytes:"; // Increase write counter m_bitsSentTotal += size * 8; - QByteArray* byteArray = new QByteArray(data,size); - qDebug() << byteArray->toHex(); - delete byteArray; + // Extra debug logging +// QByteArray* byteArray = new QByteArray(data,size); +// qDebug() << byteArray->toHex(); +// delete byteArray; } else { disconnect(); @@ -373,7 +529,7 @@ bool SerialLink::disconnect() * @return True if connection has been established, false if connection couldn't be established. **/ bool SerialLink::connect() -{ +{ if (isRunning()) disconnect(); { diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 883ffad9f..3b4f053f1 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -106,6 +106,7 @@ public: void writeSettings(); void run(); + void run2(); int getLinkQuality(); bool isFullDuplex(); -- 2.22.0