Merge pull request #1398 from DonLakeFlyer/MavlinkChannel

Remove link ids and add link mavlink channel

src/comm/LinkInterface.h

@@ -39,6 +39,8 @@ along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
 #include <QMetaType>
 #include <QSharedPointer>
 
+#include "QGCMAVLink.h"
+
 class LinkManager;
 class LinkConfiguration;
 
@@ -63,14 +65,6 @@ public:
 
     /* 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() const = 0;
-
     /**
      * @brief Get the human readable name of this link
      */
@@ -123,6 +117,10 @@ public:
     {
         return getCurrentDataRate(outDataIndex, outDataWriteTimes, outDataWriteAmounts);
     }
+    
+    /// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only
+    /// set into the link when it is added to LinkManager
+    uint8_t getMavlinkChannel(void) const { Q_ASSERT(_mavlinkChannelSet); return _mavlinkChannel; }
 
     // These are left unimplemented in order to cause linker errors which indicate incorrect usage of
     // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
@@ -146,7 +144,8 @@ public slots:
 protected:
     // Links are only created by LinkManager so constructor is not public
     LinkInterface() :
-        QThread(0)
+        QThread(0),
+        _mavlinkChannelSet(false)
     {
         // Initialize everything for the data rate calculation buffers.
         inDataIndex  = 0;
@@ -300,11 +299,6 @@ protected:
         return dataRate;
     }
 
-    static int getNextLinkId() {
-        static int nextId = 1;
-        return nextId++;
-    }
-
 protected slots:
 
     /**
@@ -329,6 +323,12 @@ private:
      * @return True if connection could be terminated, false otherwise
      **/
     virtual bool _disconnect(void) = 0;
+    
+    /// Sets the mavlink channel to use for this link
+    void _setMavlinkChannel(uint8_t channel) { Q_ASSERT(!_mavlinkChannelSet); _mavlinkChannelSet = true; _mavlinkChannel = channel; }
+    
+    bool _mavlinkChannelSet;    ///< true: _mavlinkChannel has been set
+    uint8_t _mavlinkChannel;    ///< mavlink channel to use for this link, as used by mavlink_parse_char
 };
 
 typedef QSharedPointer<LinkInterface> SharedLinkInterface;

src/comm/LinkManager.cc

@@ -52,6 +52,7 @@ LinkManager::LinkManager(QObject* parent)
     , _configUpdateSuspended(false)
     , _configurationsLoaded(false)
     , _connectionsSuspended(false)
+    , _mavlinkChannelsUsedBitMask(0)
     , _nullSharedLink(NULL)
 {
     connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateConfigurationList);
@@ -114,6 +115,16 @@ void LinkManager::_addLink(LinkInterface* link)
     _linkListMutex.lock();
 
     if (!containsLink(link)) {
+        // Find a mavlink channel to use for this link
+        for (int i=0; i<32; i++) {
+            if (!(_mavlinkChannelsUsedBitMask && 1 << i)) {
+                mavlink_reset_channel_status(i);
+                link->_setMavlinkChannel(i);
+                _mavlinkChannelsUsedBitMask |= i << i;
+                break;
+            }
+        }
+        
         _links.append(QSharedPointer<LinkInterface>(link));
         _linkListMutex.unlock();
         emit newLink(link);
@@ -203,6 +214,9 @@ void LinkManager::_deleteLink(LinkInterface* link)
     Q_ASSERT(link);
 
     _linkListMutex.lock();
+    
+    // Free up the mavlink channel associated with this link
+    _mavlinkChannelsUsedBitMask &= ~(1 << link->getMavlinkChannel());
 
     bool found = false;
     for (int i=0; i<_links.count(); i++) {

src/comm/LinkManager.h

@@ -173,6 +173,8 @@ private:
     QString _connectionsSuspendedReason;                ///< User visible reason for suspension
     QTimer  _portListTimer;
     
+    uint32_t _mavlinkChannelsUsedBitMask;
+    
     SharedLinkInterface _nullSharedLink;
 };
 

src/comm/MAVLinkProtocol.cc

@@ -145,12 +145,12 @@ void MAVLinkProtocol::storeSettings()
 
 void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link)
 {
-    int linkId = link->getId();
-    totalReceiveCounter[linkId] = 0;
-    totalLossCounter[linkId] = 0;
-    totalErrorCounter[linkId] = 0;
-    currReceiveCounter[linkId] = 0;
-    currLossCounter[linkId] = 0;
+    int channel = link->getMavlinkChannel();
+    totalReceiveCounter[channel] = 0;
+    totalLossCounter[channel] = 0;
+    totalErrorCounter[channel] = 0;
+    currReceiveCounter[channel] = 0;
+    currLossCounter[channel] = 0;
 }
 
 void MAVLinkProtocol::linkConnected(void)
