Add support for HIGH_LATENCY2

* Flight modes still not implemented. Waiting on message change.
* Also includes various cauterization of UI things which should not be
available on high latency link.

src/FactSystem/ParameterManager.cc

@@ -81,7 +81,17 @@ ParameterManager::ParameterManager(Vehicle* vehicle)
     // Ensure the cache directory exists
     QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache");
 
-    refreshAllParameters();
+    if (_vehicle->highLatencyLink()) {
+        // High latency links don't load parameters
+        _parametersReady = true;
+        _missingParameters = true;
+        _initialLoadComplete = true;
+        _waitingForDefaultComponent = false;
+        emit parametersReadyChanged(_parametersReady);
+        emit missingParametersChanged(_missingParameters);
+    } else {
+        refreshAllParameters();
+    }
 }
 
 ParameterManager::~ParameterManager()
@@ -133,12 +143,12 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString
     }
 #endif
 
-	if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") {
-    	if (!_initialLoadComplete && !_logReplay) {
-        	/* we received a cache hash, potentially load from cache */
-        	_tryCacheHashLoad(vehicleId, componentId, value);
-		}
-		return;
+    if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") {
+        if (!_initialLoadComplete && !_logReplay) {
+            /* we received a cache hash, potentially load from cache */
+            _tryCacheHashLoad(vehicleId, componentId, value);
+        }
+        return;
     }
 
     _initialRequestTimeoutTimer.stop();

src/MissionManager/PlanMasterController.cc

@@ -157,6 +157,11 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle)
 
 void PlanMasterController::loadFromVehicle(void)
 {
+    if (_managerVehicle->highLatencyLink()) {
+        qgcApp()->showMessage(tr("Download not supported on high latency links."));
+        return;
+    }
+
     if (offline()) {
         qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while offline";
     } else if (!_editMode) {
@@ -256,6 +261,11 @@ void PlanMasterController::_sendRallyPointsComplete(void)
 
 void PlanMasterController::sendToVehicle(void)
 {
+    if (_managerVehicle->highLatencyLink()) {
+        qgcApp()->showMessage(tr("Upload not supported on high latency links."));
+        return;
+    }
+
     if (offline()) {
         qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while offline";
     } else if (syncInProgress()) {

src/MissionManager/VisualMissionItem.cc

@@ -63,7 +63,7 @@ VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* pa
 {
     *this = other;
 
-    // Don't get terrain altitude information for submarines or boards
+    // Don't get terrain altitude information for submarines or boats
     if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) {
         connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude);
     }

src/Vehicle/MultiVehicleManager.cc

@@ -72,7 +72,7 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox)
                                          this);
 }
 
-void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType)
+void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType)
 {
     if (componentId != MAV_COMP_ID_AUTOPILOT1) {
         // Special case for PX4 Flow
@@ -82,7 +82,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
                                               << link->getName()
                                               << vehicleId
                                               << componentId
-                                              << vehicleMavlinkVersion
                                               << vehicleFirmwareType
                                               << vehicleType;
             return;
@@ -108,11 +107,10 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
         break;
     }
 
-    qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:componentId:vehicleMavlinkVersion:vehicleFirmwareType:vehicleType "
+    qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:componentId:vehicleFirmwareType:vehicleType "
                                       << link->getName()
                                       << vehicleId
                                       << componentId
-                                      << vehicleMavlinkVersion
                                       << vehicleFirmwareType
                                       << vehicleType;
 
@@ -120,16 +118,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
         _app->showMessage(tr("Warning: A vehicle is using the same system id as %1: %2").arg(qgcApp()->applicationName()).arg(vehicleId));
     }
 
-    //    QSettings settings;
-    //    bool mavlinkVersionCheck = settings.value("VERSION_CHECK_ENABLED", true).toBool();
-    //    if (mavlinkVersionCheck && vehicleMavlinkVersion != MAVLINK_VERSION) {
-    //        _ignoreVehicleIds += vehicleId;
-    //        _app->showMessage(QString("The MAVLink protocol version on vehicle #%1 and %2 differ! "
-    //                                  "It is unsafe to use different MAVLink versions. "
-    //                                  "%2 therefore refuses to connect to vehicle #%1, which sends MAVLink version %3 (%2 uses version %4).").arg(vehicleId).arg(qgcApp()->applicationName()).arg(vehicleMavlinkVersion).arg(MAVLINK_VERSION));
-    //        return;
-    //    }
-
     Vehicle* vehicle = new Vehicle(link, vehicleId, componentId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _joystickManager);
     connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1);
     connect(vehicle, &Vehicle::requestProtocolVersion, this, &MultiVehicleManager::_requestProtocolVersion);

