New initial connect state machine which supports COMPONENT_INFORMATION

qgroundcontrol.pro

@@ -674,6 +674,7 @@ HEADERS += \
     src/SHPFileHelper.h \
     src/Terrain/TerrainQuery.h \
     src/TerrainTile.h \
+    src/Vehicle/ComponentInformation.h \
     src/Vehicle/GPSRTKFactGroup.h \
     src/Vehicle/MAVLinkLogManager.h \
     src/Vehicle/MultiVehicleManager.h \
@@ -886,6 +887,7 @@ SOURCES += \
     src/SHPFileHelper.cc \
     src/Terrain/TerrainQuery.cc \
     src/TerrainTile.cc\
+    src/Vehicle/ComponentInformation.cc \
     src/Vehicle/GPSRTKFactGroup.cc \
     src/Vehicle/MAVLinkLogManager.cc \
     src/Vehicle/MultiVehicleManager.cc \

qgroundcontrol.qrc

@@ -336,5 +336,6 @@
     <qresource prefix="/MockLink">
         <file alias="APMArduSubMockLink.params">src/comm/APMArduSubMockLink.params</file>
         <file alias="PX4MockLink.params">src/comm/PX4MockLink.params</file>
+        <file alias="Version.MetaData.json">src/comm/MockLink.Version.MetaData.json</file>
     </qresource>
 </RCC>

src/FactSystem/ParameterManager.cc

@@ -112,18 +112,6 @@ 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 if (!_logReplay){
-        refreshAllParameters();
-    }
 }
 
 ParameterManager::~ParameterManager()
@@ -494,8 +482,14 @@ void ParameterManager::_valueUpdated(const QVariant& value)
 
 void ParameterManager::refreshAllParameters(uint8_t componentId)
 {
-    if (_logReplay) {
-        return;
+    if (_vehicle->highLatencyLink() || _logReplay) {
+        // These links don't load params
+        _parametersReady = true;
+        _missingParameters = true;
+        _initialLoadComplete = true;
+        _waitingForDefaultComponent = false;
+        emit parametersReadyChanged(_parametersReady);
+        emit missingParametersChanged(_missingParameters);
     }
 
     _dataMutex.lock();

src/Vehicle/ComponentInformation.cc

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "ComponentInformation.h"
+#include "Vehicle.h"
+
+QGC_LOGGING_CATEGORY(ComponentInformation, "ComponentInformation")
+
+ComponentInformation::ComponentInformation(Vehicle* vehicle, QObject* parent)
+    : QObject   (parent)
+    , _vehicle  (vehicle)
+{
+
+}
+
+void ComponentInformation::requestVersionMetaData(Vehicle* vehicle)
+{
+    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+                            MAV_CMD_REQUEST_MESSAGE,
+                            false,                                   // No error shown if fails
+                            MAVLINK_MSG_ID_COMPONENT_INFORMATION,
+                            COMP_METADATA_TYPE_VERSION);
+}
+
+bool ComponentInformation::requestParameterMetaData(Vehicle* vehicle)
+{
+    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+                            MAV_CMD_REQUEST_MESSAGE,
+                            false,                                   // No error shown if fails
+                            MAVLINK_MSG_ID_COMPONENT_INFORMATION,
+                            COMP_METADATA_TYPE_PARAMETER);
+    return true;
+}
+
+void ComponentInformation::componentInformationReceived(const mavlink_message_t &message)
+{
+    mavlink_component_information_t componentInformation;
+    mavlink_msg_component_information_decode(&message, &componentInformation);    
+    qDebug() << componentInformation.metadata_type << componentInformation.metadata_uri;
+}
+
+bool ComponentInformation::metaDataTypeSupported(COMP_METADATA_TYPE type)
+{
+    return _supportedMetaDataTypes.contains(type);
+}

src/Vehicle/ComponentInformation.h