@@ -227,8 +227,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
     mavlink_message_t message;
     mavlink_status_t status;
 
-    // Cache the link ID for common use.
-    int linkId = link->getId();
+    int mavlinkChannel = link->getMavlinkChannel();
 
     static int mavlink09Count = 0;
     static int nonmavlinkCount = 0;
@@ -237,9 +236,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
     static bool checkedUserNonMavlink = false;
     static bool warnedUserNonMavlink = false;
 
-    // FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS
     for (int position = 0; position < b.size(); position++) {
-        unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status);
+        unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(b[position]), &message, &status);
 
         if ((uint8_t)b[position] == 0x55) mavlink09Count++;
         if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
@@ -381,8 +379,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
             }
 
             // Increase receive counter
-            totalReceiveCounter[linkId]++;
-            currReceiveCounter[linkId]++;
+            totalReceiveCounter[mavlinkChannel]++;
+            currReceiveCounter[mavlinkChannel]++;
 
             // Determine what the next expected sequence number is, accounting for
             // never having seen a message for this system/component pair.
@@ -403,22 +401,22 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                 }
 
                 // And log how many were lost for all time and just this timestep
-                totalLossCounter[linkId] += lostMessages;
-                currLossCounter[linkId] += lostMessages;
+                totalLossCounter[mavlinkChannel] += lostMessages;
+                currLossCounter[mavlinkChannel] += lostMessages;
             }
 
             // And update the last sequence number for this system/component pair
             lastIndex[message.sysid][message.compid] = expectedSeq;
 
             // Update on every 32th packet
-            if ((totalReceiveCounter[linkId] & 0x1F) == 0)
+            if ((totalReceiveCounter[mavlinkChannel] & 0x1F) == 0)
             {
                 // Calculate new loss ratio
                 // Receive loss
-                float receiveLoss = (double)currLossCounter[linkId]/(double)(currReceiveCounter[linkId]+currLossCounter[linkId]);
+                float receiveLoss = (double)currLossCounter[mavlinkChannel]/(double)(currReceiveCounter[mavlinkChannel]+currLossCounter[mavlinkChannel]);
                 receiveLoss *= 100.0f;
-                currLossCounter[linkId] = 0;
-                currReceiveCounter[linkId] = 0;
+                currLossCounter[mavlinkChannel] = 0;
+                currReceiveCounter[mavlinkChannel] = 0;
                 emit receiveLossChanged(message.sysid, receiveLoss);
             }
 
@@ -497,7 +495,7 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message
     static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
     // Rewriting header to ensure correct link ID is set
     static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
-    if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len, messageKeys[message.msgid]);
+    mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
     // Write message into buffer, prepending start sign
     int len = mavlink_msg_to_send_buffer(buffer, &message);
     // If link is connected
@@ -520,7 +518,7 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message
     static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
     // Rewriting header to ensure correct link ID is set
     static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