src/Vehicle/MultiVehicleManager.h

@@ -94,7 +94,7 @@ private slots:
     void _setActiveVehiclePhase2(void);
     void _vehicleParametersReadyChanged(bool parametersReady);
     void _sendGCSHeartbeat(void);
-    void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType);
+    void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType);
     void _requestProtocolVersion(unsigned version);
 
 private:

src/Vehicle/Vehicle.cc

@@ -233,18 +233,27 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(_mav, SIGNAL(attitudeChanged                    (UASInterface*,double,double,double,quint64)),              this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
     connect(_mav, SIGNAL(attitudeChanged                    (UASInterface*,int,double,double,double,quint64)),          this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
 
+    if (_highLatencyLink) {
+        // High latency links don't request information
+        _setMaxProtoVersion(100);
+        _setCapabilities(0);
+        _initialPlanRequestComplete = true;
+        _missionManagerInitialRequestSent = true;
+        _geoFenceManagerInitialRequestSent = true;
+        _rallyPointManagerInitialRequestSent = true;
+    } else {
+        // Ask the vehicle for protocol version info.
+        sendMavCommand(MAV_COMP_ID_ALL,                     // Don't know default component id yet.
+                       MAV_CMD_REQUEST_PROTOCOL_VERSION,
+                       false,                               // No error shown if fails
+                       1);                                  // Request protocol version
 
-    // Ask the vehicle for protocol version info.
-    sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
-                    MAV_CMD_REQUEST_PROTOCOL_VERSION,
-                   false,                                   // No error shown if fails
-                    1);                                     // Request protocol version
-
-    // Ask the vehicle for firmware version info.
-    sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
-                    MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
-                   false,                                   // No error shown if fails
-                    1);                                     // Request firmware version
+        // Ask the vehicle for firmware version info.
+        sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
+                       MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+                       false,                                   // No error shown if fails
+                       1);                                      // Request firmware version
+    }
 
     _firmwarePlugin->initializeVehicle(this);
 
@@ -677,13 +686,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         break;
     case MAVLINK_MSG_ID_SCALED_PRESSURE3:
         _handleScaledPressure3(message);
-        break;        
+        break;
     case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
         _handleCameraImageCaptured(message);
         break;
     case MAVLINK_MSG_ID_ADSB_VEHICLE:
         _handleADSBVehicle(message);
         break;
+    case MAVLINK_MSG_ID_HIGH_LATENCY2:
+        _handleHighLatency2(message);
+        break;
 
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
     {
@@ -693,7 +705,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     }
         break;
 
-    // Following are ArduPilot dialect messages
+        // Following are ArduPilot dialect messages
 #if !defined(NO_ARDUPILOT_DIALECT)
     case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
         _handleCameraFeedback(message);
@@ -797,6 +809,47 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
     emit coordinateChanged(_coordinate);
 }
 