@@ -0,0 +1,38 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "QGCLoggingCategory.h"
+#include "QGCMAVLink.h"
+
+#include <QObject>
+
+Q_DECLARE_LOGGING_CATEGORY(ComponentInformationLog)
+
+class Vehicle;
+
+class ComponentInformation : public QObject
+{
+    Q_OBJECT
+
+public:
+    ComponentInformation(Vehicle* vehicle, QObject* parent = nullptr);
+
+    void requestVersionMetaData         (Vehicle* vehicle);
+    bool requestParameterMetaData       (Vehicle* vehicle);
+    void componentInformationReceived   (const mavlink_message_t& message);
+    bool metaDataTypeSupported          (COMP_METADATA_TYPE type);
+
+private:
+    Vehicle*                    _vehicle =                  nullptr;
+    bool                        _versionMetaDataAvailable = false;
+    bool                        _paramMetaDataAvailable =   false;
+    QList<COMP_METADATA_TYPE>   _supportedMetaDataTypes;
+};

src/Vehicle/Vehicle.h

@@ -24,6 +24,7 @@
 #include "SettingsFact.h"
 #include "QGCMapCircle.h"
 #include "TerrainFactGroup.h"
+#include "ComponentInformation.h"
 
 class UAS;
 class UASInterface;
@@ -1011,6 +1012,8 @@ public:
 
     bool containsLink(LinkInterface* link) { return _links.contains(link); }
 
