diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 0cd15b3..a36787a 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -61,6 +61,8 @@ public: */ virtual QString getName() = 0; + virtual void requestReset() = 0; + /** * @brief Determine the connection status * diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 0b8e314..180670b 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -215,8 +215,11 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) mavlink_status_t status; static int mavlink09Count = 0; + static int nonmavlinkCount = 0; static bool decodedFirstPacket = false; static bool warnedUser = false; + static bool checkedUserNonMavlink = false; + static bool warnedUserNonMavlink = false; for (int position = 0; position < b.size(); position++) { unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b[position]), &message, &status); @@ -230,6 +233,24 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) emit protocolStatusMessage("MAVLink Version or Baud Rate Mismatch", "Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. Please upgrade the MAVLink version of your autopilot. If your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same."); } + if (decodeState == 0 && !decodedFirstPacket) + { + nonmavlinkCount++; + if (nonmavlinkCount > 500 && !warnedUserNonMavlink) + { + //500 bytes with no mavlink message. Are we connected to a mavlink capable device? + if (!checkedUserNonMavlink) + { + link->requestReset(); + checkedUserNonMavlink = true; + } + else + { + warnedUserNonMavlink = true; + emit protocolStatusMessage("MAVLink Baud Rate Mismatch", "Please check if the baud rates of QGroundControl and your autopilot are the same."); + } + } + } if (decodeState == 1) { decodedFirstPacket = true; diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 64520a5..6c18111 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -54,7 +54,7 @@ public: qint64 bytesAvailable(); void run(); - + void requestReset() { } bool connect(); bool disconnect(); diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index ae2cde0..b9bb953 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -213,7 +213,8 @@ SerialLink::SerialLink(QString portname, int baudRate, bool hardwareFlowControl, port(NULL), ports(new QVector<QString>()), m_stopp(false), - bytesRead(0) + bytesRead(0), + m_reqReset(false) { // Setup settings this->porthandle = portname.trimmed(); @@ -265,6 +266,11 @@ SerialLink::SerialLink(QString portname, int baudRate, bool hardwareFlowControl, } loadSettings(); } +void SerialLink::requestReset() +{ + QMutexLocker locker(&this->m_stoppMutex); + m_reqReset = true; +} SerialLink::~SerialLink() { @@ -417,6 +423,14 @@ void SerialLink::run() this->m_stopp = false; break; } + if (m_reqReset) + { + this->m_reqReset = false; + communicationUpdate(getName(),"Reset requested via DTR signal"); + port->setDtr(true); + this->msleep(250); + port->setDtr(false); + } } // Check if new bytes have arrived, if yes, emit the notification signal checkForBytes(); diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 5ba8dee..d1f0aa2 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -71,6 +71,8 @@ public: /** @brief Get a list of the currently available ports */ QVector<QString>* getCurrentPorts(); + void requestReset(); + bool isConnected(); qint64 bytesAvailable(); @@ -170,7 +172,8 @@ protected: QVector<QString>* ports; private: - volatile bool m_stopp; + volatile bool m_stopp; + volatile bool m_reqReset; QMutex m_stoppMutex; void setName(QString name); diff --git a/src/comm/SerialLinkInterface.h b/src/comm/SerialLinkInterface.h index cc190b3..59ff901 100644 --- a/src/comm/SerialLinkInterface.h +++ b/src/comm/SerialLinkInterface.h @@ -47,7 +47,7 @@ public: virtual int getBaudRate() = 0; virtual int getDataBits() = 0; virtual int getStopBits() = 0; - + virtual void requestReset() = 0; virtual int getBaudRateType() = 0; virtual int getFlowType() = 0; virtual int getParityType() = 0; diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index 41f1158..82619c0 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -49,6 +49,8 @@ public: //UDPLink(QHostAddress host = "239.255.76.67", quint16 port = 7667); ~UDPLink(); + void requestReset() { } + bool isConnected(); qint64 bytesAvailable(); int getPort() const { diff --git a/src/comm/XbeeLink.h b/src/comm/XbeeLink.h index e07e95f..cc691a4 100644 --- a/src/comm/XbeeLink.h +++ b/src/comm/XbeeLink.h @@ -21,6 +21,7 @@ public: public: // virtual functions from XbeeLinkInterface QString getPortName(); + void requestReset() { } int getBaudRate(); public slots: // virtual functions from XbeeLinkInterface @@ -71,4 +72,4 @@ private: }; -#endif // _XBEELINK_H_ \ No newline at end of file +#endif // _XBEELINK_H_