+void Vehicle::_handleHighLatency2(mavlink_message_t& message)
+{
+    mavlink_high_latency2_t highLatency2;
+    mavlink_msg_high_latency2_decode(&message, &highLatency2);
+
+#if 0
+    typedef struct __mavlink_high_latency2_t {
+     uint16_t wp_num; /*< Current waypoint number*/
+     uint16_t failure_flags; /*< Indicates failures as defined in MAV_FAILURE_FLAG ENUM. */
+     uint8_t flight_mode; /*< Flight Mode of the vehicle as defined in the FLIGHT_MODE ENUM*/
+     uint8_t failsafe; /*< Indicates if a failsafe mode is triggered, defined in MAV_FAILSAFE_FLAG ENUM*/
+    }) mavlink_high_latency2_t;
+#endif
+
+    // FIXME: flight mode not yet supported
+
+    _coordinate.setLatitude(highLatency2.latitude  / (double)1E7);
+    _coordinate.setLongitude(highLatency2.longitude / (double)1E7);
+    _coordinate.setAltitude(highLatency2.altitude);
+    emit coordinateChanged(_coordinate);
+
+    _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);
+
+    _windFactGroup.direction()->setRawValue((double)highLatency2.wind_heading * 2.0);
+    _windFactGroup.speed()->setRawValue((double)highLatency2.windspeed / 5.0);
+
+    _batteryFactGroup.percentRemaining()->setRawValue(highLatency2.battery);
+
+    _temperatureFactGroup.temperature1()->setRawValue(highLatency2.temperature_air);
+
+    _gpsFactGroup.lat()->setRawValue(highLatency2.latitude * 1e-7);
+    _gpsFactGroup.lon()->setRawValue(highLatency2.longitude * 1e-7);
+    _gpsFactGroup.count()->setRawValue(0);
+    _gpsFactGroup.hdop()->setRawValue(highLatency2.eph == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.eph / 10.0);
+    _gpsFactGroup.vdop()->setRawValue(highLatency2.epv == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.epv / 10.0);
+}
+
 void Vehicle::_handleAltitude(mavlink_message_t& message)
 {
     mavlink_altitude_t altitude;
@@ -909,14 +962,14 @@ QString Vehicle::vehicleUIDStr()
     QString uid;
     uint8_t* pUid = (uint8_t*)(void*)&_uid;
     uid.sprintf("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X",
-        pUid[0] & 0xff,
-        pUid[1] & 0xff,
-        pUid[2] & 0xff,
-        pUid[3] & 0xff,
-        pUid[4] & 0xff,
-        pUid[5] & 0xff,
-        pUid[6] & 0xff,
-        pUid[7] & 0xff);
+                pUid[0] & 0xff,
+            pUid[1] & 0xff,
+            pUid[2] & 0xff,
+            pUid[3] & 0xff,
+            pUid[4] & 0xff,
+            pUid[5] & 0xff,
+            pUid[6] & 0xff,
+            pUid[7] & 0xff);
     return uid;
 }
 
@@ -926,22 +979,22 @@ void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
     mavlink_msg_hil_actuator_controls_decode(&message, &hil);
     emit hilActuatorControlsChanged(hil.time_usec, hil.flags,
                                     hil.controls[0],
-                                    hil.controls[1],
-                                    hil.controls[2],
-                                    hil.controls[3],
-                                    hil.controls[4],
-                                    hil.controls[5],
-                                    hil.controls[6],
-                                    hil.controls[7],
-                                    hil.controls[8],
-                                    hil.controls[9],
-                                    hil.controls[10],
-                                    hil.controls[11],
-                                    hil.controls[12],
-                                    hil.controls[13],
-                                    hil.controls[14],
-                                    hil.controls[15],
-                                    hil.mode);
+            hil.controls[1],
+            hil.controls[2],
+            hil.controls[3],
+            hil.controls[4],
+            hil.controls[5],
+            hil.controls[6],
+            hil.controls[7],
+            hil.controls[8],
+            hil.controls[9],
+            hil.controls[10],
+            hil.controls[11],
+            hil.controls[12],
+            hil.controls[13],
+            hil.controls[14],
+            hil.controls[15],
+            hil.mode);
 }
 
 void Vehicle::_handleCommandLong(mavlink_message_t& message)
@@ -954,19 +1007,19 @@ void Vehicle::_handleCommandLong(mavlink_message_t& message)
     mavlink_msg_command_long_decode(&message, &cmd);
 
     switch (cmd.command) {
-        // Other component on the same system
-        // requests that QGC frees up the serial port of the autopilot
-        case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
-            if (cmd.param6 > 0) {
-                // disconnect the USB link if its a direct connection to a Pixhawk
-                for (int i = 0; i < _links.length(); i++) {
-                    SerialLink *sl = qobject_cast<SerialLink*>(_links.at(i));
-                    if (sl && sl->getSerialConfig()->usbDirect()) {
-                        qDebug() << "Disconnecting:" << _links.at(i)->getName();
-                        qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i));
-                    }
+    // Other component on the same system
+    // requests that QGC frees up the serial port of the autopilot
+    case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+        if (cmd.param6 > 0) {
+            // disconnect the USB link if its a direct connection to a Pixhawk
+            for (int i = 0; i < _links.length(); i++) {
+                SerialLink *sl = qobject_cast<SerialLink*>(_links.at(i));
+                if (sl && sl->getSerialConfig()->usbDirect()) {
+                    qDebug() << "Disconnecting:" << _links.at(i)->getName();
+                    qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i));
                 }
             }
