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.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);