Commit f0eb0029 authored by DonLakeFlyer's avatar DonLakeFlyer

Identify PX4 Flow from board id

parent 5cbe79b7
......@@ -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) {
......
......@@ -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;
......
......@@ -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
......
......@@ -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 ///////////////////////////////////////
......
......@@ -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
......
......@@ -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);
......
......@@ -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<int /* vehicle id */, HeartbeatTimer*> _heartbeatTimers;
};
......
......@@ -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);
}
}
}
......
......@@ -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);
......
......@@ -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)
......
......@@ -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
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment