From a09fcba928194d686d48c8b50119439ee0582a38 Mon Sep 17 00:00:00 2001
From: acfloria <florian.achermann@mavt.ethz.ch>
Date: Fri, 27 Apr 2018 14:56:43 +0200
Subject: [PATCH] Do not change the priority link if it was commanded and is
 still active

---
 src/Vehicle/Vehicle.cc | 12 ++++++++++++
 src/Vehicle/Vehicle.h  |  1 +
 2 files changed, 13 insertions(+)

diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 8ab02b4bd..c21ce1095 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 7807f79be..44025338f 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
 
-- 
2.22.0