Merge pull request #6091 from DonLakeFlyer/HighLatency

More work in high latency

src/Vehicle/MultiVehicleManager.cc

@@ -359,7 +359,7 @@ void MultiVehicleManager::_sendGCSHeartbeat(void)
     LinkManager* linkMgr = _toolbox->linkManager();
     for (int i=0; i<linkMgr->links().count(); i++) {
         LinkInterface* link = linkMgr->links()[i];
-        if (link->isConnected()) {
+        if (link->isConnected() && !link->highLatency()) {
             mavlink_message_t message;
             mavlink_msg_heartbeat_pack_chan(_mavlinkProtocol->getSystemId(),
                                             _mavlinkProtocol->getComponentId(),

src/Vehicle/Vehicle.cc

@@ -221,7 +221,7 @@ Vehicle::Vehicle(LinkInterface*             link,
 
     // Send MAV_CMD ack timer
     _mavCommandAckTimer.setSingleShot(true);
-    _mavCommandAckTimer.setInterval(_mavCommandAckTimeoutMSecs);
+    _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
     connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain);
 
     _mav = uas();
@@ -833,8 +833,9 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message)
     _airSpeedFact.setRawValue((double)highLatency2.airspeed / 5.0);
     _groundSpeedFact.setRawValue((double)highLatency2.groundspeed / 5.0);
     _climbRateFact.setRawValue((double)highLatency2.climb_rate / 10.0);
-
     _headingFact.setRawValue((double)highLatency2.heading * 2.0);
+    _altitudeRelativeFact.setRawValue(std::numeric_limits<double>::quiet_NaN());
+    _altitudeAMSLFact.setRawValue(highLatency2.altitude);
 
     _windFactGroup.direction()->setRawValue((double)highLatency2.wind_heading * 2.0);
     _windFactGroup.speed()->setRawValue((double)highLatency2.windspeed / 5.0);
@@ -1497,6 +1498,7 @@ void Vehicle::_updatePriorityLink(void)
 
     if (newPriorityLink) {
         _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
+        _updateHighLatencyLink();
     }
 }
 
@@ -3028,6 +3030,7 @@ void Vehicle::_updateHighLatencyLink(void)
 {
     if (_priorityLink->highLatency() != _highLatencyLink) {
         _highLatencyLink = _priorityLink->highLatency();
+        _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
         emit highLatencyLinkChanged(_highLatencyLink);
     }
 }

src/Vehicle/Vehicle.h

@@ -1016,6 +1016,7 @@ private:
     int                             _mavCommandRetryCount;
     static const int                _mavCommandMaxRetryCount = 3;
     static const int                _mavCommandAckTimeoutMSecs = 3000;
+    static const int                _mavCommandAckTimeoutMSecsHighLatency = 120000;
 
     QString             _prearmError;
     QTimer              _prearmErrorTimer;