Merge pull request #6072 from DonLakeFlyer/HighLatency2

Add support for HIGH_LATENCY2

src/FactSystem/ParameterManager.cc

@@ -81,7 +81,17 @@ ParameterManager::ParameterManager(Vehicle* vehicle)
     // Ensure the cache directory exists
     QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache");
 
+    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()

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,7 +233,15 @@ 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,
@@ -245,6 +253,7 @@ Vehicle::Vehicle(LinkInterface*             link,
                        MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
                        false,                                   // No error shown if fails
                        1);                                      // Request firmware version
+    }
 
     _firmwarePlugin->initializeVehicle(this);
 
@@ -684,6 +693,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_ADSB_VEHICLE:
         _handleADSBVehicle(message);
         break;
+    case MAVLINK_MSG_ID_HIGH_LATENCY2:
+        _handleHighLatency2(message);
+        break;
 
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
     {
@@ -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;
@@ -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);
@@ -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);

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,6 +166,9 @@ void MockLink::run(void)
 void MockLink::_run1HzTasks(void)
 {
     if (_mavlinkStarted && _connected) {
+        if (_highLatency) {
+            _sendHighLatency2();
+        } else {
             _sendVibration();
             _sendADSBVehicles();
             if (!qgcApp()->runningUnitTests()) {
@@ -183,10 +186,15 @@ void MockLink::_run1HzTasks(void)
                 _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;
@@ -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);