diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 8ab02b4bd9f2c763fb447dc6be3994c59c144af2..c21ce1095308be1e7458f8cd56e8bbce2d3aff0b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -168,6 +168,7 @@ Vehicle::Vehicle(LinkInterface* link, , _gitHash(versionNotSetValue) , _uid(0) , _lastAnnouncedLowBatteryPercent(100) + , _priorityLinkCommanded(false) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) @@ -1681,6 +1682,16 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand) { emit linkNamesChanged(); + + // if the priority link is commanded and still active don't change anything + if (_priorityLinkCommanded) { + if (_priorityLink.data()->active(_id)) { + return; + } else { + _priorityLinkCommanded = false; + } + } + LinkInterface* newPriorityLink = NULL; // This routine specifically does not clear _priorityLink when there are no links remaining. @@ -2149,6 +2160,7 @@ void Vehicle::setPriorityLinkByName(const QString& priorityLinkName) } if (newPriorityLink) { + _priorityLinkCommanded = true; _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); _updateHighLatencyLink(true); emit priorityLinkNameChanged(_priorityLink->getName()); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7807f79bea1c9f86996d38df0a0c849f06e4c1cf..44025338fef0496e6972e063a22678e8f2bb4e30 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1271,6 +1271,7 @@ private: int _lastAnnouncedLowBatteryPercent; SharedLinkInterfacePointer _priorityLink; // We always keep a reference to the priority link to manage shutdown ordering + bool _priorityLinkCommanded; // FactGroup facts