+        }
         break;
     }
 #endif
@@ -1351,6 +1404,7 @@ void Vehicle::_addLink(LinkInterface* link)
         qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
         _links += link;
         _updatePriorityLink();
+        _updateHighLatencyLink();
         connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
         connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
         connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink);
@@ -2098,9 +2152,9 @@ void Vehicle::_connectionActive(void)
 
         // Re-negotiate protocol version for the link
         sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
-                        MAV_CMD_REQUEST_PROTOCOL_VERSION,
+                       MAV_CMD_REQUEST_PROTOCOL_VERSION,
                        false,                                   // No error shown if fails
-                        1);                                     // Request protocol version
+                       1);                                     // Request protocol version
     }
 }
 
@@ -2618,7 +2672,7 @@ void Vehicle::setSoloFirmware(bool soloFirmware)
 }
 
 #if 0
-    // Temporarily removed, waiting for new command implementation
+// Temporarily removed, waiting for new command implementation
 void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
 {
     doCommandLongUnverified(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs);
@@ -2765,11 +2819,11 @@ void Vehicle::_ackMavlinkLogData(uint16_t sequence)
     ack.target_component = _defaultComponentId;
     ack.target_system = id();
     mavlink_msg_logging_ack_encode_chan(
-        _mavlink->getSystemId(),
-        _mavlink->getComponentId(),
-        priorityLink()->mavlinkChannel(),
-        &msg,
-        &ack);
+                _mavlink->getSystemId(),
+                _mavlink->getComponentId(),
+                priorityLink()->mavlinkChannel(),
+                &msg,
+                &ack);
     sendMessageOnLink(priorityLink(), msg);
 }
 
@@ -2778,7 +2832,7 @@ void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
     mavlink_logging_data_t log;
     mavlink_msg_logging_data_decode(&message, &log);
     emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
-        log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
+                        log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
 }
 
 void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
@@ -2787,7 +2841,7 @@ void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
     mavlink_msg_logging_data_acked_decode(&message, &log);
     _ackMavlinkLogData(log.sequence);
     emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
-        log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
+                        log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
 }
 
 void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
@@ -2946,7 +3000,7 @@ QString Vehicle::hobbsMeter()
     static const char* HOOBS_LO = "LND_FLIGHT_T_LO";
     //-- TODO: Does this exist on non PX4?
     if (_parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_HI) &&
-        _parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) {
+            _parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) {
         Fact* factHi = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_HI);
         Fact* factLo = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_LO);
         uint64_t hobbsTimeSeconds = ((uint64_t)factHi->rawValue().toUInt() << 32 | (uint64_t)factLo->rawValue().toUInt()) / 1000000;

src/Vehicle/Vehicle.h

@@ -915,6 +915,7 @@ private:
     void _handleScaledPressure(mavlink_message_t& message);
     void _handleScaledPressure2(mavlink_message_t& message);
     void _handleScaledPressure3(mavlink_message_t& message);
+    void _handleHighLatency2(mavlink_message_t& message);
     // ArduPilot dialect messages
 #if !defined(NO_ARDUPILOT_DIALECT)
     void _handleCameraFeedback(const mavlink_message_t& message);

src/VehicleSetup/SetupView.qml

