Merge remote-tracking branch 'upstream/master' into fgfsrate

src/comm/SerialLink.cc

@@ -115,6 +115,32 @@ QList<QString> SerialLink::getCurrentPorts()
     return m_ports;
 }
 
+bool SerialLink::isBootloader()
+{
+    QList<QSerialPortInfo> portList =  QSerialPortInfo::availablePorts();
+
+    if( portList.count() == 0){
+        return false;
+    }
+
+    foreach (const QSerialPortInfo &info, portList)
+    {
+//        qDebug() << "PortName    : " << info.portName()
+//                 << "Description : " << info.description();
+//        qDebug() << "Manufacturer: " << info.manufacturer();
+
+       if (info.portName().trimmed() == this->m_portName.trimmed() &&
+               (info.description().toLower().contains("bootloader") ||
+                info.description().toLower().contains("px4 bl"))) {
+           qDebug() << "BOOTLOADER FOUND";
+           return true;
+       }
+    }
+
+    // Not found
+    return false;
+}
+
 void SerialLink::loadSettings()
 {
     // Load defaults from settings
@@ -445,6 +471,28 @@ bool SerialLink::hardwareConnect(QString &type)
     }
 
     qDebug() << "SerialLink: hardwareConnect to " << m_portName;
+
+    if (isBootloader()) {
+        qDebug() << "Not connecting to a bootloader, waiting for 2nd chance";
+
+        const unsigned retry_limit = 12;
+        unsigned retries;
+
+        for (retries = 0; retries < retry_limit; retries++) {
+            if (!isBootloader()) {
+                break;
+            }
+            QGC::SLEEP::msleep(500);
+        }
+
+        // Check limit
+        if (retries == retry_limit) {
+
+            // bail out
+            return false;
+        }
+    }
+
     m_port = new QSerialPort(m_portName);
     m_port->moveToThread(this);
 

src/comm/SerialLink.h

@@ -72,6 +72,9 @@ public:
     /** @brief Get a list of the currently available ports */
     QList<QString> getCurrentPorts();
 
+    /** @brief Check if the current port is a bootloader */
+    bool isBootloader();
+
     void requestReset();
 
     bool isConnected() const;

src/uas/UAS.cc

@@ -1102,13 +1102,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
 
             // Check that this message applies to the UAS.
-            if(mc.target_system == mavlink->getSystemId() && mc.target_component == mavlink->getComponentId())
+            if(mc.target_system == mavlink->getSystemId())
             {
+
+                if (mc.target_component != mavlink->getComponentId()) {
+                    qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error.";
+                    qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mc.target_component;
+                }
+
                 waypointManager.handleWaypointCount(message.sysid, message.compid, mc.count);
             }
             else
             {
-                qDebug() << tr("Received mission count message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mc.target_system);
+                qDebug() << QString("Received mission count message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mc.target_system);
             }
         }
             break;
@@ -1127,13 +1133,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
 
             // Check that the item pertains to this UAS.
-            if(mi.target_system == mavlink->getSystemId() && mi.target_component == mavlink->getComponentId())
+            if(mi.target_system == mavlink->getSystemId())
             {
+
+                if (mi.target_component != mavlink->getComponentId()) {
+                    qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error.";
+                    qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mi.target_component;
+                }
+
                 waypointManager.handleWaypoint(message.sysid, message.compid, &mi);
             }
             else
             {
-                qDebug() << tr("Received mission item message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mi.target_system);
+                qDebug() << QString("Received mission item message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mi.target_system);
             }
         }
             break;
@@ -1152,13 +1164,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
 
             // Check that the ack pertains to this UAS.
-            if(ma.target_system == mavlink->getSystemId() && ma.target_component == mavlink->getComponentId())
+            if(ma.target_system == mavlink->getSystemId())
             {
+
+                if (ma.target_component != mavlink->getComponentId()) {
+                    qDebug() << tr("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error.");
+                    qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << ma.target_component;
+                }
+
                 waypointManager.handleWaypointAck(message.sysid, message.compid, &ma);
             }
             else
             {
-                qDebug() << tr("Received mission ack message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(ma.target_system);
+                qDebug() << QString("Received mission ack message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(ma.target_system);
             }
         }
             break;
@@ -1177,13 +1195,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
 
             // Check that the request pertains to this UAS.
-            if(mr.target_system == mavlink->getSystemId() && mr.target_component == mavlink->getComponentId())
+            if(mr.target_system == mavlink->getSystemId())
             {
+
+                if (mr.target_component != mavlink->getComponentId()) {
+                    qDebug() << QString("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error.");
+                    qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mr.target_component;
+                }
+
                 waypointManager.handleWaypointRequest(message.sysid, message.compid, &mr);
             }
             else
             {
-                qDebug() << tr("Received mission request message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mr.target_system);
+                qDebug() << QString("Received mission request message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mr.target_system);
             }
         }
             break;