diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 478a8b1a6dba2f23a86a0f919554d857fd7b8ddb..dd84995872a18a3cd00f71c81d0121e855370b83 100644 --- 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 index 791634d2ed9755a8c38e768b8f0a6ff6212ab68d..2fdb1d5845956a91dc99c41025bd294f1fdbe68c 100644 --- 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 index 0c03e72a50632f60f16d674c2a0afd486a407616..329dc47ce3784cf705fb9e08b0cb8f3acefbd855 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -496,6 +496,7 @@ public: 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/FirmwareUpgradeController.cc b/src/VehicleSetup/FirmwareUpgradeController.cc index 4f60f2459a75d6294a773b8b0f84fef7eafd05ba..11bc7ea6a254ae808c54598cac0d798fd80dd495 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.cc +++ b/src/VehicleSetup/FirmwareUpgradeController.cc @@ -353,7 +353,7 @@ void FirmwareUpgradeController::_initFirmwareHash() /////////////////////////////// px4flow firmwares /////////////////////////////////////// FirmwareToUrlElement_t rgPX4FLowFirmwareArray[] = { { PX4FlowPX4, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Flow/master/px4flow.px4" }, - { PX4FlowAPM, StableFirmware, DefaultVehicleFirmware, "http://px4-travis.s3.amazonaws.com/Flow/master/px4flowAPM.px4" }, + { PX4FlowAPM, StableFirmware, DefaultVehicleFirmware, "http://firmware.ardupilot.org/Tools/PX4Flow/px4flow-klt-06Dec2014.px4" }, }; /////////////////////////////// 3dr radio firmwares /////////////////////////////////////// diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml index 9b94e26c72a696d83d3ecf87b62fc3b82a605b73..6d7057221d19a2bbbcb501c0dd2af7d74fe3db04 100644 --- a/src/VehicleSetup/SetupView.qml +++ b/src/VehicleSetup/SetupView.qml @@ -259,7 +259,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 index 49de99628c189dc2b511320e6a3b2d2fe792f39f..13da6a8508bee26ab22d69e082a70febe9279111 100644 --- 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 index 3455279fe7bc1c0bca45ed9c4b9c98b2ceeba2f0..0597d3ec4f3746c44f86864b72218646e195dd89 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -43,13 +43,16 @@ public: _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 @@ signals: 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: bool _enableRateCollection; bool _decodedFirstMavlinkPacket; ///< true: link has correctly decoded it's first mavlink packet + bool _isPX4Flow; QMap _heartbeatTimers; }; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index aa2204f6ff408cf3c4a98698ff65c3360899deee..86ec3ba1b6f91abc882d56d4407c81090b55fb04 100644 --- 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(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 index 13c0ed85261fd3f2fff06125879fb3148359ef64..8c21ed5dac0542418aaa0c5e9affa93cd74461a7 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -104,7 +104,7 @@ public: /// 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 index ecbf26ccd1a323dc48253a46f44254a6d8ec4f90..015ec95c4e22539bcba0ab7f5db7844549f83634 100644 --- 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 index 9430f20686586fd8193b571ef6b67db5944f3dac..82fe598d8cc5667839e1d134a95e7abca0440878 100644 --- 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