diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 5fa0dceda761c04108a6d2d0a36cc53a54885cd7..cbe4bdea6ee1eb4de6842f290ff453a9e73d7b2d 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -359,7 +359,7 @@ void MultiVehicleManager::_sendGCSHeartbeat(void) LinkManager* linkMgr = _toolbox->linkManager(); for (int i=0; ilinks().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(), diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 21fca049a3bf9cdf98b0cc566dbd37c102eeaef9..56eed2fa3c098250c71b13fcdc9546a4fe3a1139 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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::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); } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 35ba2a188856a859508bc27a21e3f594b9c9a68c..ba867511334781bb2cd8a4251c59709ebc10fc23 100644 --- a/src/Vehicle/Vehicle.h +++ b/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;