Do not change the priority link if it was commanded and is still active

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());

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