diff --git a/doc/Doxyfile b/doc/Doxyfile index cf2de8e66f5ada18cc27741fc7651fdff0090280..28d0c331eedafb0378f760f7995c60d68a969f81 100644 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -297,7 +297,7 @@ SYMBOL_CACHE_SIZE = 0 # Private class members and static file members will be hidden unless # the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES -EXTRACT_ALL = NO +EXTRACT_ALL = YES # If the EXTRACT_PRIVATE tag is set to YES all private members of a class # will be included in the documentation. @@ -976,7 +976,7 @@ FORMULA_FONTSIZE = 10 # there is already a search function so this one should typically # be disabled. -SEARCHENGINE = NO +SEARCHENGINE = YES #--------------------------------------------------------------------------- # configuration options related to the LaTeX output @@ -1501,7 +1501,7 @@ DOT_TRANSPARENT = YES # makes dot run faster, but since only newer versions of dot (>1.8.10) # support this, this feature is disabled by default. -DOT_MULTI_TARGETS = NO +DOT_MULTI_TARGETS = YES # If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will # generate a legend page explaining the meaning of the various boxes and diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index a7c721df2cca10ad3b2ea23a96efc204f56abc50..8ecc2e1fc8f05b45db1c251f79e9f429efcb201b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -216,3 +216,18 @@ SOURCES += src/main.cc \ src/ui/QGCDataPlot2D.cc \ src/ui/linechart/IncrementalPlot.cc RESOURCES = mavground.qrc + +# Include RT-LAB Library +win32 { + LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin -lOpalApi + INCLUDEPATH += src/lib/opalrt + SOURCES += src/comm/OpalLink.cc + HEADERS += src/comm/OpalLink.h + DEFINES += OPAL_RT +} + +macx { + SOURCES += src/comm/OpalLink.cc + HEADERS += src/comm/OpalLink.h + DEFINES += OPAL_RT +} diff --git a/src/Core.cc b/src/Core.cc index 41a099fab6593ff5167122ec3acae44e5e7956b7..38e719662791cb580cd0e898b49373867c4d663a 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -46,6 +46,9 @@ This file is part of the PIXHAWK project #include "MainWindow.h" #include "GAudioOutput.h" +#ifdef OPAL_RT +#include "OpalLink.h" +#endif #include "UDPLink.h" #include "MAVLinkSimulationLink.h" @@ -132,6 +135,13 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) } } +#ifdef OPAL_RT + // Add OpalRT Link, but do not connect + OpalLink* opalLink = new OpalLink(); + mainWindow->addLink(opalLink); + opalLink->connect(); +#warning OPAL LINK NOW AUTO CONNECTING IN CORE.CC +#endif // MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt"); MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(":/demo-log.txt"); mainWindow->addLink(simulationLink); diff --git a/src/Core.h b/src/Core.h index 2277a30685d91bc09de6df889ff4f86ea5dc3fba..78bb5f63905f9b1657f7bfea1cab01a2441caedb 100644 --- a/src/Core.h +++ b/src/Core.h @@ -39,7 +39,11 @@ This file is part of the PIXHAWK project #include "UASManager.h" #include "LinkManager.h" /*#include "ViconTarsusProtocol.h" */ +#ifdef OPAL_RT +#include "OpalLink.h" + +#endif /** * @brief The main application and management class. * diff --git a/src/comm/AS4Protocol.cc b/src/comm/AS4Protocol.cc index a76490c205d57d0b8f3b98fe6c64c98837ed3052..8b522bb4084d3f3c4e125d722991350918ede025 100644 --- a/src/comm/AS4Protocol.cc +++ b/src/comm/AS4Protocol.cc @@ -99,13 +99,13 @@ void AS4Protocol::receiveBytes(LinkInterface* link) { // receiveMutex.lock(); // Prepare buffer - static const int maxlen = 4096 * 100; - static char buffer[maxlen]; + //static const int maxlen = 4096 * 100; + //static char buffer[maxlen]; qint64 bytesToRead = link->bytesAvailable(); // Get all data at once, let link read the bytes in the buffer array - link->readBytes(buffer, maxlen); + //link->readBytes(buffer, maxlen); // // /* // // Debug output diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index f26dc34dbbf2505f46e2adef92b9d858b38a3499..b632b26ba5533c98082895a4f8bdfbcc181f4232 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -1,33 +1,33 @@ /*===================================================================== - + PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - + (c) 2009 PIXHAWK PROJECT - + This file is part of the PIXHAWK project - - PIXHAWK is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - PIXHAWK is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with PIXHAWK. If not, see . - + +PIXHAWK is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +PIXHAWK is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with PIXHAWK. If not, see . + ======================================================================*/ - + /** - * @file - * @brief Brief Description - * - * @author Lorenz Meier - * - */ +* @file +* @brief Brief Description +* +* @author Lorenz Meier +* +*/ #ifndef _LINKINTERFACE_H_ #define _LINKINTERFACE_H_ @@ -35,215 +35,214 @@ This file is part of the PIXHAWK project #include /** - * The link interface defines the interface for all links used to communicate - * with the groundstation application. - * - **/ +* The link interface defines the interface for all links used to communicate +* with the groundstation application. +* +**/ class LinkInterface : public QThread { - Q_OBJECT + Q_OBJECT public: - //virtual ~LinkInterface() = 0; - - /* Connection management */ - - /** - * @brief Get the ID of this link - * - * The ID is an unsigned integer, starting at 0 - * @return ID of this link - **/ - virtual int getId() = 0; - - /** - * @brief Get the human readable name of this link - */ - virtual QString getName() = 0; - - /** - * @brief Determine the connection status - * - * @return True if the connection is established, false otherwise - **/ - virtual bool isConnected() = 0; - - /* Connection characteristics */ - - /** - * @Brief Get the nominal data rate of the interface. - * - * The nominal data rate is the theoretical maximum data rate of the - * interface. For 100Base-T Ethernet this would be 100 Mbit/s (100'000'000 - * Bit/s, NOT 104'857'600 Bit/s). - * - * @return The nominal data rate of the interface in bit per second, 0 if unknown - * @see getLongTermDataRate() For the mean data rate - * @see getShortTermDataRate() For a the mean data rate of the last seconds - * @see getCurrentDataRate() For the data rate of the last transferred chunk - * @see getMaxDataRate() For the maximum data rate - **/ - virtual qint64 getNominalDataRate() = 0; - - /** - * @brief Full duplex support of this interface. - * - * This method returns true if the interface supports full duplex, which implies - * the full datarate when sending and receiving data simultaneously. - * - * @return True if the interface supports full duplex, false otherwise - **/ - virtual bool isFullDuplex() = 0; - - /** - * @brief Get the link quality. - * - * The link quality is reported as percent, on a scale from 0 to 100% in 1% increments. - * If this feature is not supported by the interface, a call to this method return -1. - * - * @return The link quality in integer percent or -1 if not supported - **/ - virtual int getLinkQuality() = 0; - - /** - * @Brief Get the long term (complete) mean of the data rate - * - * The mean of the total data rate. It is calculated as - * all transferred bits / total link uptime. - * - * @return The mean data rate of the interface in bit per second, 0 if unknown - * @see getNominalDataRate() For the nominal data rate of the interface - * @see getShortTermDataRate() For a the mean data rate of the last seconds - * @see getCurrentDataRate() For the data rate of the last transferred chunk - * @see getMaxDataRate() For the maximum data rate - **/ - virtual qint64 getTotalUpstream() = 0; - - /** - * @Brief Get the current data rate - * - * The datarate of the last 100 ms - * - * @return The mean data rate of the interface in bit per second, 0 if unknown - * @see getNominalDataRate() For the nominal data rate of the interface - * @see getLongTermDataRate() For the mean data rate - * @see getShortTermDataRate() For a the mean data rate of the last seconds - * @see getMaxDataRate() For the maximum data rate - **/ - virtual qint64 getCurrentUpstream() = 0; - - /** - * @Brief Get the maximum data rate - * - * The maximum peak data rate. - * - * @return The mean data rate of the interface in bit per second, 0 if unknown - * @see getNominalDataRate() For the nominal data rate of the interface - * @see getLongTermDataRate() For the mean data rate - * @see getShortTermDataRate() For a the mean data rate of the last seconds - * @see getCurrentDataRate() For the data rate of the last transferred chunk - **/ - virtual qint64 getMaxUpstream() = 0; - - /** - * @Brief Get the total number of bits sent - * - * @return The number of sent bits - **/ - virtual qint64 getBitsSent() = 0; - - /** - * @Brief Get the total number of bits received - * - * @return The number of received bits - * @bug Decide if the bits should be counted fromt the instantiation of the interface or if the counter should reset on disconnect. - **/ - virtual qint64 getBitsReceived() = 0; - - /** - * @brief Connect this interface logically - * - * @return True if connection could be established, false otherwise - **/ - virtual bool connect() = 0; - - /** - * @brief Disconnect this interface logically - * - * @return True if connection could be terminated, false otherwise - **/ - virtual bool disconnect() = 0; - - /** - * @brief Get the current number of bytes in buffer. - * - * @return The number of bytes ready to read - **/ - virtual qint64 bytesAvailable() = 0; + //virtual ~LinkInterface() = 0; + + /* Connection management */ + + /** + * @brief Get the ID of this link + * + * The ID is an unsigned integer, starting at 0 + * @return ID of this link + **/ + virtual int getId() = 0; + + /** + * @brief Get the human readable name of this link + */ + virtual QString getName() = 0; + + /** + * @brief Determine the connection status + * + * @return True if the connection is established, false otherwise + **/ + virtual bool isConnected() = 0; + + /* Connection characteristics */ + + /** + * @Brief Get the nominal data rate of the interface. + * + * The nominal data rate is the theoretical maximum data rate of the + * interface. For 100Base-T Ethernet this would be 100 Mbit/s (100'000'000 + * Bit/s, NOT 104'857'600 Bit/s). + * + * @return The nominal data rate of the interface in bit per second, 0 if unknown + * @see getLongTermDataRate() For the mean data rate + * @see getShortTermDataRate() For a the mean data rate of the last seconds + * @see getCurrentDataRate() For the data rate of the last transferred chunk + * @see getMaxDataRate() For the maximum data rate + **/ + virtual qint64 getNominalDataRate() = 0; + + /** + * @brief Full duplex support of this interface. + * + * This method returns true if the interface supports full duplex, which implies + * the full datarate when sending and receiving data simultaneously. + * + * @return True if the interface supports full duplex, false otherwise + **/ + virtual bool isFullDuplex() = 0; + + /** + * @brief Get the link quality. + * + * The link quality is reported as percent, on a scale from 0 to 100% in 1% increments. + * If this feature is not supported by the interface, a call to this method return -1. + * + * @return The link quality in integer percent or -1 if not supported + **/ + virtual int getLinkQuality() = 0; + + /** + * @Brief Get the long term (complete) mean of the data rate + * + * The mean of the total data rate. It is calculated as + * all transferred bits / total link uptime. + * + * @return The mean data rate of the interface in bit per second, 0 if unknown + * @see getNominalDataRate() For the nominal data rate of the interface + * @see getShortTermDataRate() For a the mean data rate of the last seconds + * @see getCurrentDataRate() For the data rate of the last transferred chunk + * @see getMaxDataRate() For the maximum data rate + **/ + virtual qint64 getTotalUpstream() = 0; + + /** + * @Brief Get the current data rate + * + * The datarate of the last 100 ms + * + * @return The mean data rate of the interface in bit per second, 0 if unknown + * @see getNominalDataRate() For the nominal data rate of the interface + * @see getLongTermDataRate() For the mean data rate + * @see getShortTermDataRate() For a the mean data rate of the last seconds + * @see getMaxDataRate() For the maximum data rate + **/ + virtual qint64 getCurrentUpstream() = 0; + + /** + * @Brief Get the maximum data rate + * + * The maximum peak data rate. + * + * @return The mean data rate of the interface in bit per second, 0 if unknown + * @see getNominalDataRate() For the nominal data rate of the interface + * @see getLongTermDataRate() For the mean data rate + * @see getShortTermDataRate() For a the mean data rate of the last seconds + * @see getCurrentDataRate() For the data rate of the last transferred chunk + **/ + virtual qint64 getMaxUpstream() = 0; + + /** + * @Brief Get the total number of bits sent + * + * @return The number of sent bits + **/ + virtual qint64 getBitsSent() = 0; + + /** + * @Brief Get the total number of bits received + * + * @return The number of received bits + * @bug Decide if the bits should be counted fromt the instantiation of the interface or if the counter should reset on disconnect. + **/ + virtual qint64 getBitsReceived() = 0; + + /** + * @brief Connect this interface logically + * + * @return True if connection could be established, false otherwise + **/ + virtual bool connect() = 0; + + /** + * @brief Disconnect this interface logically + * + * @return True if connection could be terminated, false otherwise + **/ + virtual bool disconnect() = 0; + + /** + * @brief Get the current number of bytes in buffer. + * + * @return The number of bytes ready to read + **/ + virtual qint64 bytesAvailable() = 0; public slots: - /** - * @brief This method allows to write bytes to the interface. - * - * If the underlying communication is packet oriented, - * one write command equals a datagram. In case of serial - * communication arbitrary byte lengths can be written - * - * @param bytes The pointer to the byte array containing the data - * @param length The length of the data array - **/ - virtual void writeBytes(const char *bytes, qint64 length) = 0; - - /** - * @brief Read a number of bytes from the interface. - * - * @param bytes The pointer to write the bytes to - * @param maxLength The maximum length which can be written - **/ - virtual void readBytes(char *bytes, qint64 maxLength) = 0; + /** + * @brief This method allows to write bytes to the interface. + * + * If the underlying communication is packet oriented, + * one write command equals a datagram. In case of serial + * communication arbitrary byte lengths can be written + * + * @param bytes The pointer to the byte array containing the data + * @param length The length of the data array + **/ + virtual void writeBytes(const char *bytes, qint64 length) = 0; signals: - /** - * @brief This signal is emitted when new data is ready to read in the queue - * - * @param link The Interface to read from - **/ - void bytesReady(LinkInterface* link); - - /** - * @brief New data arrived - * - * The use of this function is discouraged for high-performance links. - * - * @param data the new bytes - */ - void bytesReceived(LinkInterface* link, QByteArray data); - - /** - * @brief This signal is emitted instantly when the link is connected - **/ - void connected(); - - /** - * @brief This signal is emitted instantly when the link is disconnected - **/ - void disconnected(); - - /** - * @brief This signal is emitted instantly when the link status changes - **/ - void connected(bool connected); - - /** - * @brief This signal is emitted if the human readable name of this link changes - */ - void nameChanged(QString name); + + /** + * @brief New data arrived + * + * The new data is contained in the QByteArray data. The data is copied for each + * receiving protocol. For high-speed links like image transmission this might + * affect performance, for control links it is however desirable to directly + * forward the link data. + * + * @param data the new bytes + */ + void bytesReceived(LinkInterface* link, QByteArray data); + + /** + * @brief This signal is emitted instantly when the link is connected + **/ + void connected(); + + /** + * @brief This signal is emitted instantly when the link is disconnected + **/ + void disconnected(); + + /** + * @brief This signal is emitted instantly when the link status changes + **/ + void connected(bool connected); + + /** + * @brief This signal is emitted if the human readable name of this link changes + */ + void nameChanged(QString name); protected: - static int getNextLinkId() - { - static int nextId = 0; - return nextId++; - } + static int getNextLinkId() + { + static int nextId = 0; + return nextId++; + } + +protected slots: + +/** + * @brief Read a number of bytes from the interface. + * + * @param bytes The pointer to write the bytes to + * @param maxLength The maximum length which can be written + **/ +virtual void readBytes() = 0; }; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index ad4860aa06e58306456e41ea5996c02dc2ea3e4a..4c1e11d55114607a3f71115502691d7f02447d4a 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -56,7 +56,6 @@ LinkManager::LinkManager() { links = QList(); protocolLinks = QMap(); - start(QThread::LowPriority); } LinkManager::~LinkManager() @@ -64,11 +63,6 @@ LinkManager::~LinkManager() disconnectAll(); } - -void LinkManager::run() -{ -} - void LinkManager::add(LinkInterface* link) { links.append(link); @@ -79,7 +73,7 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol) { // Connect link to protocol // the protocol will receive new bytes from the link - connect(link, SIGNAL(bytesReady(LinkInterface*)), protocol, SLOT(receiveBytes(LinkInterface*))); + connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray))); // Store the connection information in the protocol links map protocolLinks.insertMulti(protocol, link); //qDebug() << __FILE__ << __LINE__ << "ADDED LINK TO PROTOCOL" << link->getName() << protocol->getName() << "NEW SIZE OF LINK LIST:" << protocolLinks.size(); diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 32c7cba224ca998acb0be3518c214a4b2f7b7e5a..b1768758b2a356362c37d93ed85ca5c22c1ca3e9 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -44,7 +44,8 @@ This file is part of the PIXHAWK project * protocol instance to transport the link data into the application. * **/ -class LinkManager : public QThread { +class LinkManager : public QObject +{ Q_OBJECT public: diff --git a/src/comm/MAVLinkLightProtocol.cc b/src/comm/MAVLinkLightProtocol.cc index 2e876bd77cadcdb780eadec1a8383291a80fa5d3..4ce662538bf779f3f9a272cbd7ac5c737e4f8afd 100644 --- a/src/comm/MAVLinkLightProtocol.cc +++ b/src/comm/MAVLinkLightProtocol.cc @@ -86,51 +86,53 @@ void MAVLinkLightProtocol::sendMessage(LinkInterface* link, mavlink_light_messag * @param link The interface to read from * @see LinkInterface **/ -void MAVLinkLightProtocol::receiveBytes(LinkInterface* link) +void MAVLinkLightProtocol::receiveBytes(LinkInterface* link, QByteArray b) { - receiveMutex.lock(); - // Prepare buffer - static const int maxlen = 4096 * 100; - static char buffer[maxlen]; - qint64 bytesToRead = link->bytesAvailable(); - - // Get all data at once, let link read the bytes in the buffer array - link->readBytes(buffer, maxlen); - - // Run through all bytes - for (int position = 0; position < bytesToRead; position++) - { - mavlink_light_message_t msg; - // FIXME PARSE - if (1 == 0/* parsing returned a message */) - { - - int sysid = 0; // system id from message, or always null if only one MAV is supported - UASInterface* uas = UASManager::instance()->getUASForId(sysid); - - // Check and (if necessary) create UAS object - if (uas == NULL) - { - ArduPilotMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists - // Connect this robot to the UAS object - // it is IMPORTANT here to use the right object type, - // else the slot of the parent object is called (and thus the special - // packets never reach their goal) - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); - uas = mav; - // Make UAS aware that this link can be used to communicate with the actual robot - uas->addLink(link); - // Now add UAS to "official" list, which makes the whole application aware of it - UASManager::instance()->addUAS(uas); - } - - // The packet is emitted as a whole, as it is only 255 - 261 bytes short - // kind of inefficient, but no issue for a groundstation pc. - // It buys as reentrancy for the whole code over all threads - emit messageReceived(link, msg); - } - } - receiveMutex.unlock(); + Q_UNUSED(link); + Q_UNUSED(b); +// receiveMutex.lock(); +// // Prepare buffer +// static const int maxlen = 4096 * 100; +// static char buffer[maxlen]; +// qint64 bytesToRead = link->bytesAvailable(); +// +// // Get all data at once, let link read the bytes in the buffer array +// link->readBytes(buffer, maxlen); +// +// // Run through all bytes +// for (int position = 0; position < bytesToRead; position++) +// { +// mavlink_light_message_t msg; +// // FIXME PARSE +// if (1 == 0/* parsing returned a message */) +// { +// +// int sysid = 0; // system id from message, or always null if only one MAV is supported +// UASInterface* uas = UASManager::instance()->getUASForId(sysid); +// +// // Check and (if necessary) create UAS object +// if (uas == NULL) +// { +// ArduPilotMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists +// // Connect this robot to the UAS object +// // it is IMPORTANT here to use the right object type, +// // else the slot of the parent object is called (and thus the special +// // packets never reach their goal) +// connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); +// uas = mav; +// // Make UAS aware that this link can be used to communicate with the actual robot +// uas->addLink(link); +// // Now add UAS to "official" list, which makes the whole application aware of it +// UASManager::instance()->addUAS(uas); +// } +// +// // The packet is emitted as a whole, as it is only 255 - 261 bytes short +// // kind of inefficient, but no issue for a groundstation pc. +// // It buys as reentrancy for the whole code over all threads +// emit messageReceived(link, msg); +// } +// } +// receiveMutex.unlock(); } /** diff --git a/src/comm/MAVLinkLightProtocol.h b/src/comm/MAVLinkLightProtocol.h index 22cb46c1aa94a0959f9ef224fbdb3956efd1dbcb..60253fbbd201d5c4e7856c1c2e51989644b1c21a 100644 --- a/src/comm/MAVLinkLightProtocol.h +++ b/src/comm/MAVLinkLightProtocol.h @@ -31,6 +31,7 @@ This file is part of the QGROUNDCONTROL project #define MAVLINKLIGHTPROTOCOL_H #include +#include #include #define MAVLINK_LIGHT_MAX_PAYLOAD_LEN 50 @@ -59,7 +60,7 @@ signals: public slots: void sendMessage(mavlink_light_message_t message); void sendMessage(LinkInterface* link, mavlink_light_message_t message); - void receiveBytes(LinkInterface* link); + void receiveBytes(LinkInterface* link, QByteArray b); }; diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index d23f6c6fa95c9865fc08bb910e5efd67840579aa..dff77e00143a05cc57329af5a14c4e7273a7c95a 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -108,21 +108,14 @@ QString MAVLinkProtocol::getLogfileName() * @param link The interface to read from * @see LinkInterface **/ -void MAVLinkProtocol::receiveBytes(LinkInterface* link) +void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { receiveMutex.lock(); - // Prepare buffer - static const int maxlen = 4096 * 100; - static char buffer[maxlen]; - qint64 bytesToRead = link->bytesAvailable(); - - // Get all data at once, let link read the bytes in the buffer array - link->readBytes(buffer, maxlen); mavlink_message_t message; mavlink_status_t status; - for (int position = 0; position < bytesToRead; position++) + for (int position = 0; position < b.size(); position++) { - unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)*(buffer + position), &message, &status); + unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status); if (decodeState == 1) { diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 82cf9b1132d25b8e2b0c218e932725603bd4e894..570efbf05cfcadf36b6c087462a6f01de285a5e6 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -73,7 +73,7 @@ public: public slots: /** @brief Receive bytes from a communication interface */ - void receiveBytes(LinkInterface* link); + void receiveBytes(LinkInterface* link, QByteArray b); /** @brief Send MAVLink message through serial interface */ void sendMessage(mavlink_message_t message); /** @brief Send MAVLink message through serial interface */ diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 40c15606b17fa4898377657ed642918810cad827..7f253e6b4737104a0bc7e7142c89cd72ad68a167 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -142,13 +142,7 @@ void MAVLinkSimulationLink::run() // } // readyBufferMutex.unlock(); - - - - - - - emit bytesReady(this); + readBytes(); } last = MG::TIME::getGroundTimeNow(); } @@ -752,9 +746,11 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } -void MAVLinkSimulationLink::readBytes(char* const data, qint64 maxLength) { +void MAVLinkSimulationLink::readBytes() { // Lock concurrent resource readyBuffer readyBufferMutex.lock(); + const qint64 maxLength = 2048; + char data[maxLength]; qint64 len = maxLength; if (maxLength > readyBuffer.size()) len = readyBuffer.size(); diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 5ff98d0110f059c9795365c7e81882b3ec87ea1c..ef223aa40cd1cac39862f028953286d50bc046c2 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -84,7 +84,7 @@ public: public slots: void writeBytes(const char* data, qint64 size); - void readBytes(char* const data, qint64 maxLength); + void readBytes(); void mainloop(); bool connectLink(bool connect); diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc new file mode 100644 index 0000000000000000000000000000000000000000..72bfbe47253db72aada34a24e08d8384f4a2b78b --- /dev/null +++ b/src/comm/OpalLink.cc @@ -0,0 +1,270 @@ +#include "OpalLink.h" + +OpalLink::OpalLink() : + connectState(false), + heartbeatTimer(new QTimer(this)), + heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), + m_heartbeatsEnabled(true), + getSignalsTimer(new QTimer(this)), + getSignalsPeriod(1000), + receiveBuffer(new QQueue()), + systemID(1), + componentID(1) +{ + start(QThread::LowPriority); + + // Set unique ID and add link to the list of links + this->id = getNextLinkId(); + this->name = tr("OpalRT link ") + QString::number(getId()); + LinkManager::instance()->add(this); + + // Start heartbeat timer, emitting a heartbeat at the configured rate + QObject::connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(heartbeat())); + + QObject::connect(getSignalsTimer, SIGNAL(timeout()), this, SLOT(getSignals())); +} + + +/* + * + Communication + * + */ + +qint64 OpalLink::bytesAvailable() +{ + return 0; +} + +void OpalLink::writeBytes(const char *bytes, qint64 length) +{ + +} + + +void OpalLink::readBytes() +{ + receiveDataMutex.lock(); + const qint64 maxLength = 2048; + char bytes[maxLength]; + qDebug() << "OpalLink::readBytes(): Reading a message. size of buffer: " << receiveBuffer->count(); + QByteArray message = receiveBuffer->dequeue(); + if (maxLength < message.size()) + { + qDebug() << "OpalLink::readBytes:: Buffer Overflow"; + + memcpy(bytes, message.data(), maxLength); + } + else + { + memcpy(bytes, message.data(), message.size()); + } + + emit bytesReceived(this, message); + receiveDataMutex.unlock(); + +} + +void OpalLink::receiveMessage(mavlink_message_t message) +{ + + // Create buffer + char buffer[MAVLINK_MAX_PACKET_LEN]; + // Write message into buffer, prepending start sign + int len = mavlink_msg_to_send_buffer((uint8_t*)(buffer), &message); + // If link is connected + if (isConnected()) + { + receiveBuffer->enqueue(QByteArray(buffer, len)); + readBytes(); + } + +} + +void OpalLink::heartbeat() +{ + + if (m_heartbeatsEnabled) + { + qDebug() << "OpalLink::heartbeat(): Generate a heartbeat"; + mavlink_message_t beat; + mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); + receiveMessage(beat); + } + +} + +void OpalLink::getSignals() +{ +// qDebug() << "OpalLink::getSignals(): Attempting to acquire signals"; +// +// +// unsigned long timeout = 0; +// unsigned short acqGroup = 0; //this is actually group 1 in the model +// unsigned short allocatedSignals = NUM_OUTPUT_SIGNALS; +// unsigned short *numSignals = new unsigned short(0); +// double *timestep = new double(0); +// double values[NUM_OUTPUT_SIGNALS] = {}; +// unsigned short *lastValues = new unsigned short(false); +// unsigned short *decimation = new unsigned short(0); +// +// int returnVal = OpalGetSignals(timeout, acqGroup, allocatedSignals, numSignals, timestep, +// values, lastValues, decimation); +// +// if (returnVal == EOK ) +// { +// qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues); +// } +// else if (returnVal == EAGAIN) +// { +// qDebug() << "OpalLink::getSignals: Data was not ready"; +// } +// // if returnVal == EAGAIN => data just wasn't ready +// else if (returnVal != EAGAIN) +// { +// getSignalsTimer->stop(); +// displayErrorMsg(); +// } +// /* deallocate used memory */ +// +// delete timestep; +// delete lastValues; +// delete lastValues; +// delete decimation; + +} + +/* + * + Administrative + * + */ +void OpalLink::run() +{ + qDebug() << "OpalLink::run():: Starting the thread"; +} + +int OpalLink::getId() +{ + return id; +} + +QString OpalLink::getName() +{ + return name; +} + +void OpalLink::setName(QString name) +{ + this->name = name; + emit nameChanged(this->name); +} + +bool OpalLink::isConnected() { + //qDebug() << "OpalLink::isConnected:: connectState: " << connectState; + return connectState; +} + + + + +bool OpalLink::connect() +{ + short modelState; + + /// \todo allow configuration of instid in window +// if (OpalConnect(101, false, &modelState) == EOK) +// { + connectState = true; + emit connected(); + heartbeatTimer->start(1000/heartbeatRate); + getSignalsTimer->start(getSignalsPeriod); +// } +// else +// { +// connectState = false; +// displayErrorMsg(); +// } + + emit connected(connectState); + return connectState; +} + +bool OpalLink::disconnect() +{ + return false; +} + +void OpalLink::displayErrorMsg() +{ + setLastErrorMsg(); + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText(lastErrorMsg); + msgBox.exec(); +} + +void OpalLink::setLastErrorMsg() +{ +// char buf[512]; +// unsigned short len; +// OpalGetLastErrMsg(buf, sizeof(buf), &len); +// lastErrorMsg.clear(); +// lastErrorMsg.append(buf); +} + + +/* + * + Statisctics + * + */ + +qint64 OpalLink::getNominalDataRate() +{ + return 0; //unknown +} + +int OpalLink::getLinkQuality() +{ + return -1; //not supported +} + +qint64 OpalLink::getTotalUpstream() +{ + statisticsMutex.lock(); + qint64 totalUpstream = bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); + statisticsMutex.unlock(); + return totalUpstream; +} + +qint64 OpalLink::getTotalDownstream() { + statisticsMutex.lock(); + qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); + statisticsMutex.unlock(); + return totalDownstream; +} + +qint64 OpalLink::getCurrentUpstream() +{ + return 0; //unknown +} + +qint64 OpalLink::getMaxUpstream() +{ + return 0; //unknown +} + +qint64 OpalLink::getBitsSent() { + return bitsSentTotal; +} + +qint64 OpalLink::getBitsReceived() { + return bitsReceivedTotal; +} + + +bool OpalLink::isFullDuplex() +{ + return false; +} diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h new file mode 100644 index 0000000000000000000000000000000000000000..3cd033cb7eb29dabd371c33f1129c17353c3cb9c --- /dev/null +++ b/src/comm/OpalLink.h @@ -0,0 +1,136 @@ +#ifndef OPALLINK_H +#define OPALLINK_H +/** + Connection to OpalRT. This class receives MAVLink packets as if it is a true link, but it + interprets the packets internally, and calls the appropriate api functions. + + \author Bryan Godbolt +*/ + +#include +#include +#include +#include +#include +#include +#include + +#include "LinkInterface.h" +#include "LinkManager.h" +#include "MG.h" +#include "mavlink.h" +#include "mavlink_types.h" +#include "configuration.h" + +#include "errno.h" + + + + +// FIXME +//#include "OpalApi.h" + + + + + +#include "string.h" + +/* + Configuration info for the model + */ + +#define NUM_OUTPUT_SIGNALS 6 + +class OpalLink : public LinkInterface +{ + Q_OBJECT + +public: + OpalLink(); + /* Connection management */ + + int getId(); + QString getName(); + bool isConnected(); + + /* Connection characteristics */ + + + qint64 getNominalDataRate(); + bool isFullDuplex(); + int getLinkQuality(); + qint64 getTotalUpstream(); + qint64 getTotalDownstream(); + qint64 getCurrentUpstream(); + qint64 getMaxUpstream(); + qint64 getBitsSent(); + qint64 getBitsReceived(); + + + bool connect(); + + + bool disconnect(); + + + qint64 bytesAvailable(); + + void run(); + +public slots: + + + void writeBytes(const char *bytes, qint64 length); + + + void readBytes(); + + void heartbeat(); + + void getSignals(); + +protected slots: + + void receiveMessage(mavlink_message_t message); + + + +protected: + QString name; + int id; + bool connectState; + + quint64 bitsSentTotal; + quint64 bitsSentCurrent; + quint64 bitsSentMax; + quint64 bitsReceivedTotal; + quint64 bitsReceivedCurrent; + quint64 bitsReceivedMax; + quint64 connectionStartTime; + + QMutex statisticsMutex; + QMutex receiveDataMutex; + QString lastErrorMsg; + void setLastErrorMsg(); + void displayErrorMsg(); + + void setName(QString name); + + QTimer* heartbeatTimer; ///< Timer to emit heartbeats + int heartbeatRate; ///< Heartbeat rate, controls the timer interval + bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission + + QTimer* getSignalsTimer; + int getSignalsPeriod; + + QQueue* receiveBuffer; + QByteArray* sendBuffer; + + const int systemID; + const int componentID; + + +}; + +#endif // OPALLINK_H diff --git a/src/comm/ProtocolInterface.h b/src/comm/ProtocolInterface.h index fc8467501ce47220e9372522dd87a4fc41b23238..10d2f23791085084a5b945a127420c54be3705d1 100644 --- a/src/comm/ProtocolInterface.h +++ b/src/comm/ProtocolInterface.h @@ -34,6 +34,7 @@ This file is part of the PIXHAWK project #include #include +#include #include "LinkInterface.h" /** @@ -53,7 +54,7 @@ public: virtual QString getName() = 0; public slots: - virtual void receiveBytes(LinkInterface *link) = 0; + virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0; signals: /** @brief Update the packet loss from one system */ diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 55751cc2820372a333f8cc30bdb5a07a09ce6911..5e949212e70594194119520c3706fe257fa05e2a 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -114,13 +114,14 @@ SerialLink::~SerialLink() * @brief Runs the thread * **/ -void SerialLink::run() { - +void SerialLink::run() +{ // Initialize the connection hardwareConnect(); // Qt way to make clear what a while(1) loop does - forever { + forever + { // Check if new bytes have arrived, if yes, emit the notification signal checkForBytes(); /* Serial data isn't arriving that fast normally, this saves the thread @@ -128,30 +129,33 @@ void SerialLink::run() { */ MG::SLEEP::msleep(SerialLink::poll_interval); } - } -void SerialLink::checkForBytes() { +void SerialLink::checkForBytes() +{ /* Check if bytes are available */ - if(port->isOpen()) { + if(port->isOpen()) + { dataMutex.lock(); qint64 available = port->bytesAvailable(); dataMutex.unlock(); - if(available > 0) { - emit bytesReady(this); - //qDebug() << "Bytes available" << available << "connected:" << port->isOpen(); + if(available > 0) + { + readBytes(); } - } else { + } + else + { emit disconnected(); } } -void SerialLink::writeBytes(const char* data, qint64 size) { - +void SerialLink::writeBytes(const char* data, qint64 size) +{ if(port->isOpen()) { int b = port->write(data, size); @@ -176,17 +180,20 @@ void SerialLink::writeBytes(const char* data, qint64 size) { * @param data Pointer to the data byte array to write the bytes to * @param maxLength The maximum number of bytes to write **/ -void SerialLink::readBytes(char* data, qint64 maxLength) { +void SerialLink::readBytes() +{ dataMutex.lock(); - if(port->isOpen()) { + if(port->isOpen()) + { + const qint64 maxLength = 2048; + char data[maxLength]; qint64 numBytes = port->bytesAvailable(); - if(numBytes > 0) { + if(numBytes > 0) + { /* Read as much data in buffer as possible without overflow */ if(maxLength < numBytes) numBytes = maxLength; port->read(data, numBytes); - - // FIXME TODO Check if this method should be preferred over manual read by process QByteArray b(data, numBytes); emit bytesReceived(this, b); @@ -210,23 +217,18 @@ void SerialLink::readBytes(char* data, qint64 maxLength) { * * @return The number of bytes to read **/ -qint64 SerialLink::bytesAvailable() { +qint64 SerialLink::bytesAvailable() +{ return port->bytesAvailable(); } -/** - * @brief Convenience method to emit the bytesReady signal - **/ -void SerialLink::emitBytesReady() { - emit bytesReady(this); -} - /** * @brief Disconnect the connection. * * @return True if connection has been disconnected, false if connection couldn't be disconnected. **/ -bool SerialLink::disconnect() { +bool SerialLink::disconnect() +{ //#if !defined _WIN32 || !defined _WIN64 /* Block the thread until it returns from run() */ //#endif @@ -276,10 +278,8 @@ bool SerialLink::connect() * @return True if the connection could be established, false otherwise * @see connect() For the right function to establish the connection. **/ -bool SerialLink::hardwareConnect() { - - qDebug() << "Opening serial port" << porthandle; - +bool SerialLink::hardwareConnect() +{ QObject::connect(port, SIGNAL(aboutToClose()), this, SIGNAL(disconnected())); port->open(QIODevice::ReadWrite); @@ -300,12 +300,15 @@ bool SerialLink::hardwareConnect() { return connectionUp; } + + /** * @brief Check if connection is active. * * @return True if link is connected, false otherwise. **/ -bool SerialLink::isConnected() { +bool SerialLink::isConnected() +{ return port->isOpen(); } @@ -326,7 +329,8 @@ void SerialLink::setName(QString name) } -qint64 SerialLink::getNominalDataRate() { +qint64 SerialLink::getNominalDataRate() +{ qint64 dataRate = 0; switch (baudrate) { case BAUD50: @@ -399,77 +403,94 @@ qint64 SerialLink::getNominalDataRate() { return dataRate; } -qint64 SerialLink::getTotalUpstream() { +qint64 SerialLink::getTotalUpstream() +{ statisticsMutex.lock(); return bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); statisticsMutex.unlock(); } -qint64 SerialLink::getCurrentUpstream() { +qint64 SerialLink::getCurrentUpstream() +{ return 0; // TODO } -qint64 SerialLink::getMaxUpstream() { +qint64 SerialLink::getMaxUpstream() +{ return 0; // TODO } -qint64 SerialLink::getBitsSent() { +qint64 SerialLink::getBitsSent() +{ return bitsSentTotal; } -qint64 SerialLink::getBitsReceived() { +qint64 SerialLink::getBitsReceived() +{ return bitsReceivedTotal; } -qint64 SerialLink::getTotalDownstream() { +qint64 SerialLink::getTotalDownstream() +{ statisticsMutex.lock(); return bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); statisticsMutex.unlock(); } -qint64 SerialLink::getCurrentDownstream() { +qint64 SerialLink::getCurrentDownstream() +{ return 0; // TODO } -qint64 SerialLink::getMaxDownstream() { +qint64 SerialLink::getMaxDownstream() +{ return 0; // TODO } -bool SerialLink::isFullDuplex() { +bool SerialLink::isFullDuplex() +{ /* Serial connections are always half duplex */ return false; } -int SerialLink::getLinkQuality() { +int SerialLink::getLinkQuality() +{ /* This feature is not supported with this interface */ return -1; } -QString SerialLink::getPortName() { +QString SerialLink::getPortName() +{ return porthandle; } -int SerialLink::getBaudRate() { +int SerialLink::getBaudRate() +{ return getNominalDataRate(); } -int SerialLink::getBaudRateType() { +int SerialLink::getBaudRateType() +{ return baudrate; } -int SerialLink::getFlowType() { +int SerialLink::getFlowType() +{ return flow; } -int SerialLink::getParityType() { +int SerialLink::getParityType() +{ return parity; } -int SerialLink::getDataBitsType() { +int SerialLink::getDataBitsType() +{ return dataBits; } -int SerialLink::getStopBitsType() { +int SerialLink::getStopBitsType() +{ return stopBits; } @@ -689,7 +710,8 @@ bool SerialLink::setBaudRate(int rate) return accepted; } -bool SerialLink::setFlowType(int flow) { +bool SerialLink::setFlowType(int flow) +{ bool reconnect = false; bool accepted = true; if(isConnected()) { @@ -717,7 +739,8 @@ bool SerialLink::setFlowType(int flow) { return accepted; } -bool SerialLink::setParityType(int parity) { +bool SerialLink::setParityType(int parity) +{ bool reconnect = false; bool accepted = true; if(isConnected()) { @@ -752,7 +775,8 @@ bool SerialLink::setParityType(int parity) { return accepted; } -bool SerialLink::setDataBitsType(int dataBits) { +bool SerialLink::setDataBitsType(int dataBits) +{ bool accepted = true; switch (dataBits) { @@ -783,7 +807,8 @@ bool SerialLink::setDataBitsType(int dataBits) { return accepted; } -bool SerialLink::setStopBitsType(int stopBits) { +bool SerialLink::setStopBitsType(int stopBits) +{ bool reconnect = false; bool accepted = true; if(isConnected()) { diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index e80cbad0769d09686251ce59ecb5cf2aec1cc777..09837e1b22a3d9989e81ebcb910dac16f62df112 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -106,7 +106,7 @@ public slots: bool setDataBitsType(int dataBits); bool setStopBitsType(int stopBits); - void readBytes(char* data, qint64 maxLength); + void readBytes(); /** * @brief Write a number of bytes to the interface. * @@ -118,7 +118,6 @@ public slots: bool disconnect(); protected slots: - void emitBytesReady(); void checkForBytes(); protected: diff --git a/src/comm/SerialSimulationLink.cc b/src/comm/SerialSimulationLink.cc index 55389c783b2c7ed91690219abadec77788c07ff3..7a9a929a3477e0549b05ebd737ba65b62478335b 100644 --- a/src/comm/SerialSimulationLink.cc +++ b/src/comm/SerialSimulationLink.cc @@ -1,26 +1,26 @@ /*===================================================================== - + PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - + (c) 2009 PIXHAWK PROJECT - + This file is part of the PIXHAWK project - + PIXHAWK is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - + PIXHAWK is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with PIXHAWK. If not, see . - + ======================================================================*/ - + /** * @file * @brief Brief Description @@ -49,8 +49,8 @@ This file is part of the PIXHAWK project * @param writeFile The received messages are written to that file * @param sendrate The rate at which the messages are sent (in intervals of milliseconds) **/ -SerialSimulationLink::SerialSimulationLink(QFile* readFile, QFile* writeFile, int sendrate) { - +SerialSimulationLink::SerialSimulationLink(QFile* readFile, QFile* writeFile, int sendrate) +{ // If a non-empty portname is supplied, the serial simulation link should attempt loopback simulation loopBack = NULL; @@ -72,16 +72,17 @@ SerialSimulationLink::SerialSimulationLink(QFile* readFile, QFile* writeFile, in QObject::connect(timer, SIGNAL(timeout()), this, SLOT(readLine())); _isConnected = false; rate = sendrate; - } -SerialSimulationLink::~SerialSimulationLink() { +SerialSimulationLink::~SerialSimulationLink() +{ //TODO Check destructor fileStream->flush(); outStream->flush(); } -void SerialSimulationLink::run() { +void SerialSimulationLink::run() +{ /* forever { quint64 currentTime = OG::TIME::getGroundTimeNow(); @@ -94,15 +95,16 @@ void SerialSimulationLink::run() { }*/ } -void SerialSimulationLink::enableLoopBackMode(SerialLink* loop) { - +void SerialSimulationLink::enableLoopBackMode(SerialLink* loop) +{ // Lock the data readyBufferMutex.lock(); // Disconnect this link disconnect(); // Delete previous loopback link if exists - if(loopBack != NULL) { + if(loopBack != NULL) + { delete loopBack; loopBack = NULL; } @@ -119,12 +121,16 @@ void SerialSimulationLink::enableLoopBackMode(SerialLink* loop) { } -qint64 SerialSimulationLink::bytesAvailable() { +qint64 SerialSimulationLink::bytesAvailable() +{ readyBufferMutex.lock(); qint64 size = 0; - if(loopBack == 0) { + if(loopBack == 0) + { size = readyBuffer.size(); - } else { + } + else + { size = loopBack->bytesAvailable(); } readyBufferMutex.unlock(); @@ -132,9 +138,11 @@ qint64 SerialSimulationLink::bytesAvailable() { return size; } -void SerialSimulationLink::writeBytes(char* data, qint64 length) { +void SerialSimulationLink::writeBytes(char* data, qint64 length) +{ /* Write bytes to one line */ - for(qint64 i = 0; i < length; i++) { + for(qint64 i = 0; i < length; i++) + { outStream->operator <<(data[i]); outStream->flush(); } @@ -142,15 +150,22 @@ void SerialSimulationLink::writeBytes(char* data, qint64 length) { } -void SerialSimulationLink::readBytes(char* const data, qint64 maxLength) { +void SerialSimulationLink::readBytes() +{ + const qint64 maxLength = 2048; + char data[maxLength]; /* Lock concurrent resource readyBuffer */ readyBufferMutex.lock(); - if(loopBack == NULL) { + if(loopBack == NULL) + { + // FIXME Maxlength has no meaning here /* copy leftmost maxLength bytes and remove them from buffer */ qstrncpy(data, readyBuffer.left(maxLength).data(), maxLength); readyBuffer.remove(0, maxLength); - } else { - loopBack->readBytes(data, maxLength); + } + else + { + //loopBack->readBytes(data, maxLength); } readyBufferMutex.unlock(); } @@ -166,9 +181,11 @@ void SerialSimulationLink::readBytes(char* const data, qint64 maxLength) { * @bug The time noise addition is commented out because it adds some delay * which leads to a drift in the timer. This can be fixed by multithreading. **/ -void SerialSimulationLink::readLine() { +void SerialSimulationLink::readLine() +{ - if(_isConnected) { + if(_isConnected) + { /* The order of operations in this method is arranged to * minimize the impact of slow file read operations on the * message emit timing. The functions should be kept in this order @@ -179,16 +196,20 @@ void SerialSimulationLink::readLine() { /* (2) Save content of line buffer in readyBuffer (has to be lock for thread safety)*/ readyBufferMutex.lock(); - if(loopBack == NULL) { + if(loopBack == NULL) + { readyBuffer.append(lineBuffer); //qDebug() << "readLine readyBuffer: " << readyBuffer; - } else { + } + else + { loopBack->writeBytes(lineBuffer.data(), lineBuffer.size()); } readyBufferMutex.unlock(); - if(loopBack == NULL) { - emit bytesReady(this); + if(loopBack == NULL) + { + readBytes(); } /* (4) Read one line and save it in line buffer */ @@ -224,7 +245,8 @@ void SerialSimulationLink::readLine() { * @bug The current implementation might induce one milliseconds additional * discrepancy, this will be fixed by multithreading **/ -void SerialSimulationLink::setMaximumTimeNoise(int milliseconds) { +void SerialSimulationLink::setMaximumTimeNoise(int milliseconds) +{ maxTimeNoise = milliseconds; } @@ -235,7 +257,8 @@ void SerialSimulationLink::setMaximumTimeNoise(int milliseconds) { * * @see setMaximumTimeNoise() **/ -void SerialSimulationLink::addTimeNoise() { +void SerialSimulationLink::addTimeNoise() +{ /* Calculate the time deviation */ if(maxTimeNoise == 0) { /* Don't do expensive calculations if no noise is desired */ @@ -294,7 +317,8 @@ bool SerialSimulationLink::disconnect() { * @return True if connection has been established, false if connection * couldn't be established. **/ -bool SerialSimulationLink::connect() { +bool SerialSimulationLink::connect() +{ /* Open files */ //@TODO Add check if file can be read simulationFile->open(QIODevice::ReadOnly); @@ -328,126 +352,154 @@ bool SerialSimulationLink::connect() { * * @return True if link is connected, false otherwise. **/ -bool SerialSimulationLink::isConnected() { +bool SerialSimulationLink::isConnected() +{ return _isConnected; } -qint64 SerialSimulationLink::getNominalDataRate() { +qint64 SerialSimulationLink::getNominalDataRate() +{ /* 100 Mbit is reasonable fast and sufficient for all embedded applications */ return 100000000; } -qint64 SerialSimulationLink::getTotalUpstream() { +qint64 SerialSimulationLink::getTotalUpstream() +{ return 0; //TODO Add functionality here // @todo Add functionality here } -qint64 SerialSimulationLink::getShortTermUpstream() { +qint64 SerialSimulationLink::getShortTermUpstream() +{ return 0; } -qint64 SerialSimulationLink::getCurrentUpstream() { +qint64 SerialSimulationLink::getCurrentUpstream() +{ return 0; } -qint64 SerialSimulationLink::getMaxUpstream() { +qint64 SerialSimulationLink::getMaxUpstream() +{ return 0; } -qint64 SerialSimulationLink::getBitsSent() { +qint64 SerialSimulationLink::getBitsSent() +{ return 0; } -qint64 SerialSimulationLink::getBitsReceived() { +qint64 SerialSimulationLink::getBitsReceived() +{ return 0; } -qint64 SerialSimulationLink::getTotalDownstream() { +qint64 SerialSimulationLink::getTotalDownstream() +{ return 0; } -qint64 SerialSimulationLink::getShortTermDownstream() { +qint64 SerialSimulationLink::getShortTermDownstream() +{ return 0; } -qint64 SerialSimulationLink::getCurrentDownstream() { +qint64 SerialSimulationLink::getCurrentDownstream() +{ return 0; } -qint64 SerialSimulationLink::getMaxDownstream() { +qint64 SerialSimulationLink::getMaxDownstream() +{ return 0; } -bool SerialSimulationLink::isFullDuplex() { +bool SerialSimulationLink::isFullDuplex() +{ /* Full duplex is no problem when running in pure software, but this is a serial simulation */ return false; } -int SerialSimulationLink::getLinkQuality() { +int SerialSimulationLink::getLinkQuality() +{ /* The Link quality is always perfect when running in software */ return 100; } -bool SerialSimulationLink::setPortName(QString portName) { - (void)portName; // Cast to void prevents the unused parameter warning +bool SerialSimulationLink::setPortName(QString portName) +{ + Q_UNUSED(portName); return true; } -bool SerialSimulationLink::setBaudRate(int rate) { - (void)rate; // Cast to void prevents the unused parameter warning +bool SerialSimulationLink::setBaudRate(int rate) +{ + Q_UNUSED(rate); return true; } -bool SerialSimulationLink::setFlowType(int type) { - (void)type; // Cast to void prevents the unused parameter warning +bool SerialSimulationLink::setFlowType(int type) +{ + Q_UNUSED(type); return true; } -bool SerialSimulationLink::setParityType(int type) { - (void)type; // Cast to void prevents the unused parameter warning +bool SerialSimulationLink::setParityType(int type) +{ + Q_UNUSED(type); return true; } -bool SerialSimulationLink::setDataBitsType(int type) { - (void)type; // Cast to void prevents the unused parameter warning +bool SerialSimulationLink::setDataBitsType(int type) +{ + Q_UNUSED(type); return true; } -bool SerialSimulationLink::setStopBitsType(int type) { - (void)type; // Cast to void prevents the unused parameter warning +bool SerialSimulationLink::setStopBitsType(int type) +{ + Q_UNUSED(type) return true; } -QString SerialSimulationLink::getPortName() { +QString SerialSimulationLink::getPortName() +{ return tr("simulated/port"); } -int SerialSimulationLink::getBaudRate() { +int SerialSimulationLink::getBaudRate() +{ return 115200; } -int SerialSimulationLink::getBaudRateType() { +int SerialSimulationLink::getBaudRateType() +{ return 19; } -int SerialSimulationLink::getFlowType() { +int SerialSimulationLink::getFlowType() +{ return 0; } -int SerialSimulationLink::getParityType() { +int SerialSimulationLink::getParityType() +{ return 0; } -int SerialSimulationLink::getDataBitsType() { +int SerialSimulationLink::getDataBitsType() +{ return 8; } -int SerialSimulationLink::getStopBitsType() { +int SerialSimulationLink::getStopBitsType() +{ return 2; } -bool SerialSimulationLink::setBaudRateType(int rateIndex) { - (void)rateIndex; // Cast to void prevents the unused parameter warning +bool SerialSimulationLink::setBaudRateType(int rateIndex) +{ + Q_UNUSED(rateIndex); return true; } diff --git a/src/comm/SerialSimulationLink.h b/src/comm/SerialSimulationLink.h index 7df321393379a0d5cb6821371a5822eb651b949c..4a95a47571e54b8d4fbf7f58a0e02c980a8566c8 100644 --- a/src/comm/SerialSimulationLink.h +++ b/src/comm/SerialSimulationLink.h @@ -95,7 +95,7 @@ public slots: void readLine(); void writeBytes(char *bytes, qint64 length); - void readBytes(char *bytes, qint64 maxLength); + void readBytes(); bool connect(); bool disconnect(); diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 61c1ed5170f53465baa46da113cc32e843b0a5e2..048bacdf5e4f51af9beeac9f2d6a944d01eb0370 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -120,8 +120,10 @@ void UDPLink::writeBytes(const char* data, qint64 size) * @param data Pointer to the data byte array to write the bytes to * @param maxLength The maximum number of bytes to write **/ -void UDPLink::readBytes(char* data, qint64 maxLength) +void UDPLink::readBytes() { + const qint64 maxLength = 2048; + char data[maxLength]; QHostAddress sender; quint16 senderPort; @@ -169,14 +171,6 @@ qint64 UDPLink::bytesAvailable() { return socket->pendingDatagramSize(); } -/** - * @brief Convenience method to emit the bytesReady signal - **/ -void UDPLink::emitBytesReady() -{ - emit bytesReady(this); -} - /** * @brief Disconnect the connection. * @@ -204,7 +198,7 @@ bool UDPLink::connect() socket = new QUdpSocket(this); //QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams())); - QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(emitBytesReady())); + QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); connectState = socket->bind(host, port); diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index 6bde904c33e7300d2502261d55899a87efd5171a..704020e889d3662658e52ab4206ff5523d86c39d 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -48,8 +48,6 @@ public: UDPLink(QHostAddress host = QHostAddress::Any, quint16 port = 14550); ~UDPLink(); - static const int poll_interval = UDP_POLL_INTERVAL; ///< Polling interval, defined in configuration.h - bool isConnected(); qint64 bytesAvailable(); @@ -86,7 +84,7 @@ public slots: void setPort(quint16 port); // void readPendingDatagrams(); - void readBytes(char* data, qint64 maxLength); + void readBytes(); /** * @brief Write a number of bytes to the interface. * @@ -97,10 +95,6 @@ public slots: bool connect(); bool disconnect(); -protected slots: - void emitBytesReady(); - //void checkForBytes(); - protected: QString name; QHostAddress host; diff --git a/src/configuration.h b/src/configuration.h index 6fe339966d613437572a21380a15477e26bf4dd6..16650c1e009f189bc4a2c18038ccf67d74ffc50e 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -2,9 +2,7 @@ #define CONFIGURATION_H /** @brief Polling interval in ms */ -#define SERIAL_POLL_INTERVAL 1 -/** @brief UDP polling interval in ms */ -#define UDP_POLL_INTERVAL 1 +#define SERIAL_POLL_INTERVAL 2 /** @brief Heartbeat emission rate, in Hertz (times per second) */ #define MAVLINK_HEARTBEAT_DEFAULT_RATE 1 @@ -12,6 +10,6 @@ #define WITH_TEXT_TO_SPEECH 1 #define QGC_APPLICATION_NAME "QGroundControl" -#define QGC_APPLICATION_VERSION "v. 0.7.6 (Beta)" +#define QGC_APPLICATION_VERSION "v. 0.7.7 (Beta)" #endif // CONFIGURATION_H diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 9e5ea343b12d1e60d45687dad4918d0ff570e5b3..e7ccdee42972add49067608adec90ae2fe2ff3b2 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -386,6 +386,16 @@ void UASWaypointManager::localLoadWaypoints(const QString &loadFile) emit waypointListChanged(); } +void UASWaypointManager::globalAddWaypoint(Waypoint *wp) +{ + +} + +int UASWaypointManager::globalRemoveWaypoint(quint16 seq) +{ + return 0; +} + void UASWaypointManager::clearWaypointList() { if(current_state == WP_IDLE) diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index e21d4cd6b068e2b48a50c0c57c381bb48dcdf704..e9dce5d6d6f7c66ef3ce3d87df0994a73e4531ef 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -93,6 +93,13 @@ public: void localLoadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile /*@}*/ + /** @name Global waypoint list operations */ + /*@{*/ + const QVector &getGlobalWaypointList(void) { return waypoints; } ///< Returns a const reference to the global waypoint list. + void globalAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly + int globalRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage + /*@}*/ + private: /** @name Message send functions */ /*@{*/ diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 1fecb1b6e87c4c395b75db50ab73918173f4cc1d..b48fe7262cd5dcf68d7ab306652ffc6e54e17cd2 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -39,8 +39,12 @@ This file is part of the PIXHAWK project #include "CommConfigurationWindow.h" #include "SerialConfigurationWindow.h" -#include "SerialLinkInterface.h" +#include "SerialLink.h" #include "UDPLink.h" +#include "MAVLinkSimulationLink.h" +#ifdef OPAL_RT +#include "OpalLink.h" +#endif #include "MAVLinkProtocol.h" #include "MAVLinkSettingsWidget.h" @@ -86,7 +90,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn // TODO Move these calls to each link so that dynamic casts vanish // Open details pane for serial link if necessary - SerialLinkInterface* serial = dynamic_cast(link); + SerialLink* serial = dynamic_cast(link); if(serial != 0) { QWidget* conf = new SerialConfigurationWindow(serial, this); @@ -97,11 +101,28 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn //ui.linkGroupBox->setTitle(link->getName()); //connect(link, SIGNAL(nameChanged(QString)), ui.linkGroupBox, SLOT(setTitle(QString))); } - else if (dynamic_cast(link) != 0) + UDPLink* udp = dynamic_cast(link); + if (udp != 0) { ui.linkGroupBox->setTitle(tr("UDP Link")); } - else + MAVLinkSimulationLink* sim = dynamic_cast(link); + if (sim != 0) + { + ui.linkGroupBox->setTitle(tr("MAVLink Simulation Link")); + } +#ifdef OPAL_RT + OpalLink* opal = dynamic_cast(link); + if (opal != 0) + { + ui.linkGroupBox->setTitle(tr("Opal-RT Link")); + } +#endif + if (serial == 0 && udp == 0 && sim == 0 +#ifdef OPAL_RT + && opal == 0 +#endif + ) { qDebug() << "Link is NOT a known link, can't open configuration window"; } diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h index 7d3c3d9020ce69754abd6eb9eb2d49867e91b96d..ab0e5100b3de9d209bbb23865d69e67cedaba02b 100644 --- a/src/ui/CommConfigurationWindow.h +++ b/src/ui/CommConfigurationWindow.h @@ -40,6 +40,10 @@ This file is part of the PIXHAWK project #include "ProtocolInterface.h" #include "ui_CommSettings.h" +#ifdef OPAL_RT +#include "OpalLink.h" +#endif + class CommConfigurationWindow : public QWidget { Q_OBJECT diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index dfd9864702385b9c0a39c8196b1e19fe4555ec33..eddc97f3e0ce2f230d4f46769421987eee346057 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -574,7 +574,7 @@ void HUD::paintHUD() if ((yawTrans < 5.0) && (yawTrans > -5.0)) yawTrans = 0; - qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw; + //qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw; // Update scaling factor // adjust scaling to fit both horizontally and vertically diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index e748dfb9493b2ac39c969ad4931bb7b58fb460ac..5a1f2cc2a4cbc926b7b9f1e034f9640d6da2c50d 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -374,9 +374,9 @@ void MainWindow::addLink() void MainWindow::addLink(LinkInterface *link) { + LinkManager::instance()->addProtocol(link, mavlink); CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); ui.menuNetwork->addAction(commWidget->getAction()); - LinkManager::instance()->addProtocol(link, mavlink); // Special case for simulationlink MAVLinkSimulationLink* sim = dynamic_cast(link); diff --git a/src/ui/QGCDataPlot2D.cc b/src/ui/QGCDataPlot2D.cc index 303d5a1f3da4c6e2b94470bfdd24781794e062fa..381c45b254ddbf0c720e4751879225c16f788562 100644 --- a/src/ui/QGCDataPlot2D.cc +++ b/src/ui/QGCDataPlot2D.cc @@ -116,7 +116,7 @@ void QGCDataPlot2D::print() QPrintDialog dialog(&printer); if ( dialog.exec() ) { - plot->setStyleSheet("QWidget { background-color: #FFFFFF; color: #000000; background-clip: border; font-size: 11pt;}"); + plot->setStyleSheet("QWidget { background-color: #FFFFFF; color: #000000; background-clip: border; font-size: 10pt;}"); plot->setCanvasBackground(Qt::white); QwtPlotPrintFilter filter; filter.color(Qt::white, QwtPlotPrintFilter::CanvasBackground); @@ -132,6 +132,8 @@ void QGCDataPlot2D::print() filter.setOptions(options); } plot->print(printer, filter); + plot->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; font-size: 11pt;}"); + //plot->setCanvasBackground(QColor(5, 5, 8)); } }