-    if (link->getId() != 0) mavlink_finalize_message_chan(&message, systemid, componentid, link->getId(), message.len, messageKeys[message.msgid]);
+    mavlink_finalize_message_chan(&message, systemid, componentid, link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
     // Write message into buffer, prepending start sign
     int len = mavlink_msg_to_send_buffer(buffer, &message);
     // If link is connected

src/comm/MAVLinkProtocol.h

@@ -121,21 +121,21 @@ public:
      * @returns -1 if this is not available for this protocol, # of packets otherwise.
      */
     qint32 getReceivedPacketCount(const LinkInterface *link) const {
-        return totalReceiveCounter[link->getId()];
+        return totalReceiveCounter[link->getMavlinkChannel()];
     }
     /**
      * Retrieve a total of all parsing errors for the specified link.
      * @returns -1 if this is not available for this protocol, # of errors otherwise.
      */
     qint32 getParsingErrorCount(const LinkInterface *link) const {
-        return totalErrorCounter[link->getId()];
+        return totalErrorCounter[link->getMavlinkChannel()];
     }
     /**
      * Retrieve a total of all dropped packets for the specified link.
      * @returns -1 if this is not available for this protocol, # of packets otherwise.
      */
     qint32 getDroppedPacketCount(const LinkInterface *link) const {
-        return totalLossCounter[link->getId()];
+        return totalLossCounter[link->getMavlinkChannel()];
     }
     /**
      * Reset the counters for all metadata for this link.

src/comm/MAVLinkSimulationLink.cc

@@ -105,7 +105,6 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     // Initialize the pseudo-random number generator
     srand(QTime::currentTime().msec());
     maxTimeNoise = 0;
-    this->id = getNextLinkId();
     LinkManager::instance()->_addLink(this);
 }
 
@@ -595,7 +594,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
     // Output all bytes as hex digits
     for (int i=0; i<size; i++)
     {
-        if (mavlink_parse_char(this->id, data[i], &msg, &comm))
+        if (mavlink_parse_char(getMavlinkChannel(), data[i], &msg, &comm))
         {
             // MESSAGE RECEIVED!
 //            qDebug() << "SIMULATION LINK RECEIVED MESSAGE!";
@@ -839,11 +838,6 @@ bool MAVLinkSimulationLink::isConnected() const
     return _isConnected;
 }
 
-int MAVLinkSimulationLink::getId() const
-{
-    return id;
-}
-
 QString MAVLinkSimulationLink::getName() const
 {
     return name;

src/comm/MAVLinkSimulationLink.h

@@ -61,7 +61,6 @@ public:
     qint64 getCurrentOutDataRate() const;
 
     QString getName() const;
-    int getId() const;
     int getBaudRate() const;
     int getBaudRateType() const;
     int getFlowType() const;
@@ -114,7 +113,6 @@ protected:
     int readyBytes;
     QQueue<uint8_t> readyBuffer;
 
-    int id;
     QString name;
     qint64 timeOffset;
     mavlink_sys_status_t status;

src/comm/SerialLink.cc

@@ -34,8 +34,6 @@ SerialLink::SerialLink(SerialConfiguration* config)
     // We're doing it wrong - because the Qt folks got the API wrong:
     // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
     moveToThread(this);
-    // Set unique ID and add link to the list of links
-    _id = getNextLinkId();
 
     qCDebug(SerialLinkLog) << "Create SerialLink " << config->portName() << config->baud() << config->flowControl()
              << config->parity() << config->dataBits() << config->stopBits();
@@ -391,11 +389,6 @@ bool SerialLink::isConnected() const
     return isConnected;
 }
 
-int SerialLink::getId() const
-{
-    return _id;
-}
-
 QString SerialLink::getName() const
 {
     return _config->portName();

src/comm/SerialLink.h

@@ -110,7 +110,6 @@ public:
     // LinkInterface
 
     LinkConfiguration* getLinkConfiguration();
-    int     getId() const;
     QString getName() const;
     void    requestReset();
     bool    isConnected() const;
@@ -144,7 +143,6 @@ protected:
     QSerialPort* _port;
     quint64 _bytesRead;
     int     _timeout;
-    int     _id;
     QMutex  _dataMutex;       // Mutex for reading data from _port
     QMutex  _writeMutex;      // Mutex for accessing the _transmitBuffer.
     QString _type;

src/comm/TCPLink.cc

@@ -46,7 +46,6 @@ TCPLink::TCPLink(TCPConfiguration *config)
     // We're doing it wrong - because the Qt folks got the API wrong:
     // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
     moveToThread(this);
-    _linkId = getNextLinkId();
     qDebug() << "TCP Created " << _config->name();
 }
 
@@ -200,11 +199,6 @@ bool TCPLink::isConnected() const
     return _socketIsConnected;
 }
 
-int TCPLink::getId() const
-{
-    return _linkId;
-}
-
 QString TCPLink::getName() const
 {
     return _config->name();

src/comm/TCPLink.h

@@ -126,7 +126,6 @@ public:
     void signalBytesWritten(void);
 
     // LinkInterface methods
-    virtual int     getId(void) const;
     virtual QString getName(void) const;
     virtual bool    isConnected(void) const;
     virtual void    requestReset(void) {};
@@ -174,7 +173,6 @@ private:
     void _writeDebugBytes(const char *data, qint16 size);
 #endif
 
-    int               _linkId;
     TCPConfiguration* _config;
     QTcpSocket*       _socket;
     bool              _socketIsConnected;

src/comm/UDPLink.cc

@@ -51,8 +51,6 @@ UDPLink::UDPLink(UDPConfiguration* config)
     // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
     moveToThread(this);
 
-    // Set unique ID and add link to the list of links
-    _id = getNextLinkId();
     qDebug() << "UDP Created " << _config->name();
 }
 
@@ -240,11 +238,6 @@ bool UDPLink::isConnected() const
     return _connectState;
 }
 
-int UDPLink::getId() const
-{
-    return _id;
-}
-
 qint64 UDPLink::getConnectionSpeed() const
 {
     return 54000000; // 54 Mbit

src/comm/UDPLink.h

@@ -158,7 +158,6 @@ public:
     qint64 getCurrentOutDataRate() const;
 
     void run();
-    int getId() const;
 
     // These are left unimplemented in order to cause linker errors which indicate incorrect usage of
     // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
@@ -189,7 +188,6 @@ protected:
     QUdpSocket*         _socket;
     UDPConfiguration*   _config;
     bool                _connectState;
-    int                 _id;
 
 private:
     // Links are only created/destroyed by LinkManager so constructor/destructor is not public

src/comm/XbeeLink.cpp

@@ -8,7 +8,6 @@
 
 XbeeLink::XbeeLink(QString portName, int baudRate) : 
 	m_xbeeCon(NULL),
-    m_id(-1),
     m_portName(NULL),
     m_portNameLength(0),
     m_baudRate(baudRate),
@@ -19,13 +18,6 @@ XbeeLink::XbeeLink(QString portName, int baudRate) :
 
 	/* setup the xbee */
 	this->setPortName(portName);
-	
-	//this->connect();
-	// Set unique ID and add link to the list of links
-	this->m_id = getNextLinkId();
-	// set the Name
-	this->m_name = tr("xbee link") + QString::number(this->m_id);
-	emit nameChanged(this->m_name);
 }
 
 XbeeLink::~XbeeLink()