+    typedef void (*MavCmdResultHandler)(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+
     /// Sends the specified MAV_CMD to the vehicle. If no Ack is received command will be retried. If a sendMavCommand is already in progress
     /// the command will be queued and sent when the previous command completes.
     ///     @param component Component to send to
@@ -1018,6 +1021,7 @@ public:
     ///     @param showError true: Display error to user if command failed, false:  no error shown
     /// Signals: mavCommandResult on success or failure
     void sendMavCommand(int component, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
+    void sendMavCommand(int component, MAV_CMD command, MavCmdResultHandler errorHandler, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
     void sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7);
 
     /// Same as sendMavCommand but available from Qml.
@@ -1232,6 +1236,7 @@ signals:
     void gimbalYawChanged               ();
     void gimbalDataChanged              ();
     void isROIEnabledChanged            ();
+    void initialConnectComplete         ();
 
 private slots:
     void _mavlinkMessageReceived        (LinkInterface* link, mavlink_message_t message);
@@ -1250,9 +1255,9 @@ private slots:
     /** @brief A new camera image has arrived */
     void _imageReady                    (UASInterface* uas);
     void _prearmErrorTimeout            ();
-    void _missionLoadComplete           ();
-    void _geoFenceLoadComplete          ();
-    void _rallyPointLoadComplete        ();
+    void _firstMissionLoadComplete           ();
+    void _firstGeoFenceLoadComplete          ();
+    void _firstRallyPointLoadComplete        ();
     void _sendMavCommandAgain           ();
     void _clearCameraTriggerPoints      ();
     void _updateDistanceHeadingToHome   ();
@@ -1266,7 +1271,6 @@ private slots:
 
     void _trafficUpdate                 (bool alert, QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
     void _orbitTelemetryTimeout         ();
-    void _protocolVersionTimeOut        ();
     void _updateFlightTime              ();
 
 private:
@@ -1289,8 +1293,6 @@ private:
     void _handleExtendedSysState        (mavlink_message_t& message);
     void _handleCommandAck              (mavlink_message_t& message);
     void _handleCommandLong             (mavlink_message_t& message);
-    void _handleAutopilotVersion        (LinkInterface* link, mavlink_message_t& message);
-    void _handleProtocolVersion         (LinkInterface* link, mavlink_message_t& message);
     void _handleGpsRawInt               (mavlink_message_t& message);
     void _handleGlobalPositionInt       (mavlink_message_t& message);
     void _handleAltitude                (mavlink_message_t& message);
@@ -1329,14 +1331,11 @@ private:
     void _sendNextQueuedMavCommand      ();
     void _updatePriorityLink            (bool updateActive, bool sendCommand);
     void _commonInit                    ();
-    void _startPlanRequest              ();
     void _setupAutoDisarmSignalling     ();
     void _setCapabilities               (uint64_t capabilityBits);
     void _updateArmed                   (bool armed);
     bool _apmArmingNotRequired          ();
     void _pidTuningAdjustRates          (bool setRatesForTuning);
-    void _handleUnsupportedRequestAutopilotCapabilities();
-    void _handleUnsupportedRequestProtocolVersion();
     void _initializeCsv                 ();
     void _writeCsvLine                  ();
     void _flightTimerStart              ();
@@ -1416,19 +1415,18 @@ private:
     QGCCameraManager* _cameras;
 
     typedef struct {
-        int         component;
-        bool        commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG
-        MAV_CMD     command;
-        MAV_FRAME   frame;
-        double      rgParam[7];
-        bool        showError;
+        int                 component;
+        bool                commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG
+        MAV_CMD             command;
+        MAV_FRAME           frame;
+        double              rgParam[7];
+        bool                showError;
+        MavCmdResultHandler resultHandler;
     } MavCommandQueueEntry_t;
 
     QList<MavCommandQueueEntry_t>   _mavCommandQueue;
     QTimer                          _mavCommandAckTimer;
     int                             _mavCommandRetryCount;
-    int                             _capabilitiesRetryCount =               0;
-    QElapsedTimer                   _capabilitiesRetryElapsed;
     static const int                _mavCommandMaxRetryCount =              3;
     static const int                _mavCommandAckTimeoutMSecs =            3000;
     static const int                _mavCommandAckTimeoutMSecsHighLatency = 120000;
@@ -1444,13 +1442,8 @@ private:
     bool                _initialPlanRequestComplete;
 
     MissionManager*     _missionManager;
-    bool                _missionManagerInitialRequestSent;
-
     GeoFenceManager*    _geoFenceManager;
-    bool                _geoFenceManagerInitialRequestSent;
-
     RallyPointManager*  _rallyPointManager;
-    bool                _rallyPointManagerInitialRequestSent;
 
     ParameterManager*       _parameterManager   = nullptr;
     VehicleObjectAvoidance* _objectAvoidance    = nullptr;
@@ -1551,6 +1544,54 @@ private:
     QMap<uint8_t /* compId */, ChunkedStatusTextInfo_t> _chunkedStatusTextInfoMap;
     QTimer _chunkedStatusTextTimer;
 
+    ComponentInformation _componentInformation;
+
+    typedef void (*WaitForMavlinkMessageTimeoutHandler)(Vehicle* vehicle);
+    typedef void (*WaitForMavlinkMessageMessageHandler)(Vehicle* vehicle, const mavlink_message_t& message);
+
+    /// Waits for the specified msecs for the message to be received. Calls timeoutHandler if not received.
+    void _waitForMavlinkMessage     (int messageId, int timeoutMsecs, WaitForMavlinkMessageMessageHandler messageHandler, WaitForMavlinkMessageTimeoutHandler timeoutHandler);
+    void _waitForMavlinkMessageClear(void);
+
+    int                                 _waitForMavlinkMessageId =              0;
+    int                                 _waitForMavlinkMessageTimeoutMsecs =    0;
+    QElapsedTimer                       _waitForMavlinkMessageElapsed;
+    WaitForMavlinkMessageTimeoutHandler _waitForMavlinkMessageTimeoutHandler =  nullptr;
+    WaitForMavlinkMessageMessageHandler _waitForMavlinkMessageMessageHandler =  nullptr;
+
+    // Initial connection state machine
+    typedef void (*StateFn)(Vehicle* vehicle);
+    static const int _cInitialConnectStateEntries = 9;
+    static const StateFn _rgInitialConnectStates[_cInitialConnectStateEntries];
+    int _currentInitialConnectState = -1;
+    void _advanceInitialConnectStateMachine(void);
+    void _advanceInitialConnectStateMachine(StateFn stateFn);
+
+    static void _stateRequestCapabilities           (Vehicle* vehicle);
+    static void _stateRequestProtocolVersion        (Vehicle* vehicle);
+    static void _stateRequestCompInfoVersion        (Vehicle* vehicle);
+    static void _stateRequestCompInfoParam          (Vehicle* vehicle);
+    static void _stateRequestParameters             (Vehicle* vehicle);
+    static void _stateRequestMission                (Vehicle* vehicle);
+    static void _stateRequestGeoFence               (Vehicle* vehicle);
+    static void _stateRequestRallyPoints            (Vehicle* vehicle);
+    static void _stateSignalInitialConnectComplete  (Vehicle* vehicle);
+
+    static void _capabilitiesCmdResultHandler   (Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+    static void _protocolVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+    static void _compInfoVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+    static void _compInfoParamCmdResultHandler  (Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+
+    static void _waitForAutopilotVersionMessageHandler  (Vehicle* vehicle, const mavlink_message_t& message);
+    static void _waitForProtocolVersionMessageHandler   (Vehicle* vehicle, const mavlink_message_t& message);
+    static void _waitForCompInfoVersionMessageHandler   (Vehicle* vehicle, const mavlink_message_t& message);
+    static void _waitForCompInfoParamMessageHandler     (Vehicle* vehicle, const mavlink_message_t& message);
+
+    static void _waitForAutopilotVersionTimeoutHandler  (Vehicle* vehicle);
+    static void _waitForProtocolVersionTimeoutHandler   (Vehicle* vehicle);
+    static void _waitForCompInfoVersionTimeoutHandler   (Vehicle* vehicle);
+    static void _waitForCompInfoParamTimeoutHandler     (Vehicle* vehicle);
+
     // FactGroup facts
 
     Fact _rollFact;

src/comm/MockLink.Version.MetaData.json

@@ -0,0 +1,8 @@
+{
+    "version":                      1,
+    "vendorName":                   "QGC MockLink Vendor",
+    "modelName":                    "QGC MockLink Model",
+    "firmwareVersion":              "1.0.0",
+    "hardwareVersion":              "1.0.0",
+    "supportedCompMetadataTypes":   [ 0, 1 ]
+} 

src/comm/MockLink.cc

@@ -476,47 +476,36 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
         case MAVLINK_MSG_ID_HEARTBEAT:
             _handleHeartBeat(msg);
             break;
-
         case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
             _handleParamRequestList(msg);
             break;
-
         case MAVLINK_MSG_ID_SET_MODE:
             _handleSetMode(msg);
             break;
-
         case MAVLINK_MSG_ID_PARAM_SET:
             _handleParamSet(msg);
             break;
-
         case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
             _handleParamRequestRead(msg);
             break;
-
         case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
             _handleFTP(msg);
             break;
-
         case MAVLINK_MSG_ID_COMMAND_LONG:
             _handleCommandLong(msg);
             break;
-
         case MAVLINK_MSG_ID_MANUAL_CONTROL:
             _handleManualControl(msg);
             break;
-
         case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
             _handleLogRequestList(msg);
             break;
-
         case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
             _handleLogRequestData(msg);
             break;
-
         case MAVLINK_MSG_ID_PARAM_MAP_RC:
             _handleParamMapRC(msg);
             break;
-
         default:
             break;
         }
@@ -945,6 +934,11 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
         commandResult = MAV_RESULT_ACCEPTED;
         _respondWithAutopilotVersion();
         break;
+    case MAV_CMD_REQUEST_MESSAGE:
+        if (request.param1 == MAVLINK_MSG_ID_COMPONENT_INFORMATION && _handleRequestMessage(request)) {
+            commandResult = MAV_RESULT_ACCEPTED;
+        }
+        break;
     case MAV_CMD_USER_1:
         // Test command which always returns MAV_RESULT_ACCEPTED
         commandResult = MAV_RESULT_ACCEPTED;
@@ -1466,3 +1460,59 @@ void MockLink::_sendADSBVehicles(void)
 
     respondWithMavlinkMessage(responseMsg);
 }
+
+bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request)
+{
+    if (request.param1 != MAVLINK_MSG_ID_COMPONENT_INFORMATION) {
+        return false;
+    }
+
+    switch (static_cast<int>(request.param2)) {
+    case COMP_METADATA_TYPE_VERSION:
+        _sendVersionMetaData();
+        return true;
+    case COMP_METADATA_TYPE_PARAMETER:
+        _sendParameterMetaData();
+        return true;
+    }
+
+    return false;
+}
+
+void MockLink::_sendVersionMetaData(void)
+{
+    mavlink_message_t   responseMsg;
+    char                metaDataURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN] =       "mavlinkftp://version.json";
+    char                translationURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN] = "";
+
+    mavlink_msg_component_information_pack_chan(_vehicleSystemId,
+                                                _vehicleComponentId,
+                                                _mavlinkChannel,
+                                                &responseMsg,
+                                                0,                          // time_boot_ms
+                                                COMP_METADATA_TYPE_VERSION,
+                                                1,                          // comp_metadata_uid
+                                                metaDataURI,
+                                                0,                          // comp_translation_uid
+                                                translationURI);
+    respondWithMavlinkMessage(responseMsg);
+}
+
+void MockLink::_sendParameterMetaData(void)
+{
+    mavlink_message_t   responseMsg;
+    char                metaDataURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN] =       "mavlinkftp://parameter.json";
+    char                translationURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN] = "";
+
+    mavlink_msg_component_information_pack_chan(_vehicleSystemId,
+                                                _vehicleComponentId,
+                                                _mavlinkChannel,
+                                                &responseMsg,
+                                                0,                              // time_boot_ms
+                                                COMP_METADATA_TYPE_PARAMETER,
+                                                1,                              // comp_metadata_uid
+                                                metaDataURI,
+                                                0,                              // comp_translation_uid
+                                                translationURI);
+    respondWithMavlinkMessage(responseMsg);
+}

