Merge pull request #6514 from DonLakeFlyer/OpticalFlow

PX4 Optical Flow fixes
mavlink · May 28, 2018 · f1e2638 · f1e2638
2 parents 34931bf + f0eb002
commit f1e2638
Show file tree

Hide file tree

Showing 13 changed files with 70 additions and 27 deletions.
diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc
@@ -74,6 +74,14 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox)
 
 void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType)
 {
+    // Special case PX4 Flow since depending on firmware it can have different settings. We force to the PX4 Firmware settings.
+    if (link->isPX4Flow()) {
+        vehicleId = 81;
+        componentId = 50;//MAV_COMP_ID_AUTOPILOT1;
+        vehicleFirmwareType = MAV_AUTOPILOT_GENERIC;
+        vehicleType = 0;
+    }
+
     if (componentId != MAV_COMP_ID_AUTOPILOT1) {
         // Special case for PX4 Flow
         if (vehicleId != 81 || componentId != 50) {

diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
@@ -239,8 +239,8 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(_mav, SIGNAL(attitudeChanged                    (UASInterface*,double,double,double,quint64)),              this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
     connect(_mav, SIGNAL(attitudeChanged                    (UASInterface*,int,double,double,double,quint64)),          this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
 
-    if (_highLatencyLink) {
-        // High latency links don't request information
+    if (_highLatencyLink || link->isPX4Flow()) {
+        // These links don't request information
         _setMaxProtoVersion(100);
         _setCapabilities(0);
         _initialPlanRequestComplete = true;

diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
@@ -496,6 +496,7 @@ class Vehicle : public FactGroup
     Q_PROPERTY(bool                 supportsTerrainFrame    READ supportsTerrainFrame                                   NOTIFY firmwareTypeChanged)
     Q_PROPERTY(QString              priorityLinkName        READ priorityLinkName       WRITE setPriorityLinkByName     NOTIFY priorityLinkNameChanged)
     Q_PROPERTY(QVariantList         links                   READ links                                                  NOTIFY linksChanged)
+    Q_PROPERTY(LinkInterface*       priorityLink            READ priorityLink                                           NOTIFY priorityLinkNameChanged)
 
     // Vehicle state used for guided control
     Q_PROPERTY(bool flying                  READ flying NOTIFY flyingChanged)                               ///< Vehicle is flying

diff --git a/src/VehicleSetup/FirmwareUpgrade.qml b/src/VehicleSetup/FirmwareUpgrade.qml
@@ -170,16 +170,20 @@ SetupPage {
                         if (_singleFirmwareMode) {
                             controller.flashSingleFirmwareMode(controller.selectedFirmwareType)
                         } else {
-                            var stack = apmFlightStack.checked ? FirmwareUpgradeController.AutoPilotStackAPM : FirmwareUpgradeController.AutoPilotStackPX4
-                            if (px4Flow) {
-                                stack = FirmwareUpgradeController.PX4Flow
-                            }
-
+                            var stack
                             var firmwareType = firmwareVersionCombo.model.get(firmwareVersionCombo.currentIndex).firmwareType
                             var vehicleType = FirmwareUpgradeController.DefaultVehicleFirmware
-                            if (apmFlightStack.checked) {
-                                vehicleType = controller.vehicleTypeFromVersionIndex(vehicleTypeSelectionCombo.currentIndex)
+
+                            if (px4Flow) {
+                                stack = px4FlowTypeSelectionCombo.model.get(px4FlowTypeSelectionCombo.currentIndex).stackType
+                                vehicleType = FirmwareUpgradeController.DefaultVehicleFirmware
+                            } else {
+                                stack = apmFlightStack.checked ? FirmwareUpgradeController.AutoPilotStackAPM : FirmwareUpgradeController.AutoPilotStackPX4
+                                if (apmFlightStack.checked) {
+                                    vehicleType = controller.vehicleTypeFromVersionIndex(vehicleTypeSelectionCombo.currentIndex)
+                                }
                             }
+
                             controller.flash(stack, firmwareType, vehicleType)
                         }
                     }
@@ -214,6 +218,19 @@ SetupPage {
                         }
                     }
 
+                    ListModel {
+                        id: px4FlowFirmwareList
+
+                        ListElement {
+                            text:           qsTr("PX4 Pro")
+                            stackType:   FirmwareUpgradeController.PX4FlowPX4
+                        }
+                        ListElement {
+                            text:           qsTr("ArduPilot")
+                            stackType:   FirmwareUpgradeController.PX4FlowAPM
+                        }
+                    }
+
                     ListModel {
                         id: px4FlowTypeList
 
@@ -249,7 +266,7 @@ SetupPage {
                             wrapMode:   Text.WordWrap
                             text:       _singleFirmwareMode ? _singleFirmwareLabel : (px4Flow ? _px4FlowLabel : _pixhawkLabel)
 
-                            readonly property string _px4FlowLabel:          qsTr("Detected PX4 Flow board. You can select from the following firmware:")
+                            readonly property string _px4FlowLabel:          qsTr("Detected PX4 Flow board. The firmware you use on the PX4 Flow must match the AutoPilot firmware type you are using on the vehicle:")
                             readonly property string _pixhawkLabel:          qsTr("Detected Pixhawk board. You can select from the following flight stacks:")
                             readonly property string _singleFirmwareLabel:   qsTr("Press Ok to upgrade your vehicle.")
                         }
@@ -301,10 +318,19 @@ SetupPage {
                             id:             vehicleTypeSelectionCombo
                             anchors.left:   parent.left
                             anchors.right:  parent.right
-                            visible:        apmFlightStack.checked
+                            visible:        !px4Flow && apmFlightStack.checked
                             model:          controller.apmAvailableVersions
                         }
 
+                        QGCComboBox {
+                            id:             px4FlowTypeSelectionCombo
+                            anchors.left:   parent.left
+                            anchors.right:  parent.right
+                            visible:        px4Flow
+                            model:          px4FlowFirmwareList
+                            currentIndex:   _defaultFirmwareIsPX4 ? 0 : 1
+                        }
+
                         Row {
                             width:      parent.width
                             spacing:    ScreenTools.defaultFontPixelWidth / 2
@@ -349,7 +375,7 @@ SetupPage {
                             anchors.left:   parent.left
                             anchors.right:  parent.right
                             visible:        showFirmwareTypeSelection
-                            model:          _singleFirmwareMode ? singleFirmwareModeTypeList: (px4Flow ? px4FlowTypeList : firmwareTypeList)
+                            model:          _singleFirmwareMode ? singleFirmwareModeTypeList : (px4Flow ? px4FlowTypeList : firmwareTypeList)
                             currentIndex:   controller.selectedFirmwareType
 
                             onActivated: {

diff --git a/src/VehicleSetup/FirmwareUpgradeController.cc b/src/VehicleSetup/FirmwareUpgradeController.cc
@@ -99,6 +99,7 @@ void FirmwareUpgradeController::flash(AutoPilotStackType_t stackType,
                                       FirmwareType_t firmwareType,
                                       FirmwareVehicleType_t vehicleType)
 {
+    qCDebug(FirmwareUpgradeLog) << "_flash stackType:firmwareType:vehicleType" << stackType << firmwareType << vehicleType;
     FirmwareIdentifier firmwareId = FirmwareIdentifier(stackType, firmwareType, vehicleType);
     if (_bootloaderFound) {
         _getFirmwareFile(firmwareId);
@@ -351,7 +352,8 @@ void FirmwareUpgradeController::_initFirmwareHash()
 
     /////////////////////////////// px4flow firmwares ///////////////////////////////////////
     FirmwareToUrlElement_t rgPX4FLowFirmwareArray[] = {
-        { PX4Flow, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Flow/master/px4flow.px4" },
+        { PX4FlowPX4, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Flow/master/px4flow.px4" },
+        { PX4FlowAPM, StableFirmware, DefaultVehicleFirmware, "http://firmware.ardupilot.org/Tools/PX4Flow/px4flow-klt-06Dec2014.px4" },
     };
 
     /////////////////////////////// 3dr radio firmwares ///////////////////////////////////////

diff --git a/src/VehicleSetup/FirmwareUpgradeController.h b/src/VehicleSetup/FirmwareUpgradeController.h
@@ -40,7 +40,8 @@ class FirmwareUpgradeController : public QObject
         typedef enum {
             AutoPilotStackPX4,
             AutoPilotStackAPM,
-            PX4Flow,
+            PX4FlowPX4,
+            PX4FlowAPM,
             ThreeDRRadio,
             SingleFirmwareMode
         } AutoPilotStackType_t;

diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml
@@ -258,7 +258,7 @@ Rectangle {
             SubMenuButton {
                 id:                 px4FlowButton
                 exclusiveGroup:     setupButtonGroup
-                visible:            QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle.genericFirmware : false
+                visible:            QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle.priorityLink.isPX4Flow : false
                 setupIndicator:     false
                 text:               qsTr("PX4Flow")
                 Layout.fillWidth:   true

diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc
@@ -42,13 +42,14 @@ uint8_t LinkInterface::mavlinkChannel(void) const
     return _mavlinkChannel;
 }
 // Links are only created by LinkManager so constructor is not public
-LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config)
+LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4Flow)
     : QThread                   (0)
     , _config                   (config)
     , _highLatency              (config->isHighLatency())
     , _mavlinkChannelSet        (false)
     , _enableRateCollection     (false)
     , _decodedFirstMavlinkPacket(false)
+    , _isPX4Flow                (isPX4Flow)
 {
     QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
 

diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h
@@ -43,13 +43,16 @@ class LinkInterface : public QThread
         _config->setLink(NULL);
     }
 
-    Q_PROPERTY(bool active      READ active                                 NOTIFY activeChanged)
+    Q_PROPERTY(bool active      READ active     NOTIFY activeChanged)
+    Q_PROPERTY(bool isPX4Flow   READ isPX4Flow  CONSTANT)
 
-    // Property accessors
-    bool active() const;
     Q_INVOKABLE bool link_active(int vehicle_id) const;
     Q_INVOKABLE bool getHighLatency(void) const { return _highLatency; }
 
+    // Property accessors
+    bool active() const;
+    bool isPX4Flow(void) const { return _isPX4Flow; }
+
     LinkConfiguration* getLinkConfiguration(void) { return _config.data(); }
 
     /* Connection management */
@@ -201,7 +204,7 @@ private slots:
 
 protected:
     // Links are only created by LinkManager so constructor is not public
-    LinkInterface(SharedLinkConfigurationPointer& config);
+    LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4Flow = false);
 
     /// This function logs the send times and amounts of datas for input. Data is used for calculating
     /// the transmission rate.
@@ -298,6 +301,7 @@ private slots:
 
     bool _enableRateCollection;
     bool _decodedFirstMavlinkPacket;    ///< true: link has correctly decoded it's first mavlink packet
+    bool _isPX4Flow;
 
     QMap<int /* vehicle id */, HeartbeatTimer*> _heartbeatTimers;
 };

diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc
@@ -97,7 +97,7 @@ void LinkManager::createConnectedLink(LinkConfiguration* config)
     }
 }
 
-LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& config)
+LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& config, bool isPX4Flow)
 {
     if (!config) {
         qWarning() << "LinkManager::createConnectedLink called with NULL config";
@@ -111,7 +111,7 @@ LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer&
     {
         SerialConfiguration* serialConfig = dynamic_cast<SerialConfiguration*>(config.data());
         if (serialConfig) {
-            pLink = new SerialLink(config);
+            pLink = new SerialLink(config, isPX4Flow);
             if (serialConfig->usbDirect()) {
                 _activeLinkCheckList.append((SerialLink*)pLink);
                 if (!_activeLinkCheckTimer.isActive()) {
@@ -599,7 +599,7 @@ void LinkManager::_updateAutoConnectLinks(void)
                     pSerialConfig->setDynamic(true);
                     pSerialConfig->setPortName(portInfo.systemLocation());
                     _sharedAutoconnectConfigurations.append(SharedLinkConfigurationPointer(pSerialConfig));
-                    createConnectedLink(_sharedAutoconnectConfigurations.last());
+                    createConnectedLink(_sharedAutoconnectConfigurations.last(), boardType == QGCSerialPortInfo::BoardTypePX4Flow);
                 }
             }
         }

diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h
@@ -104,7 +104,7 @@ class LinkManager : public QGCTool
 
     /// Creates, connects (and adds) a link  based on the given configuration instance.
     /// Link takes ownership of config.
-    LinkInterface* createConnectedLink(SharedLinkConfigurationPointer& config);
+    LinkInterface* createConnectedLink(SharedLinkConfigurationPointer& config, bool isPX4Flow = false);
 
     // This should only be used by Qml code
     Q_INVOKABLE void createConnectedLink(LinkConfiguration* config);

diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc
@@ -30,8 +30,8 @@ QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog")
 
 static QStringList kSupportedBaudRates;
 
-SerialLink::SerialLink(SharedLinkConfigurationPointer& config)
-    : LinkInterface(config)
+SerialLink::SerialLink(SharedLinkConfigurationPointer& config, bool isPX4Flow)
+    : LinkInterface(config, isPX4Flow)
     , _port(NULL)
     , _bytesRead(0)
     , _stopp(false)

diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h
@@ -169,7 +169,7 @@ private slots:
 
 private:
     // Links are only created/destroyed by LinkManager so constructor/destructor is not public
-    SerialLink(SharedLinkConfigurationPointer& config);
+    SerialLink(SharedLinkConfigurationPointer& config, bool isPX4Flow = false);
     ~SerialLink();
 
     // From LinkInterface