@@ -110,11 +102,6 @@ bool XbeeLink::setBaudRate(int rate)
 	return retVal;
 }
 
-int XbeeLink::getId() const
-{
-	return this->m_id;
-}
-
 QString XbeeLink::getName() const
 {
 	return this->m_name;

src/comm/XbeeLink.h

@@ -35,8 +35,8 @@ public slots: // virtual functions from XbeeLinkInterface
 	bool setRemoteAddressHigh(quint32 high);
 	bool setRemoteAddressLow(quint32 low);
 
-public: // virtual functions from LinkInterface
-    int getId() const;
+public:
+    // virtual functions from LinkInterface
     QString getName() const;
     bool isConnected() const;
 
@@ -56,7 +56,6 @@ public:
 
 protected:
 	xbee_con *m_xbeeCon;
-	int m_id;
 	char *m_portName;
 	unsigned int m_portNameLength;
 	int m_baudRate;

src/qgcunittest/MockLink.cc

@@ -67,7 +67,6 @@ union px4_custom_mode {
 };
 
 MockLink::MockLink(MockConfiguration* config) :
-    _linkId(getNextLinkId()),
     _name("MockLink"),
     _connected(false),
     _vehicleSystemId(128),     // FIXME: Pull from eventual parameter manager
@@ -287,7 +286,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
 
     for (qint64 i=0; i<cBytes; i++)
     {
-        if (!mavlink_parse_char(_linkId, bytes[i], &msg, &comm)) {
+        if (!mavlink_parse_char(getMavlinkChannel(), bytes[i], &msg, &comm)) {
             continue;
         }
 

src/qgcunittest/MockLink.h

@@ -61,7 +61,6 @@ public:
     ~MockLink(void);
 
     // Virtuals from LinkInterface
-    virtual int getId(void) const { return _linkId; }
     virtual QString getName(void) const { return _name; }
     virtual void requestReset(void){ }
     virtual bool isConnected(void) const { return _connected; }
@@ -123,7 +122,6 @@ private:
 
     MockLinkMissionItemHandler* _missionItemHandler;
 
-    int     _linkId;
     QString _name;
     bool    _connected;
 

src/uas/UAS.cc

@@ -1749,7 +1749,7 @@ void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
     // Write message into buffer, prepending start sign
     int len = mavlink_msg_to_send_buffer(buffer, &message);
     static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
-    mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getId(), message.len, messageKeys[message.msgid]);
+    mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
 
     // If link is connected
     if (link->isConnected())

src/ui/DebugConsole.cc

@@ -172,9 +172,9 @@ void DebugConsole::uasCreated(UASInterface* uas)
 void DebugConsole::addLink(LinkInterface* link)
 {
     // Add link to link list
-    links.insert(link->getId(), link);
+    links.insert(link->getMavlinkChannel(), link);
 
-    m_ui->linkComboBox->insertItem(link->getId(), link->getName());
+    m_ui->linkComboBox->insertItem(link->getMavlinkChannel(), link->getName());
     // Set new item as current
     m_ui->linkComboBox->setCurrentIndex(qMax(0, links.size() - 1));
     linkSelected(m_ui->linkComboBox->currentIndex());

src/ui/QGCMAVLinkLogPlayer.cc

@@ -576,7 +576,7 @@ quint64 QGCMAVLinkLogPlayer::findNextMavlinkMessage(mavlink_message_t *msg)
     char nextByte;
     mavlink_status_t comm;
     while (logFile.getChar(&nextByte)) { // Loop over every byte
-        bool messageFound = mavlink_parse_char(logLink->getId(), nextByte, msg, &comm);
+        bool messageFound = mavlink_parse_char(logLink->getMavlinkChannel(), nextByte, msg, &comm);
 
         // If we've found a message, jump back to the start of the message, grab the timestamp,
         // and go back to the end of this file.