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;