src/comm/MockLink.h

@@ -198,6 +198,7 @@ private:
     void _handleLogRequestList          (const mavlink_message_t& msg);
     void _handleLogRequestData          (const mavlink_message_t& msg);
     void _handleParamMapRC              (const mavlink_message_t& msg);
+    bool _handleRequestMessage          (const mavlink_command_long_t& request);
     float _floatUnionForParam           (int componentId, const QString& paramName);
     void _setParamFloatUnionIntoMap     (int componentId, const QString& paramName, float paramFloat);
     void _sendHomePosition              (void);
@@ -212,6 +213,8 @@ private:
     void _logDownloadWorker             (void);
     void _sendADSBVehicles              (void);
     void _moveADSBVehicle               (void);
+    void _sendVersionMetaData           (void);
+    void _sendParameterMetaData         (void);
 
     static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode);
     static MockLink* _startMockLink(MockConfiguration* mockConfig);

src/qgcunittest/UnitTest.cc

@@ -401,11 +401,9 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot)
     _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
     QVERIFY(_vehicle);
 
-    // Wait for plan request to complete
-    if (!_vehicle->initialPlanRequestComplete()) {
-        QSignalSpy spyPlan(_vehicle, SIGNAL(initialPlanRequestCompleteChanged(bool)));
-        QCOMPARE(spyPlan.wait(10000), true);
-    }
+    // Wait for initial connect sequence to complete
+    QSignalSpy spyPlan(_vehicle, SIGNAL(initialConnectComplete()));
+    QCOMPARE(spyPlan.wait(10000), true);
 }
 
 void UnitTest::_disconnectMockLink(void)