@@ -299,7 +299,9 @@ Rectangle {
             SubMenuButton {
                 setupIndicator:     false
                 exclusiveGroup:     setupButtonGroup
-                visible:            QGroundControl.multiVehicleManager && QGroundControl.multiVehicleManager.parameterReadyVehicleAvailable && _corePlugin.showAdvancedUI
+                visible:            QGroundControl.multiVehicleManager.parameterReadyVehicleAvailable &&
+                                    !QGroundControl.multiVehicleManager.activeVehicle.highLatencyLink &&
+                                    _corePlugin.showAdvancedUI
                 text:               qsTr("Parameters")
                 Layout.fillWidth:   true
 

src/comm/MAVLinkProtocol.cc

@@ -267,11 +267,17 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
             }
 
             if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
-                // Start loggin on first heartbeat
                 _startLogging();
                 mavlink_heartbeat_t heartbeat;
                 mavlink_msg_heartbeat_decode(&message, &heartbeat);
-                emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.mavlink_version, heartbeat.autopilot, heartbeat.type);
+                emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.autopilot, heartbeat.type);
+            }
+
+            if (message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY2) {
+                _startLogging();
+                mavlink_high_latency2_t highLatency2;
+                mavlink_msg_high_latency2_decode(&message, &highLatency2);
+                emit vehicleHeartbeatInfo(link, message.sysid, message.compid, highLatency2.autopilot, highLatency2.type);
             }
 
             // Detect if we are talking to an old radio not supporting v2

src/comm/MAVLinkProtocol.h

@@ -137,7 +137,7 @@ protected:
 
 signals:
     /// Heartbeat received on link
-    void vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType);
+    void vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType);
 
     /** @brief Message received and directly copied via signal */
     void messageReceived(LinkInterface* link, mavlink_message_t message);

src/comm/MockLink.cc

@@ -13,7 +13,7 @@
 #include "QGCApplication.h"
 
 #ifdef UNITTEST_BUILD
-    #include "UnitTest.h"
+#include "UnitTest.h"
 #endif
 
 #include <QTimer>
@@ -166,27 +166,35 @@ void MockLink::run(void)
 void MockLink::_run1HzTasks(void)
 {
     if (_mavlinkStarted && _connected) {
-        _sendVibration();
-        _sendADSBVehicles();
-        if (!qgcApp()->runningUnitTests()) {
-            // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
-            _sendRCChannels();
-        }
-        if (_sendHomePositionDelayCount > 0) {
-            // We delay home position for better testing
-            _sendHomePositionDelayCount--;
+        if (_highLatency) {
+            _sendHighLatency2();
         } else {
-            _sendHomePosition();
-        }
-        if (_sendStatusText) {
-            _sendStatusText = false;
-            _sendStatusTextMessages();
+            _sendVibration();
+            _sendADSBVehicles();
+            if (!qgcApp()->runningUnitTests()) {
+                // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
+                _sendRCChannels();
+            }
+            if (_sendHomePositionDelayCount > 0) {
+                // We delay home position for better testing
+                _sendHomePositionDelayCount--;
+            } else {
+                _sendHomePosition();
+            }
+            if (_sendStatusText) {
+                _sendStatusText = false;
+                _sendStatusTextMessages();
+            }
         }
     }
 }
 
 void MockLink::_run10HzTasks(void)
 {
+    if (_highLatency) {
+        return;
+    }
+
     if (_mavlinkStarted && _connected) {
         _sendHeartBeat();
         if (_sendGPSPositionDelayCount > 0) {
@@ -200,6 +208,10 @@ void MockLink::_run10HzTasks(void)
 
 void MockLink::_run500HzTasks(void)
 {
+    if (_highLatency) {
+        return;
+    }
+
     if (_mavlinkStarted && _connected) {
         _paramRequestListWorker();
         _logDownloadWorker();
@@ -296,6 +308,44 @@ void MockLink::_sendHeartBeat(void)
     respondWithMavlinkMessage(msg);
 }
 
+void MockLink::_sendHighLatency2(void)
+{
+    mavlink_message_t   msg;
+
+    mavlink_msg_high_latency2_pack_chan(_vehicleSystemId,
+                                        _vehicleComponentId,
+                                        _mavlinkChannel,
+                                        &msg,
+                                        0,                          // timestamp
+                                        _vehicleType,               // MAV_TYPE
+                                        _firmwareType,              // MAV_AUTOPILOT
+                                        _flightModeEnumValue(),     // flight_mode
+                                        (int32_t)(_vehicleLatitude  * 1E7),
+                                        (int32_t)(_vehicleLongitude * 1E7),
+                                        (int16_t)_vehicleAltitude,
+                                        (int16_t)_vehicleAltitude,  // target_altitude,
+                                        0,                          // heading
+                                        0,                          // target_heading
+                                        0,                          // target_distance
+                                        0,                          // throttle
+                                        0,                          // airspeed
+                                        0,                          // airspeed_sp
+                                        0,                          // groundspeed
+                                        0,                          // windspeed,
+                                        0,                          // wind_heading
+                                        UINT8_MAX,                  // eph not known
+                                        UINT8_MAX,                  // epv not known
+                                        0,                          // temperature_air
+                                        0,                          // climb_rate
+                                        -1,                         // battery, do not use?
+                                        0,                          // wp_num
+                                        0,                          // failure_flags
+                                        0,                          // failsafe
+                                        0, 0, 0);                   // custom0, custom1, custom2
+
+    respondWithMavlinkMessage(msg);
+}
+
 void MockLink::_sendVibration(void)
 {
     mavlink_message_t   msg;
@@ -842,8 +892,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
     case MAV_CMD_USER_3:
         // Test command which returns MAV_RESULT_ACCEPTED on second attempt
         if (firstCmdUser3) {
-           firstCmdUser3 = false;
-           return;
+            firstCmdUser3 = false;
+            return;
         } else {
             firstCmdUser3 = true;
             commandResult = MAV_RESULT_ACCEPTED;
@@ -852,8 +902,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
     case MAV_CMD_USER_4:
         // Test command which returns MAV_RESULT_FAILED on second attempt
         if (firstCmdUser4) {
-           firstCmdUser4 = false;
-           return;
+            firstCmdUser4 = false;
+            return;
         } else {
             firstCmdUser4 = true;
             commandResult = MAV_RESULT_FAILED;
@@ -943,8 +993,8 @@ void MockLink::_sendHomePosition(void)
                                         (int32_t)(_vehicleAltitude * 1000),
                                         0.0f, 0.0f, 0.0f,
                                         &bogus[0],
-                                        0.0f, 0.0f, 0.0f,
-                                        0);
+            0.0f, 0.0f, 0.0f,
+            0);
     respondWithMavlinkMessage(msg);
 }
 
@@ -972,7 +1022,7 @@ void MockLink::_sendGpsRawInt(void)
                                       0,                                    // Altitude uncertainty in meters * 1000 (positive for up).
                                       0,                                    // Speed uncertainty in meters * 1000 (positive for up).
                                       0);                                   // Heading / track uncertainty in degrees * 1e5.
-        respondWithMavlinkMessage(msg);
+    respondWithMavlinkMessage(msg);
 }
 
 void MockLink::_sendStatusTextMessages(void)
@@ -1237,9 +1287,9 @@ void MockLink::_handleLogRequestData(const mavlink_message_t& msg)
     mavlink_msg_log_request_data_decode(&msg, &request);
 
     if (_logDownloadFilename.isEmpty()) {
-        #ifdef UNITTEST_BUILD
+#ifdef UNITTEST_BUILD
         _logDownloadFilename = UnitTest::createRandomFile(_logDownloadFileSize);
-        #endif
+#endif
     }
 
     if (request.id != 0) {
@@ -1320,3 +1370,8 @@ void MockLink::_sendADSBVehicles(void)
 
     respondWithMavlinkMessage(responseMsg);
 }
+
+uint8_t MockLink::_flightModeEnumValue(void)
+{
+    return FLIGHT_MODE_STABILIZED;
+}

src/comm/MockLink.h

@@ -175,6 +175,7 @@ private:
 
     // MockLink methods
     void _sendHeartBeat(void);
+    void _sendHighLatency2(void);
     void _handleIncomingNSHBytes(const char* bytes, int cBytes);
     void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes);
     void _loadParams(void);
@@ -201,6 +202,7 @@ private:
     void _logDownloadWorker(void);
     void _sendADSBVehicles(void);
     void _moveADSBVehicle(void);
+    uint8_t _flightModeEnumValue(void);
 
     static MockLink* _startMockLink(MockConfiguration* mockConfig);