Merge branch 'master' of https://bdgit.educoder.net/pxsi3jbgc/wwcs-1314

src/Camera/QGCCameraControl.h

@@ -0,0 +1,422 @@
+
+
+#pragma once
+
+#include "QGCApplication.h"
+#include <QLoggingCategory>
+
+class QDomNode;
+class QDomNodeList;
+class QGCCameraParamIO;
+
+Q_DECLARE_LOGGING_CATEGORY(CameraControlLog)
+Q_DECLARE_LOGGING_CATEGORY(CameraControlVerboseLog)
+
+//-----------------------------------------------------------------------------
+/// Video Stream Info
+/// Encapsulates the contents of a [VIDEO_STREAM_INFORMATION](https://mavlink.io/en/messages/common.html#VIDEO_STREAM_INFORMATION) message
+class QGCVideoStreamInfo : public QObject
+{
+    Q_OBJECT
+public:
+    QGCVideoStreamInfo(QObject* parent, const mavlink_video_stream_information_t* si);
+
+    Q_PROPERTY(QString      uri                 READ uri                NOTIFY infoChanged)
+    Q_PROPERTY(QString      name                READ name               NOTIFY infoChanged)
+    Q_PROPERTY(int          streamID            READ streamID           NOTIFY infoChanged)
+    Q_PROPERTY(int          type                READ type               NOTIFY infoChanged)
+    Q_PROPERTY(qreal        aspectRatio         READ aspectRatio        NOTIFY infoChanged)
+    Q_PROPERTY(qreal        hfov                READ hfov               NOTIFY infoChanged)
+    Q_PROPERTY(bool         isThermal           READ isThermal          NOTIFY infoChanged)
+
+    QString uri             () { return QString(_streamInfo.uri);  }
+    QString name            () { return QString(_streamInfo.name); }
+    qreal   aspectRatio     () const;
+    qreal   hfov            () const{ return _streamInfo.hfov; }
+    int     type            () const{ return _streamInfo.type; }
+    int     streamID        () const{ return _streamInfo.stream_id; }
+    bool    isThermal       () const{ return _streamInfo.flags & VIDEO_STREAM_STATUS_FLAGS_THERMAL; }
+
+    bool    update          (const mavlink_video_stream_status_t* vs);
+
+signals:
+    void    infoChanged     ();
+
+private:
+    mavlink_video_stream_information_t _streamInfo;
+};
+
+//-----------------------------------------------------------------------------
+/// Camera option exclusions
+class QGCCameraOptionExclusion : public QObject
+{
+public:
+    QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_);
+    QString param;
+    QString value;
+    QStringList exclusions;
+};
+
+//-----------------------------------------------------------------------------
+/// Camera option ranges
+class QGCCameraOptionRange : public QObject
+{
+public:
+    QGCCameraOptionRange(QObject* parent, QString param_, QString value_, QString targetParam_, QString condition_, QStringList optNames_, QStringList optValues_);
+    QString param;
+    QString value;
+    QString targetParam;
+    QString condition;
+    QStringList  optNames;
+    QStringList  optValues;
+    QVariantList optVariants;
+};
+
+//-----------------------------------------------------------------------------
+/// MAVLink Camera API controller
+class QGCCameraControl : public FactGroup
+{
+    Q_OBJECT
+    friend class QGCCameraParamIO;
+public:
+    QGCCameraControl(const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = nullptr);
+    virtual ~QGCCameraControl();
+
+    //-- cam_mode
+    enum CameraMode {
+        CAM_MODE_UNDEFINED = -1,
+        CAM_MODE_PHOTO  = 0,
+        CAM_MODE_VIDEO  = 1,
+        CAM_MODE_SURVEY = 2,
+    };
+
+    //-- Video Capture Status
+    enum VideoStatus {
+        VIDEO_CAPTURE_STATUS_STOPPED = 0,
+        VIDEO_CAPTURE_STATUS_RUNNING,
+        VIDEO_CAPTURE_STATUS_LAST,
+        VIDEO_CAPTURE_STATUS_UNDEFINED = 255
+    };
+
+    //-- Photo Capture Status
+    enum PhotoStatus {
+        PHOTO_CAPTURE_IDLE = 0,
+        PHOTO_CAPTURE_IN_PROGRESS,
+        PHOTO_CAPTURE_INTERVAL_IDLE,
+        PHOTO_CAPTURE_INTERVAL_IN_PROGRESS,
+        PHOTO_CAPTURE_LAST,
+        PHOTO_CAPTURE_STATUS_UNDEFINED = 255
+    };
+
+    //-- Photo Capture Modes
+    enum PhotoMode {
+        PHOTO_CAPTURE_SINGLE = 0,
+        PHOTO_CAPTURE_TIMELAPSE,
+    };
+
+    //-- Storage Status
+    enum StorageStatus {
+        STORAGE_EMPTY = STORAGE_STATUS_EMPTY,
+        STORAGE_UNFORMATTED = STORAGE_STATUS_UNFORMATTED,
+        STORAGE_READY = STORAGE_STATUS_READY,
+        STORAGE_NOT_SUPPORTED = STORAGE_STATUS_NOT_SUPPORTED
+    };
+
+    enum ThermalViewMode {
+        THERMAL_OFF = 0,
+        THERMAL_BLEND,
+        THERMAL_FULL,
+        THERMAL_PIP,
+    };
+
+    Q_ENUM(CameraMode)
+    Q_ENUM(VideoStatus)
+    Q_ENUM(PhotoStatus)
+    Q_ENUM(PhotoMode)
+    Q_ENUM(StorageStatus)
+    Q_ENUM(ThermalViewMode)
+
+    Q_PROPERTY(int          version             READ version            NOTIFY infoChanged)
+    Q_PROPERTY(QString      modelName           READ modelName          NOTIFY infoChanged)
+    Q_PROPERTY(QString      vendor              READ vendor             NOTIFY infoChanged)
+    Q_PROPERTY(QString      firmwareVersion     READ firmwareVersion    NOTIFY infoChanged)
+    Q_PROPERTY(qreal        focalLength         READ focalLength        NOTIFY infoChanged)
+    Q_PROPERTY(QSizeF       sensorSize          READ sensorSize         NOTIFY infoChanged)
+    Q_PROPERTY(QSize        resolution          READ resolution         NOTIFY infoChanged)
+    Q_PROPERTY(bool         capturesVideo       READ capturesVideo      NOTIFY infoChanged)
+    Q_PROPERTY(bool         capturesPhotos      READ capturesPhotos     NOTIFY infoChanged)
+    Q_PROPERTY(bool         hasModes            READ hasModes           NOTIFY infoChanged)
+    Q_PROPERTY(bool         hasZoom             READ hasZoom            NOTIFY infoChanged)
+    Q_PROPERTY(bool         hasFocus            READ hasFocus           NOTIFY infoChanged)
+    Q_PROPERTY(bool         hasVideoStream      READ hasVideoStream     NOTIFY infoChanged)
+    Q_PROPERTY(bool         photosInVideoMode   READ photosInVideoMode  NOTIFY infoChanged)
+    Q_PROPERTY(bool         videoInPhotoMode    READ videoInPhotoMode   NOTIFY infoChanged)
+    Q_PROPERTY(bool         isBasic             READ isBasic            NOTIFY infoChanged)
+    Q_PROPERTY(quint32      storageFree         READ storageFree        NOTIFY storageFreeChanged)
+    Q_PROPERTY(QString      storageFreeStr      READ storageFreeStr     NOTIFY storageFreeChanged)
+    Q_PROPERTY(quint32      storageTotal        READ storageTotal       NOTIFY storageTotalChanged)
+    Q_PROPERTY(int          batteryRemaining    READ batteryRemaining       NOTIFY batteryRemainingChanged)
+    Q_PROPERTY(QString      batteryRemainingStr READ batteryRemainingStr    NOTIFY batteryRemainingChanged)
+    Q_PROPERTY(bool         paramComplete       READ paramComplete      NOTIFY parametersReady)
+
+    Q_PROPERTY(qreal        zoomLevel           READ zoomLevel          WRITE  setZoomLevel         NOTIFY zoomLevelChanged)
+    Q_PROPERTY(qreal        focusLevel          READ focusLevel         WRITE  setFocusLevel        NOTIFY focusLevelChanged)
+
+    Q_PROPERTY(Fact*        exposureMode        READ exposureMode       NOTIFY parametersReady)
+    Q_PROPERTY(Fact*        ev                  READ ev                 NOTIFY parametersReady)
+    Q_PROPERTY(Fact*        iso                 READ iso                NOTIFY parametersReady)
+    Q_PROPERTY(Fact*        shutterSpeed        READ shutterSpeed       NOTIFY parametersReady)
+    Q_PROPERTY(Fact*        aperture            READ aperture           NOTIFY parametersReady)
+    Q_PROPERTY(Fact*        wb                  READ wb                 NOTIFY parametersReady)
+    Q_PROPERTY(Fact*        mode                READ mode               NOTIFY parametersReady)
+
+    Q_PROPERTY(QStringList  activeSettings      READ activeSettings                                 NOTIFY activeSettingsChanged)
+    Q_PROPERTY(VideoStatus  videoStatus         READ videoStatus                                    NOTIFY videoStatusChanged)
+    Q_PROPERTY(PhotoStatus  photoStatus         READ photoStatus                                    NOTIFY photoStatusChanged)
+    Q_PROPERTY(CameraMode   cameraMode          READ cameraMode         WRITE   setCameraMode       NOTIFY cameraModeChanged)
+    Q_PROPERTY(StorageStatus storageStatus      READ storageStatus                                  NOTIFY storageStatusChanged)
+    Q_PROPERTY(qreal        photoLapse          READ photoLapse         WRITE   setPhotoLapse       NOTIFY photoLapseChanged)
+    Q_PROPERTY(int          photoLapseCount     READ photoLapseCount    WRITE   setPhotoLapseCount  NOTIFY photoLapseCountChanged)
+    Q_PROPERTY(PhotoMode    photoMode           READ photoMode          WRITE   setPhotoMode        NOTIFY photoModeChanged)
+    Q_PROPERTY(int          currentStream       READ currentStream      WRITE   setCurrentStream    NOTIFY currentStreamChanged)
+    Q_PROPERTY(bool         autoStream          READ autoStream                                     NOTIFY autoStreamChanged)
+    Q_PROPERTY(QmlObjectListModel* streams      READ streams                                        NOTIFY streamsChanged)
+    Q_PROPERTY(QGCVideoStreamInfo* currentStreamInstance READ currentStreamInstance                 NOTIFY currentStreamChanged)
+    Q_PROPERTY(QGCVideoStreamInfo* thermalStreamInstance READ thermalStreamInstance                 NOTIFY thermalStreamChanged)
+    Q_PROPERTY(quint32      recordTime          READ recordTime                                     NOTIFY recordTimeChanged)
+    Q_PROPERTY(QString      recordTimeStr       READ recordTimeStr                                  NOTIFY recordTimeChanged)
+    Q_PROPERTY(QStringList  streamLabels        READ streamLabels                                   NOTIFY streamLabelsChanged)
+    Q_PROPERTY(ThermalViewMode thermalMode      READ thermalMode        WRITE  setThermalMode       NOTIFY thermalModeChanged)
+    Q_PROPERTY(double       thermalOpacity      READ thermalOpacity     WRITE  setThermalOpacity    NOTIFY thermalOpacityChanged)
+
+    Q_INVOKABLE virtual void setVideoMode   ();
+    Q_INVOKABLE virtual void setPhotoMode   ();
+    Q_INVOKABLE virtual void toggleMode     ();
+    Q_INVOKABLE virtual bool takePhoto      ();
+    Q_INVOKABLE virtual bool stopTakePhoto  ();
+    Q_INVOKABLE virtual bool startVideo     ();
+    Q_INVOKABLE virtual bool stopVideo      ();
+    Q_INVOKABLE virtual bool toggleVideo    ();
+    Q_INVOKABLE virtual void resetSettings  ();
+    Q_INVOKABLE virtual void formatCard     (int id = 1);
+    Q_INVOKABLE virtual void stepZoom       (int direction);
+    Q_INVOKABLE virtual void startZoom      (int direction);
+    Q_INVOKABLE virtual void stopZoom       ();
+    Q_INVOKABLE virtual void stopStream     ();
+    Q_INVOKABLE virtual void resumeStream   ();
+
+    virtual int         version             () { return _version; }
+    virtual QString     modelName           () { return _modelName; }
+    virtual QString     vendor              () { return _vendor; }
+    virtual QString     firmwareVersion     ();
+    virtual qreal       focalLength         () { return static_cast<qreal>(_info.focal_length); }
+    virtual QSizeF      sensorSize          () { return QSizeF(static_cast<qreal>(_info.sensor_size_h), static_cast<qreal>(_info.sensor_size_v)); }
+    virtual QSize       resolution          () { return QSize(_info.resolution_h, _info.resolution_v); }
+    virtual bool        capturesVideo       () { return _info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO; }
+    virtual bool        capturesPhotos      () { return _info.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE; }
+    virtual bool        hasModes            () { return _info.flags & CAMERA_CAP_FLAGS_HAS_MODES; }
+    virtual bool        hasZoom             () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM; }
+    virtual bool        hasFocus            () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS; }
+    virtual bool        hasVideoStream      () { return _info.flags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM; }
+    virtual bool        photosInVideoMode   () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE; }
+    virtual bool        videoInPhotoMode    () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE; }
+
+    virtual int         compID              () { return _compID; }
+    virtual bool        isBasic             () { return _settings.size() == 0; }
+    virtual VideoStatus videoStatus         ();
+    virtual PhotoStatus photoStatus         ();
+    virtual PhotoMode   photoMode           () { return _photoMode; }
+    virtual qreal       photoLapse          () { return _photoLapse; }
+    virtual int         photoLapseCount     () { return _photoLapseCount; }
+    virtual CameraMode  cameraMode          () { return _cameraMode; }
+    virtual StorageStatus storageStatus     () { return _storageStatus; }
+    virtual QStringList activeSettings      ();
+    virtual quint32     storageFree         () { return _storageFree;  }
+    virtual QString     storageFreeStr      ();
+    virtual quint32     storageTotal        () { return _storageTotal; }
+    virtual int         batteryRemaining    () { return _batteryRemaining; }
+    virtual QString     batteryRemainingStr ();
+    virtual bool        paramComplete       () { return _paramComplete; }
+    virtual qreal       zoomLevel           () { return _zoomLevel; }
+    virtual qreal       focusLevel          () { return _focusLevel; }
+
+    virtual QmlObjectListModel* streams     () { return &_streams; }
+    virtual QGCVideoStreamInfo* currentStreamInstance();
+    virtual QGCVideoStreamInfo* thermalStreamInstance();
+    virtual int          currentStream      () { return _currentStream; }
+    virtual void         setCurrentStream   (int stream);
+    virtual bool         autoStream         ();
+    virtual quint32      recordTime         () { return _recordTime; }
+    virtual QString      recordTimeStr      ();
+
+    virtual Fact*       exposureMode        ();
+    virtual Fact*       ev                  ();
+    virtual Fact*       iso                 ();
+    virtual Fact*       shutterSpeed        ();
+    virtual Fact*       aperture            ();
+    virtual Fact*       wb                  ();
+    virtual Fact*       mode                ();
+
+    /// Stream names to show the user (for selection)
+    virtual QStringList streamLabels        () { return _streamLabels; }
+
+    virtual ThermalViewMode thermalMode     () { return _thermalMode; }
+    virtual void        setThermalMode      (ThermalViewMode mode);
+    virtual double      thermalOpacity      () { return _thermalOpacity; }
+    virtual void        setThermalOpacity   (double val);
+
+    virtual void        setZoomLevel        (qreal level);
+    virtual void        setFocusLevel       (qreal level);
+    virtual void        setCameraMode       (CameraMode mode);
+    virtual void        setPhotoMode        (PhotoMode mode);
+    virtual void        setPhotoLapse       (qreal interval);
+    virtual void        setPhotoLapseCount  (int count);
+
+    virtual void        handleSettings      (const mavlink_camera_settings_t& settings);
+    virtual void        handleCaptureStatus (const mavlink_camera_capture_status_t& capStatus);
+    virtual void        handleParamAck      (const mavlink_param_ext_ack_t& ack);
+    virtual void        handleParamValue    (const mavlink_param_ext_value_t& value);
+    virtual void        handleStorageInfo   (const mavlink_storage_information_t& st);
+    virtual void        handleBatteryStatus (const mavlink_battery_status_t& bs);
+    virtual void        handleVideoInfo     (const mavlink_video_stream_information_t *vi);
+    virtual void        handleVideoStatus   (const mavlink_video_stream_status_t *vs);
+
+    /// Notify controller a parameter has changed
+    virtual void        factChanged         (Fact* pFact);
+    /// Allow controller to modify or invalidate incoming parameter
+    virtual bool        incomingParameter   (Fact* pFact, QVariant& newValue);
+    /// Allow controller to modify or invalidate parameter change
+    virtual bool        validateParameter   (Fact* pFact, QVariant& newValue);
+
+    // Known Parameters
+    static const char* kCAM_EV;
+    static const char* kCAM_EXPMODE;
+    static const char* kCAM_ISO;
+    static const char* kCAM_SHUTTERSPD;
+    static const char* kCAM_APERTURE;
+    static const char* kCAM_WBMODE;
+    static const char* kCAM_MODE;
+
+signals:
+    void    infoChanged                     ();
+    void    videoStatusChanged              ();
+    void    photoStatusChanged              ();
+    void    photoModeChanged                ();
+    void    photoLapseChanged               ();
+    void    photoLapseCountChanged          ();
+    void    cameraModeChanged               ();
+    void    activeSettingsChanged           ();
+    void    storageFreeChanged              ();
+    void    storageTotalChanged             ();
+    void    batteryRemainingChanged         ();
+    void    dataReady                       (QByteArray data);
+    void    parametersReady                 ();
+    void    zoomLevelChanged                ();
+    void    focusLevelChanged               ();
+    void    streamsChanged                  ();
+    void    currentStreamChanged            ();
+    void    thermalStreamChanged            ();
+    void    autoStreamChanged               ();
+    void    recordTimeChanged               ();
+    void    streamLabelsChanged             ();
+    void    thermalModeChanged              ();
+    void    thermalOpacityChanged           ();
+    void    storageStatusChanged            ();
+
+protected:
+    virtual void    _setVideoStatus         (VideoStatus status);
+    virtual void    _setPhotoStatus         (PhotoStatus status);
+    virtual void    _setCameraMode          (CameraMode mode);
+    virtual void    _requestStreamInfo      (uint8_t streamID);
+    virtual void    _requestStreamStatus    (uint8_t streamID);
+    virtual QGCVideoStreamInfo* _findStream (uint8_t streamID, bool report = true);
+    virtual QGCVideoStreamInfo* _findStream (const QString name);
+
+protected slots:
+    virtual void    _initWhenReady          ();
+    virtual void    _requestCameraSettings  ();
+    virtual void    _requestAllParameters   ();
+    virtual void    _requestParamUpdates    ();
+    virtual void    _requestCaptureStatus   ();
+    virtual void    _requestStorageInfo     ();
+    virtual void    _downloadFinished       ();
+    virtual void    _mavCommandResult       (int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
+    virtual void    _dataReady              (QByteArray data);
+    virtual void    _paramDone              ();
+    virtual void    _streamTimeout          ();
+    virtual void    _streamStatusTimeout    ();
+    virtual void    _recTimerHandler        ();
+    virtual void    _checkForVideoStreams   ();
+
+private:
+    bool    _handleLocalization             (QByteArray& bytes);
+    bool    _replaceLocaleStrings           (const QDomNode node, QByteArray& bytes);
+    bool    _loadCameraDefinitionFile       (QByteArray& bytes);
+    bool    _loadConstants                  (const QDomNodeList nodeList);
+    bool    _loadSettings                   (const QDomNodeList nodeList);
+    void    _processRanges                  ();
+    bool    _processCondition               (const QString condition);
+    bool    _processConditionTest           (const QString conditionTest);
+    bool    _loadNameValue                  (QDomNode option, const QString factName, FactMetaData* metaData, QString& optName, QString& optValue, QVariant& optVariant);
+    bool    _loadRanges                     (QDomNode option, const QString factName, QString paramValue);
+    void    _updateActiveList               ();
+    void    _updateRanges                   (Fact* pFact);
+    void    _httpRequest                    (const QString& url);
+    void    _handleDefinitionFile           (const QString& url);
+    void    _ftpDownloadComplete            (const QString& fileName, const QString& errorMsg);
+
+    QStringList     _loadExclusions         (QDomNode option);
+    QStringList     _loadUpdates            (QDomNode option);
+    QString         _getParamName           (const char* param_id);
+
+protected:
+    Vehicle*                            _vehicle            = nullptr;
+    int                                 _compID             = 0;
+    mavlink_camera_information_t        _info;
+    int                                 _version            = 0;
+    bool                                _cached             = false;
+    bool                                _paramComplete      = false;
+    qreal                               _zoomLevel          = 0.0;
+    qreal                               _focusLevel         = 0.0;
+    uint32_t                            _storageFree        = 0;
+    uint32_t                            _storageTotal       = 0;
+    int                                 _batteryRemaining   = -1;
+    QNetworkAccessManager*              _netManager         = nullptr;
+    QString                             _modelName;
+    QString                             _vendor;
+    QString                             _cacheFile;
+    CameraMode                          _cameraMode         = CAM_MODE_UNDEFINED;
+    StorageStatus                       _storageStatus      = STORAGE_NOT_SUPPORTED;
+    PhotoMode                           _photoMode          = PHOTO_CAPTURE_SINGLE;
+    qreal                               _photoLapse         = 1.0;
+    int                                 _photoLapseCount    = 0;
+    VideoStatus                         _video_status       = VIDEO_CAPTURE_STATUS_UNDEFINED;
+    PhotoStatus                         _photo_status       = PHOTO_CAPTURE_STATUS_UNDEFINED;
+    QStringList                         _activeSettings;
+    QStringList                         _settings;
+    QTimer                              _captureStatusTimer;
+    QList<QGCCameraOptionExclusion*>    _valueExclusions;
+    QList<QGCCameraOptionRange*>        _optionRanges;
+    QMap<QString, QStringList>          _originalOptNames;
+    QMap<QString, QVariantList>         _originalOptValues;
+    QMap<QString, QGCCameraParamIO*>    _paramIO;
+    int                                 _storageInfoRetries = 0;
+    int                                 _captureInfoRetries = 0;
+    bool                                _resetting          = false;
+    QTimer                              _recTimer;
+    QTime                               _recTime;
+    uint32_t                            _recordTime         = 0;
+    //-- Parameters that require a full update
+    QMap<QString, QStringList>          _requestUpdates;
+    QStringList                         _updatesToRequest;
+    //-- Video Streams
+    int                                 _requestCount       = 0;
+    int                                 _currentStream      = 0;
+    int                                 _expectedCount      = 1;
+    QTimer                              _streamInfoTimer;
+    QTimer                              _streamStatusTimer;
+    QmlObjectListModel                  _streams;
+    QStringList                         _streamLabels;
+    ThermalViewMode                     _thermalMode        = THERMAL_BLEND;
+    double                              _thermalOpacity     = 85.0;
+};

src/Camera/QGCCameraIO.cc

@@ -0,0 +1,372 @@
+
+
+#include "QGCCameraControl.h"
+#include "QGCCameraIO.h"
+
+QGC_LOGGING_CATEGORY(CameraIOLog, "CameraIOLog")
+QGC_LOGGING_CATEGORY(CameraIOLogVerbose, "CameraIOLogVerbose")
+
+//-----------------------------------------------------------------------------
+QGCCameraParamIO::QGCCameraParamIO(QGCCameraControl *control, Fact* fact, Vehicle *vehicle)
+    : QObject(control)
+    , _control(control)
+    , _fact(fact)
+    , _vehicle(vehicle)
+    , _sentRetries(0)
+    , _requestRetries(0)
+    , _done(false)
+    , _updateOnSet(false)
+    , _forceUIUpdate(false)
+{
+    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+    _paramWriteTimer.setSingleShot(true);
+    _paramWriteTimer.setInterval(3000);
+    _paramRequestTimer.setSingleShot(true);
+    _paramRequestTimer.setInterval(3500);
+    if(_fact->writeOnly()) {
+        //-- Write mode is always "done" as it won't ever read
+        _done = true;
+    } else {
+        connect(&_paramRequestTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramRequestTimeout);
+    }
+    connect(&_paramWriteTimer,   &QTimer::timeout, this, &QGCCameraParamIO::_paramWriteTimeout);
+    connect(_fact, &Fact::rawValueChanged, this, &QGCCameraParamIO::_factChanged);
+    connect(_fact, &Fact::_containerRawValueChanged, this, &QGCCameraParamIO::_containerRawValueChanged);
+    _pMavlink = qgcApp()->toolbox()->mavlinkProtocol();
+    //-- TODO: Even though we don't use anything larger than 32-bit, this should
+    //   probably be updated.
+    switch (_fact->type()) {
+        case FactMetaData::valueTypeUint8:
+        case FactMetaData::valueTypeBool:
+            _mavParamType = MAV_PARAM_EXT_TYPE_UINT8;
+            break;
+        case FactMetaData::valueTypeInt8:
+            _mavParamType = MAV_PARAM_EXT_TYPE_INT8;
+            break;
+        case FactMetaData::valueTypeUint16:
+            _mavParamType = MAV_PARAM_EXT_TYPE_UINT16;
+            break;
+        case FactMetaData::valueTypeInt16:
+            _mavParamType = MAV_PARAM_EXT_TYPE_INT16;
+            break;
+        case FactMetaData::valueTypeUint32:
+            _mavParamType = MAV_PARAM_EXT_TYPE_UINT32;
+            break;
+        case FactMetaData::valueTypeUint64:
+            _mavParamType = MAV_PARAM_EXT_TYPE_UINT64;
+            break;
+        case FactMetaData::valueTypeInt64:
+            _mavParamType = MAV_PARAM_EXT_TYPE_INT64;
+            break;
+        case FactMetaData::valueTypeFloat:
+            _mavParamType = MAV_PARAM_EXT_TYPE_REAL32;
+            break;
+        case FactMetaData::valueTypeDouble:
+            _mavParamType = MAV_PARAM_EXT_TYPE_REAL64;
+            break;
+            //-- String and custom are the same for now
+        case FactMetaData::valueTypeString:
+        case FactMetaData::valueTypeCustom:
+            _mavParamType = MAV_PARAM_EXT_TYPE_CUSTOM;
+            break;
+        default:
+            qWarning() << "Unsupported fact type" << _fact->type() << "for" << _fact->name();
+        case FactMetaData::valueTypeInt32:
+            _mavParamType = MAV_PARAM_EXT_TYPE_INT32;
+            break;
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::setParamRequest()
+{
+    if(!_fact->writeOnly()) {
+        _paramRequestReceived = false;
+        _requestRetries = 0;
+        _paramRequestTimer.start();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::_factChanged(QVariant value)
+{
+    if(!_forceUIUpdate) {
+        Q_UNUSED(value);
+        qCDebug(CameraIOLog) << "UI Fact" << _fact->name() << "changed to" << value;
+        _control->factChanged(_fact);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::_containerRawValueChanged(const QVariant value)
+{
+    if(!_fact->readOnly()) {
+        Q_UNUSED(value);
+        qCDebug(CameraIOLog) << "Update Fact from camera" << _fact->name();
+        _sentRetries = 0;
+        _sendParameter();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::sendParameter(bool updateUI)
+{
+    qCDebug(CameraIOLog) << "Send Fact" << _fact->name();
+    _sentRetries = 0;
+    _updateOnSet = updateUI;
+    _sendParameter();
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::_sendParameter()
+{
+    WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();
+    if (!weakLink.expired()) {
+        SharedLinkInterfacePtr sharedLink = weakLink.lock();
+
+        mavlink_param_ext_set_t p;
+        memset(&p, 0, sizeof(mavlink_param_ext_set_t));
+        param_ext_union_t   union_value;
+        mavlink_message_t   msg;
+        FactMetaData::ValueType_t factType = _fact->type();
+        p.param_type = _mavParamType;
+        switch (factType) {
+        case FactMetaData::valueTypeUint8:
+        case FactMetaData::valueTypeBool:
+            union_value.param_uint8 = static_cast<uint8_t>(_fact->rawValue().toUInt());
+            break;
+        case FactMetaData::valueTypeInt8:
+            union_value.param_int8 = static_cast<int8_t>(_fact->rawValue().toInt());
+            break;
+        case FactMetaData::valueTypeUint16:
+            union_value.param_uint16 = static_cast<uint16_t>(_fact->rawValue().toUInt());
+            break;
+        case FactMetaData::valueTypeInt16:
+            union_value.param_int16 = static_cast<int16_t>(_fact->rawValue().toInt());
+            break;
+        case FactMetaData::valueTypeUint32:
+            union_value.param_uint32 = static_cast<uint32_t>(_fact->rawValue().toUInt());
+            break;
+        case FactMetaData::valueTypeInt64:
+            union_value.param_int64 = static_cast<int64_t>(_fact->rawValue().toLongLong());
+            break;
+        case FactMetaData::valueTypeUint64:
+            union_value.param_uint64 = static_cast<uint64_t>(_fact->rawValue().toULongLong());
+            break;
+        case FactMetaData::valueTypeFloat:
+            union_value.param_float = _fact->rawValue().toFloat();
+            break;
+        case FactMetaData::valueTypeDouble:
+            union_value.param_double = _fact->rawValue().toDouble();
+            break;
+            //-- String and custom are the same for now
+        case FactMetaData::valueTypeString:
+        case FactMetaData::valueTypeCustom:
+        {
+            QByteArray custom = _fact->rawValue().toByteArray();
+            memcpy(union_value.bytes, custom.data(), static_cast<size_t>(std::max(custom.size(), static_cast<qsizetype>(MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN))));
+        }
+            break;
+        default:
+            qCritical() << "Unsupported fact type" << factType << "for" << _fact->name();
+        case FactMetaData::valueTypeInt32:
+            union_value.param_int32 = static_cast<int32_t>(_fact->rawValue().toInt());
+            break;
+        }
+        memcpy(&p.param_value[0], &union_value.bytes[0], MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN);
+        p.target_system     = static_cast<uint8_t>(_vehicle->id());
+        p.target_component  = static_cast<uint8_t>(_control->compID());
+        strncpy(p.param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN);
+        mavlink_msg_param_ext_set_encode_chan(
+                    static_cast<uint8_t>(_pMavlink->getSystemId()),
+                    static_cast<uint8_t>(_pMavlink->getComponentId()),
+                    sharedLink->mavlinkChannel(),
+                    &msg,
+                    &p);
+        _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
+    }
+    _paramWriteTimer.start();
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::_paramWriteTimeout()
+{
+    if(++_sentRetries > 3) {
+        qCWarning(CameraIOLog) << "No response for param set:" << _fact->name();
+        _updateOnSet = false;
+    } else {
+        //-- Send it again
+        qCDebug(CameraIOLog) << "Param set retry:" << _fact->name() << _sentRetries;
+        _sendParameter();
+        _paramWriteTimer.start();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack)
+{
+    _paramWriteTimer.stop();
+    if(ack.param_result == PARAM_ACK_ACCEPTED) {
+        QVariant val = _valueFromMessage(ack.param_value, ack.param_type);
+        if(_fact->rawValue() != val) {
+            _fact->_containerSetRawValue(val);
+            if(_updateOnSet) {
+                _updateOnSet = false;
+                _control->factChanged(_fact);
+            }
+        }
+    } else if(ack.param_result == PARAM_ACK_IN_PROGRESS) {
+        //-- Wait a bit longer for this one
+        qCDebug(CameraIOLogVerbose) << "Param set in progress:" << _fact->name();
+        _paramWriteTimer.start();
+    } else {
+        if(ack.param_result == PARAM_ACK_FAILED) {
+            if(++_sentRetries < 3) {
+                //-- Try again
+                qCWarning(CameraIOLog) << "Param set failed:" << _fact->name() << _sentRetries;
+                _paramWriteTimer.start();
+            }
+            return;
+        } else if(ack.param_result == PARAM_ACK_VALUE_UNSUPPORTED) {
+            qCWarning(CameraIOLog) << "Param set unsuported:" << _fact->name();
+        }
+        //-- If UI changed and value was not set, restore UI
+        QVariant val = _valueFromMessage(ack.param_value, ack.param_type);
+        if(_fact->rawValue() != val) {
+            if(_control->validateParameter(_fact, val)) {
+                _fact->_containerSetRawValue(val);
+            }
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t& value)
+{
+    _paramRequestTimer.stop();
+    QVariant newValue = _valueFromMessage(value.param_value, value.param_type);
+    if(_control->incomingParameter(_fact, newValue)) {
+        _fact->_containerSetRawValue(newValue);
+    }
+    _paramRequestReceived = true;
+    if(_forceUIUpdate) {
+        emit _fact->rawValueChanged(_fact->rawValue());
+        emit _fact->valueChanged(_fact->rawValue());
+        _forceUIUpdate = false;
+    }
+    if(!_done) {
+        _done = true;
+        _control->_paramDone();
+    }
+    qCDebug(CameraIOLog) << QString("handleParamValue() %1 %2").arg(_fact->name()).arg(_fact->rawValueString());
+}
+
+//-----------------------------------------------------------------------------
+QVariant
+QGCCameraParamIO::_valueFromMessage(const char* value, uint8_t param_type)
+{
+    QVariant var;
+    param_ext_union_t u;
+    memcpy(u.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN);
+    switch (param_type) {
+        case MAV_PARAM_EXT_TYPE_REAL32:
+            var = QVariant(u.param_float);
+            break;
+        case MAV_PARAM_EXT_TYPE_UINT8:
+            var = QVariant(u.param_uint8);
+            break;
+        case MAV_PARAM_EXT_TYPE_INT8:
+            var = QVariant(u.param_int8);
+            break;
+        case MAV_PARAM_EXT_TYPE_UINT16:
+            var = QVariant(u.param_uint16);
+            break;
+        case MAV_PARAM_EXT_TYPE_INT16:
+            var = QVariant(u.param_int16);
+            break;
+        case MAV_PARAM_EXT_TYPE_UINT32:
+            var = QVariant(u.param_uint32);
+            break;
+        case MAV_PARAM_EXT_TYPE_INT32:
+            var = QVariant(u.param_int32);
+            break;
+        case MAV_PARAM_EXT_TYPE_UINT64:
+            var = QVariant(static_cast<qulonglong>(u.param_uint64));
+            break;
+        case MAV_PARAM_EXT_TYPE_INT64:
+            var = QVariant(static_cast<qulonglong>(u.param_int64));
+            break;
+        case MAV_PARAM_EXT_TYPE_CUSTOM:
+            var = QVariant(QByteArray(value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN));
+            break;
+        default:
+            var = QVariant(0);
+            qCritical() << "Invalid param_type used for camera setting:" << param_type;
+    }
+    return var;
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::_paramRequestTimeout()
+{
+    if(++_requestRetries > 3) {
+        qCWarning(CameraIOLog) << "No response for param request:" << _fact->name();
+        if(!_done) {
+            _done = true;
+            _control->_paramDone();
+        }
+    } else {
+        //-- Request it again
+        qCDebug(CameraIOLog) << "Param request retry:" << _fact->name();
+        paramRequest(false);
+        _paramRequestTimer.start();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraParamIO::paramRequest(bool reset)
+{
+    //-- If it's write only, we don't request it.
+    if(_fact->writeOnly()) {
+        if(!_done) {
+            _done = true;
+            _control->_paramDone();
+        }
+        return;
+    }
+    if(reset) {
+        _requestRetries = 0;
+        _forceUIUpdate  = true;
+    }
+    qCDebug(CameraIOLog) << "Request parameter:" << _fact->name();
+    WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();
+    if (!weakLink.expired()) {
+        SharedLinkInterfacePtr sharedLink = weakLink.lock();
+
+        char param_id[MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
+        memset(param_id, 0, sizeof(param_id));
+        strncpy(param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN);
+        mavlink_message_t msg;
+        mavlink_msg_param_ext_request_read_pack_chan(
+                    static_cast<uint8_t>(_pMavlink->getSystemId()),
+                    static_cast<uint8_t>(_pMavlink->getComponentId()),
+                    sharedLink->mavlinkChannel(),
+                    &msg,
+                    static_cast<uint8_t>(_vehicle->id()),
+                    static_cast<uint8_t>(_control->compID()),
+                    param_id,
+                    -1);
+        _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
+    }
+    _paramRequestTimer.start();
+}

src/Camera/QGCCameraIO.h

@@ -0,0 +1,72 @@
+
+#pragma once
+
+#include "QGCApplication.h"
+#include <QLoggingCategory>
+
+class QGCCameraControl;
+
+Q_DECLARE_LOGGING_CATEGORY(CameraIOLog)
+Q_DECLARE_LOGGING_CATEGORY(CameraIOLogVerbose)
+
+MAVPACKED(
+typedef struct {
+    union {
+        float       param_float;
+        double      param_double;
+        int64_t     param_int64;
+        uint64_t    param_uint64;
+        int32_t     param_int32;
+        uint32_t    param_uint32;
+        int16_t     param_int16;
+        uint16_t    param_uint16;
+        int8_t      param_int8;
+        uint8_t     param_uint8;
+        uint8_t     bytes[MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN];
+    };
+    uint8_t type;
+}) param_ext_union_t;
+
+//-----------------------------------------------------------------------------
+/// Camera parameter handler.
+class QGCCameraParamIO : public QObject
+{
+public:
+    QGCCameraParamIO(QGCCameraControl* control, Fact* fact, Vehicle* vehicle);
+
+    void        handleParamAck              (const mavlink_param_ext_ack_t& ack);
+    void        handleParamValue            (const mavlink_param_ext_value_t& value);
+    void        setParamRequest             ();
+    bool        paramDone                   () const { return _done; }
+    void        paramRequest                (bool reset = true);
+    void        sendParameter               (bool updateUI = false);
+
+    QStringList  optNames;
+    QVariantList optVariants;
+
+private slots:
+    void        _paramWriteTimeout          ();
+    void        _paramRequestTimeout        ();
+    void        _factChanged                (QVariant value);
+    void        _containerRawValueChanged   (const QVariant value);
+
+private:
+    void        _sendParameter              ();
+    QVariant    _valueFromMessage           (const char* value, uint8_t param_type);
+
+private:
+    QGCCameraControl*   _control;
+    Fact*               _fact;
+    Vehicle*            _vehicle;
+    int                 _sentRetries;
+    int                 _requestRetries;
+    bool                _paramRequestReceived;
+    QTimer              _paramWriteTimer;
+    QTimer              _paramRequestTimer;
+    bool                _done;
+    bool                _updateOnSet;
+    MAV_PARAM_EXT_TYPE  _mavParamType;
+    MAVLinkProtocol*    _pMavlink;
+    bool                _forceUIUpdate;
+};
+

src/Camera/QGCCameraManager.cc

@@ -0,0 +1,518 @@
+
+
+#include "QGCApplication.h"
+#include "QGCCameraManager.h"
+#include "JoystickManager.h"
+
+QGC_LOGGING_CATEGORY(CameraManagerLog, "CameraManagerLog")
+
+//-----------------------------------------------------------------------------
+QGCCameraManager::CameraStruct::CameraStruct(QObject* parent, uint8_t compID_)
+    : QObject(parent)
+    , compID(compID_)
+{
+}
+
+//-----------------------------------------------------------------------------
+QGCCameraManager::QGCCameraManager(Vehicle *vehicle)
+    : _vehicle(vehicle)
+{
+    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+    qCDebug(CameraManagerLog) << "QGCCameraManager Created";
+    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &QGCCameraManager::_vehicleReady);
+    connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &QGCCameraManager::_mavlinkMessageReceived);
+    connect(&_cameraTimer, &QTimer::timeout, this, &QGCCameraManager::_cameraTimeout);
+    _cameraTimer.setSingleShot(false);
+    _lastZoomChange.start();
+    _lastCameraChange.start();
+    _cameraTimer.start(500);
+}
+
+//-----------------------------------------------------------------------------
+QGCCameraManager::~QGCCameraManager()
+{
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::setCurrentCamera(int sel)
+{
+    if(sel != _currentCamera && sel >= 0 && sel < _cameras.count()) {
+        _currentCamera = sel;
+        emit currentCameraChanged();
+        emit streamChanged();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_vehicleReady(bool ready)
+{
+    qCDebug(CameraManagerLog) << "_vehicleReady(" << ready << ")";
+    if(ready) {
+        if(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle() == _vehicle) {
+            _vehicleReadyState = true;
+            JoystickManager *pJoyMgr = qgcApp()->toolbox()->joystickManager();
+            _activeJoystickChanged(pJoyMgr->activeJoystick());
+            connect(pJoyMgr, &JoystickManager::activeJoystickChanged, this, &QGCCameraManager::_activeJoystickChanged);
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message)
+{
+    //-- Only pay attention to camera components, as identified by their compId
+    if(message.sysid == _vehicle->id() && (message.compid >= MAV_COMP_ID_CAMERA && message.compid <= MAV_COMP_ID_CAMERA6)) {
+        switch (message.msgid) {
+            case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
+                _handleCaptureStatus(message);
+                break;
+            case MAVLINK_MSG_ID_STORAGE_INFORMATION:
+                _handleStorageInfo(message);
+                break;
+            case MAVLINK_MSG_ID_HEARTBEAT:
+                _handleHeartbeat(message);
+                break;
+            case MAVLINK_MSG_ID_CAMERA_INFORMATION:
+                _handleCameraInfo(message);
+                break;
+            case MAVLINK_MSG_ID_CAMERA_SETTINGS:
+                _handleCameraSettings(message);
+                break;
+            case MAVLINK_MSG_ID_PARAM_EXT_ACK:
+                _handleParamAck(message);
+                break;
+            case MAVLINK_MSG_ID_PARAM_EXT_VALUE:
+                _handleParamValue(message);
+                break;
+            case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:
+                _handleVideoStreamInfo(message);
+                break;
+            case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:
+                _handleVideoStreamStatus(message);
+                break;
+            case MAVLINK_MSG_ID_BATTERY_STATUS:
+                _handleBatteryStatus(message);
+                break;
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message)
+{
+    //-- First time hearing from this one?
+    QString sCompID = QString::number(message.compid);
+    if(!_cameraInfoRequest.contains(sCompID)) {
+        qCDebug(CameraManagerLog) << "Hearbeat from " << message.compid;
+        CameraStruct* pInfo = new CameraStruct(this, message.compid);
+        pInfo->lastHeartbeat.start();
+        _cameraInfoRequest[sCompID] = pInfo;
+        //-- Request camera info
+        _requestCameraInfo(message.compid);
+    } else {
+        if(_cameraInfoRequest[sCompID]) {
+            CameraStruct* pInfo = _cameraInfoRequest[sCompID];
+            //-- Check if we have indeed received the camera info
+            if(pInfo->infoReceived) {
+                //-- We have it. Just update the heartbeat timeout
+                pInfo->lastHeartbeat.start();
+            } else {
+                //-- Try again. Maybe.
+                if(pInfo->lastHeartbeat.elapsed() > 2000) {
+                    if(pInfo->tryCount > 10) {
+                        if(!pInfo->gaveUp) {
+                            pInfo->gaveUp = true;
+                            qWarning() << "Giving up requesting camera info from" << _vehicle->id() << message.compid;
+                        }
+                    } else {
+                        pInfo->tryCount++;
+                        //-- Request camera info again.
+                        _requestCameraInfo(message.compid);
+                    }
+                }
+            }
+        } else {
+            qWarning() << "_cameraInfoRequest[" << sCompID << "] is null";
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+QGCCameraControl*
+QGCCameraManager::currentCameraInstance()
+{
+    if(_currentCamera < _cameras.count() && _cameras.count()) {
+        QGCCameraControl* pCamera = qobject_cast<QGCCameraControl*>(_cameras[_currentCamera]);
+        return pCamera;
+    }
+    return nullptr;
+}
+
+//-----------------------------------------------------------------------------
+QGCVideoStreamInfo*
+QGCCameraManager::currentStreamInstance()
+{
+    QGCCameraControl* pCamera = currentCameraInstance();
+    if(pCamera) {
+        QGCVideoStreamInfo* pInfo = pCamera->currentStreamInstance();
+        return pInfo;
+    }
+    return nullptr;
+}
+
+//-----------------------------------------------------------------------------
+QGCVideoStreamInfo*
+QGCCameraManager::thermalStreamInstance()
+{
+    QGCCameraControl* pCamera = currentCameraInstance();
+    if(pCamera) {
+        QGCVideoStreamInfo* pInfo = pCamera->thermalStreamInstance();
+        return pInfo;
+    }
+    return nullptr;
+}
+
+//-----------------------------------------------------------------------------
+QGCCameraControl*
+QGCCameraManager::_findCamera(int id)
+{
+    for(int i = 0; i < _cameras.count(); i++) {
+        if(_cameras[i]) {
+            QGCCameraControl* pCamera = qobject_cast<QGCCameraControl*>(_cameras[i]);
+            if(pCamera) {
+                if(pCamera->compID() == id) {
+                    return pCamera;
+                }
+            } else {
+                qCritical() << "Null QGCCameraControl instance";
+            }
+        }
+    }
+    //qWarning() << "Camera component id not found:" << id;
+    return nullptr;
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message)
+{
+    //-- Have we requested it?
+    QString sCompID = QString::number(message.compid);
+    if(_cameraInfoRequest.contains(sCompID) && !_cameraInfoRequest[sCompID]->infoReceived) {
+        //-- Flag it as done
+        _cameraInfoRequest[sCompID]->infoReceived = true;
+        mavlink_camera_information_t info;
+        mavlink_msg_camera_information_decode(&message, &info);
+        qCDebug(CameraManagerLog) << "_handleCameraInfo:" << reinterpret_cast<const char*>(info.model_name) << reinterpret_cast<const char*>(info.vendor_name) << "Comp ID:" << message.compid;
+        QGCCameraControl* pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this);
+        if(pCamera) {
+            QQmlEngine::setObjectOwnership(pCamera, QQmlEngine::CppOwnership);
+            _cameras.append(pCamera);
+            _cameraLabels << pCamera->modelName();
+            emit camerasChanged();
+            emit cameraLabelsChanged();
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_cameraTimeout()
+{
+    //-- Iterate cameras
+    foreach(QString sCompID, _cameraInfoRequest.keys()) {
+        if(_cameraInfoRequest[sCompID]) {
+            CameraStruct* pInfo = _cameraInfoRequest[sCompID];
+            //-- Have we received a camera info message?
+            if(pInfo->infoReceived) {
+                //-- Has the camera stopped talking to us?
+                if(pInfo->lastHeartbeat.elapsed() > 5000) {
+                    //-- Camera is gone. Remove it.
+                    bool autoStream = false;
+                    QGCCameraControl* pCamera = _findCamera(pInfo->compID);
+                    if(pCamera) {
+                        qWarning() << "Camera" << pCamera->modelName() << "stopped transmitting. Removing from list.";
+                        int idx = _cameraLabels.indexOf(pCamera->modelName());
+                        if(idx >= 0) {
+                            _cameraLabels.removeAt(idx);
+                        }
+                        idx = _cameras.indexOf(pCamera);
+                        if(idx >= 0) {
+                            _cameras.removeAt(idx);
+                        }
+                        autoStream = pCamera->autoStream();
+                        pCamera->deleteLater();
+                        delete pInfo;
+                    }
+                    _cameraInfoRequest.remove(sCompID);
+                    emit cameraLabelsChanged();
+                    //-- If we have another camera, switch current camera.
+                    if(_cameras.count()) {
+                        setCurrentCamera(0);
+                    } else {
+                        //-- We're out of cameras
+                        emit camerasChanged();
+                        if(autoStream) {
+                            emit streamChanged();
+                        }
+                    }
+                    //-- Exit loop.
+                    return;
+                }
+            }
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleCaptureStatus(const mavlink_message_t &message)
+{
+    QGCCameraControl* pCamera = _findCamera(message.compid);
+    if(pCamera) {
+        mavlink_camera_capture_status_t cap;
+        mavlink_msg_camera_capture_status_decode(&message, &cap);
+        pCamera->handleCaptureStatus(cap);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleStorageInfo(const mavlink_message_t& message)
+{
+    QGCCameraControl* pCamera = _findCamera(message.compid);
+    if(pCamera) {
+        mavlink_storage_information_t st;
+        mavlink_msg_storage_information_decode(&message, &st);
+        pCamera->handleStorageInfo(st);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleCameraSettings(const mavlink_message_t& message)
+{
+    QGCCameraControl* pCamera = _findCamera(message.compid);
+    if(pCamera) {
+        mavlink_camera_settings_t settings;
+        mavlink_msg_camera_settings_decode(&message, &settings);
+        pCamera->handleSettings(settings);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleParamAck(const mavlink_message_t& message)
+{
+    QGCCameraControl* pCamera = _findCamera(message.compid);
+    if(pCamera) {
+        mavlink_param_ext_ack_t ack;
+        mavlink_msg_param_ext_ack_decode(&message, &ack);
+        pCamera->handleParamAck(ack);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleParamValue(const mavlink_message_t& message)
+{
+    QGCCameraControl* pCamera = _findCamera(message.compid);
+    if(pCamera) {
+        mavlink_param_ext_value_t value;
+        mavlink_msg_param_ext_value_decode(&message, &value);
+        pCamera->handleParamValue(value);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleVideoStreamInfo(const mavlink_message_t& message)
+{
+    QGCCameraControl* pCamera = _findCamera(message.compid);
+    if(pCamera) {
+        mavlink_video_stream_information_t streamInfo;
+        mavlink_msg_video_stream_information_decode(&message, &streamInfo);
+        pCamera->handleVideoInfo(&streamInfo);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleVideoStreamStatus(const mavlink_message_t& message)
+{
+    QGCCameraControl* pCamera = _findCamera(message.compid);
+    if(pCamera) {
+        mavlink_video_stream_status_t streamStatus;
+        mavlink_msg_video_stream_status_decode(&message, &streamStatus);
+        pCamera->handleVideoStatus(&streamStatus);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_handleBatteryStatus(const mavlink_message_t& message)
+{
+    QGCCameraControl* pCamera = _findCamera(message.compid);
+    if(pCamera) {
+        mavlink_battery_status_t batteryStatus;
+        mavlink_msg_battery_status_decode(&message, &batteryStatus);
+        pCamera->handleBatteryStatus(batteryStatus);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_requestCameraInfo(int compID)
+{
+    qCDebug(CameraManagerLog) << "_requestCameraInfo(" << compID << ")";
+    if(_vehicle) {
+        _vehicle->sendMavCommand(
+            compID,                                 // target component
+            MAV_CMD_REQUEST_CAMERA_INFORMATION,     // command id
+            false,                                  // showError
+            1);                                     // Do Request
+    }
+}
+
+//----------------------------------------------------------------------------------------
+void
+QGCCameraManager::_activeJoystickChanged(Joystick* joystick)
+{
+    qCDebug(CameraManagerLog) << "Joystick changed";
+    if(_activeJoystick) {
+        disconnect(_activeJoystick, &Joystick::stepZoom,            this, &QGCCameraManager::_stepZoom);
+        disconnect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom);
+        disconnect(_activeJoystick, &Joystick::stopContinuousZoom,  this, &QGCCameraManager::_stopZoom);
+        disconnect(_activeJoystick, &Joystick::stepCamera,          this, &QGCCameraManager::_stepCamera);
+        disconnect(_activeJoystick, &Joystick::stepStream,          this, &QGCCameraManager::_stepStream);
+        disconnect(_activeJoystick, &Joystick::triggerCamera,       this, &QGCCameraManager::_triggerCamera);
+        disconnect(_activeJoystick, &Joystick::startVideoRecord,    this, &QGCCameraManager::_startVideoRecording);
+        disconnect(_activeJoystick, &Joystick::stopVideoRecord,     this, &QGCCameraManager::_stopVideoRecording);
+        disconnect(_activeJoystick, &Joystick::toggleVideoRecord,   this, &QGCCameraManager::_toggleVideoRecording);
+    }
+    _activeJoystick = joystick;
+    if(_activeJoystick) {
+        connect(_activeJoystick, &Joystick::stepZoom,               this, &QGCCameraManager::_stepZoom);
+        connect(_activeJoystick, &Joystick::startContinuousZoom,    this, &QGCCameraManager::_startZoom);
+        connect(_activeJoystick, &Joystick::stopContinuousZoom,     this, &QGCCameraManager::_stopZoom);
+        connect(_activeJoystick, &Joystick::stepCamera,             this, &QGCCameraManager::_stepCamera);
+        connect(_activeJoystick, &Joystick::stepStream,             this, &QGCCameraManager::_stepStream);
+        connect(_activeJoystick, &Joystick::triggerCamera,          this, &QGCCameraManager::_triggerCamera);
+        connect(_activeJoystick, &Joystick::startVideoRecord,       this, &QGCCameraManager::_startVideoRecording);
+        connect(_activeJoystick, &Joystick::stopVideoRecord,        this, &QGCCameraManager::_stopVideoRecording);
+        connect(_activeJoystick, &Joystick::toggleVideoRecord,      this, &QGCCameraManager::_toggleVideoRecording);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_triggerCamera()
+{
+    QGCCameraControl* pCamera = currentCameraInstance();
+    if(pCamera) {
+        pCamera->takePhoto();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_startVideoRecording()
+{
+    QGCCameraControl* pCamera = currentCameraInstance();
+    if(pCamera) {
+        pCamera->startVideo();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_stopVideoRecording()
+{
+    QGCCameraControl* pCamera = currentCameraInstance();
+    if(pCamera) {
+        pCamera->stopVideo();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_toggleVideoRecording()
+{
+    QGCCameraControl* pCamera = currentCameraInstance();
+    if(pCamera) {
+        pCamera->toggleVideo();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_stepZoom(int direction)
+{
+    if(_lastZoomChange.elapsed() > 40) {
+        _lastZoomChange.start();
+        qCDebug(CameraManagerLog) << "Step Camera Zoom" << direction;
+        QGCCameraControl* pCamera = currentCameraInstance();
+        if(pCamera) {
+            pCamera->stepZoom(direction);
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_startZoom(int direction)
+{
+    qCDebug(CameraManagerLog) << "Start Camera Zoom" << direction;
+    QGCCameraControl* pCamera = currentCameraInstance();
+    if(pCamera) {
+        pCamera->startZoom(direction);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_stopZoom()
+{
+    qCDebug(CameraManagerLog) << "Stop Camera Zoom";
+    QGCCameraControl* pCamera = currentCameraInstance();
+    if(pCamera) {
+        pCamera->stopZoom();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_stepCamera(int direction)
+{
+    if(_lastCameraChange.elapsed() > 1000) {
+        _lastCameraChange.start();
+        qCDebug(CameraManagerLog) << "Step Camera" << direction;
+        int c = _currentCamera + direction;
+        if(c < 0) c = _cameras.count() - 1;
+        if(c >= _cameras.count()) c = 0;
+        setCurrentCamera(c);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraManager::_stepStream(int direction)
+{
+    if(_lastCameraChange.elapsed() > 1000) {
+        _lastCameraChange.start();
+        QGCCameraControl* pCamera = currentCameraInstance();
+        if(pCamera) {
+            qCDebug(CameraManagerLog) << "Step Camera Stream" << direction;
+            int c = pCamera->currentStream() + direction;
+            if(c < 0) c = pCamera->streams()->count() - 1;
+            if(c >= pCamera->streams()->count()) c = 0;
+            pCamera->setCurrentStream(c);
+        }
+    }
+}
+
+

src/Camera/QGCCameraManager.h

@@ -0,0 +1,112 @@
+/*!
+ * @file
+ *   @brief Camera Controller
+ *   @author Gus Grubba <gus@auterion.com>
+ *
+ */
+
+/// @file
+/// @brief  MAVLink Camera API. Camera Manager.
+/// @author Gus Grubba <gus@auterion.com>
+
+#pragma once
+
+#include "QGCApplication.h"
+#include <QLoggingCategory>
+#include "QmlObjectListModel.h"
+#include "QGCCameraControl.h"
+
+#include <QObject>
+#include <QTimer>
+
+Q_DECLARE_LOGGING_CATEGORY(CameraManagerLog)
+
+class Joystick;
+
+//-----------------------------------------------------------------------------
+/// Camera Manager
+class QGCCameraManager : public QObject
+{
+    Q_OBJECT
+public:
+    QGCCameraManager(Vehicle* vehicle);
+    virtual ~QGCCameraManager();
+
+    Q_PROPERTY(QmlObjectListModel*  cameras                 READ cameras                                        NOTIFY camerasChanged)
+    Q_PROPERTY(QStringList          cameraLabels            READ cameraLabels                                   NOTIFY cameraLabelsChanged)
+    Q_PROPERTY(QGCCameraControl*    currentCameraInstance   READ currentCameraInstance                          NOTIFY currentCameraChanged)
+    Q_PROPERTY(int                  currentCamera           READ currentCamera      WRITE  setCurrentCamera     NOTIFY currentCameraChanged)
+
+    //-- Return a list of cameras provided by this vehicle
+    virtual QmlObjectListModel* cameras             () { return &_cameras; }
+    //-- Camera names to show the user (for selection)
+    virtual QStringList         cameraLabels        () { return _cameraLabels; }
+    //-- Current selected camera
+    virtual int                 currentCamera       () { return _currentCamera; }
+    virtual QGCCameraControl*   currentCameraInstance();
+    //-- Set current camera
+    virtual void                setCurrentCamera    (int sel);
+    //-- Current stream
+    virtual QGCVideoStreamInfo* currentStreamInstance();
+    //-- Current thermal stream
+    virtual QGCVideoStreamInfo* thermalStreamInstance();
+
+signals:
+    void    camerasChanged          ();
+    void    cameraLabelsChanged     ();
+    void    currentCameraChanged    ();
+    void    streamChanged           ();
+
+protected slots:
+    virtual void    _vehicleReady           (bool ready);
+    virtual void    _mavlinkMessageReceived (const mavlink_message_t& message);
+    virtual void    _activeJoystickChanged  (Joystick* joystick);
+    virtual void    _stepZoom               (int direction);
+    virtual void    _startZoom              (int direction);
+    virtual void    _stopZoom               ();
+    virtual void    _stepCamera             (int direction);
+    virtual void    _stepStream             (int direction);
+    virtual void    _cameraTimeout          ();
+    virtual void    _triggerCamera          ();
+    virtual void    _startVideoRecording    ();
+    virtual void    _stopVideoRecording     ();
+    virtual void    _toggleVideoRecording   ();
+
+protected:
+    virtual QGCCameraControl* _findCamera   (int id);
+    virtual void    _requestCameraInfo      (int compID);
+    virtual void    _handleHeartbeat        (const mavlink_message_t& message);
+    virtual void    _handleCameraInfo       (const mavlink_message_t& message);
+    virtual void    _handleStorageInfo      (const mavlink_message_t& message);
+    virtual void    _handleCameraSettings   (const mavlink_message_t& message);
+    virtual void    _handleParamAck         (const mavlink_message_t& message);
+    virtual void    _handleParamValue       (const mavlink_message_t& message);
+    virtual void    _handleCaptureStatus    (const mavlink_message_t& message);
+    virtual void    _handleVideoStreamInfo  (const mavlink_message_t& message);
+    virtual void    _handleVideoStreamStatus(const mavlink_message_t& message);
+    virtual void    _handleBatteryStatus    (const mavlink_message_t& message);
+
+protected:
+
+    class CameraStruct : public QObject {
+    public:
+        CameraStruct(QObject* parent, uint8_t compID_);
+        QElapsedTimer lastHeartbeat;
+        bool    infoReceived = false;
+        bool    gaveUp       = false;
+        int     tryCount     = 0;
+        uint8_t compID       = 0;
+    };
+
+    Vehicle*            _vehicle            = nullptr;
+    Joystick*           _activeJoystick     = nullptr;
+    bool                _vehicleReadyState  = false;
+    int                 _currentTask        = 0;
+    QmlObjectListModel  _cameras;
+    QStringList         _cameraLabels;
+    int                 _currentCamera      = 0;
+    QElapsedTimer       _lastZoomChange;
+    QElapsedTimer       _lastCameraChange;
+    QTimer              _cameraTimer;
+    QMap<QString, CameraStruct*> _cameraInfoRequest;
+};

src/GPS/CMakeLists.txt

@@ -1,26 +0,0 @@
-
-
-add_library(gps
-	Drivers/src/ashtech.cpp
-	Drivers/src/gps_helper.cpp
-	Drivers/src/mtk.cpp
-	Drivers/src/rtcm.cpp
-	Drivers/src/sbf.cpp
-	Drivers/src/ubx.cpp
-	GPSManager.cc
-	GPSProvider.cc
-	RTCM/RTCMMavlink.cc
-)
-
-target_link_libraries(gps
-	Qt6::Core
-	Qt6::Location
-	Qt6::SerialPort
-	Qt6::Svg
-	Qt6::TextToSpeech
-
-	qgc
-)
-
-target_include_directories(gps INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
-

src/GPS/GPSManager.h

@@ -1,50 +0,0 @@
-/****************************************************************************
- *
- * (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 "GPSProvider.h"
-#include "RTCM/RTCMMavlink.h"
-#include <QGCToolbox.h>
-
-#include <QString>
-#include <QObject>
-
-/**
- ** class GPSManager
- * handles a GPS provider and RTK
- */
-class GPSManager : public QGCTool
-{
-    Q_OBJECT
-public:
-    GPSManager(QGCApplication* app, QGCToolbox* toolbox);
-    ~GPSManager();
-
-    void connectGPS     (const QString& device, const QString& gps_type);
-    void disconnectGPS  (void);
-    bool connected      (void) const { return _gpsProvider && _gpsProvider->isRunning(); }
-
-signals:
-    void onConnect();
-    void onDisconnect();
-    void surveyInStatus(float duration, float accuracyMM,  double latitude, double longitude, float altitude, bool valid, bool active);
-    void satelliteUpdate(int numSats);
-
-private slots:
-    void GPSPositionUpdate(GPSPositionMessage msg);
-    void GPSSatelliteUpdate(GPSSatelliteMessage msg);
-
-private:
-    GPSProvider* _gpsProvider = nullptr;
-    RTCMMavlink* _rtcmMavlink = nullptr;
-
-    std::atomic_bool _requestGpsStop; ///< signals the thread to quit
-};

src/GPS/GPSPositionMessage.h

@@ -1,33 +0,0 @@
-/****************************************************************************
- *
- * (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 "vehicle_gps_position.h"
-#include "satellite_info.h"
-#include <QMetaType>
-
-/**
- ** struct GPSPositionMessage
- * wrapper that can be used for Qt signal/slots
- */
-struct GPSPositionMessage
-{
-    sensor_gps_s position_data;
-};
-
-Q_DECLARE_METATYPE(GPSPositionMessage);
-
-
-struct GPSSatelliteMessage
-{
-    satellite_info_s satellite_data;
-};
-Q_DECLARE_METATYPE(GPSSatelliteMessage);

src/GPS/GPSProvider.h

@@ -1,93 +0,0 @@
-/****************************************************************************
- *
- * (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 <QString>
-#include <QThread>
-#include <QByteArray>
-#include <QSerialPort>
-
-#include <atomic>
-
-#include "GPSPositionMessage.h"
-#include "Drivers/src/gps_helper.h"
-
-
-/**
- ** class GPSProvider
- * opens a GPS device and handles the protocol
- */
-class GPSProvider : public QThread
-{
-    Q_OBJECT
-public:
-
-    enum class GPSType {
-        u_blox,
-        trimble,
-        septentrio
-    };
-
-    GPSProvider(const QString& device,
-                GPSType type,
-                bool    enableSatInfo,
-                double  surveyInAccMeters,
-                int     surveryInDurationSecs,
-                bool    useFixedBaseLocation,
-                double  fixedBaseLatitude,
-                double  fixedBaseLongitude,
-                float   fixedBaseAltitudeMeters,
-                float   fixedBaseAccuracyMeters,
-                const std::atomic_bool& requestStop);
-    ~GPSProvider();
-
-    /**
-     * this is called by the callback method
-     */
-    void gotRTCMData(uint8_t *data, size_t len);
-
-signals:
-    void positionUpdate(GPSPositionMessage message);
-    void satelliteInfoUpdate(GPSSatelliteMessage message);
-    void RTCMDataUpdate(QByteArray message);
-    void surveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active);
-
-protected:
-    void run();
-
-private:
-    void publishGPSPosition();
-    void publishGPSSatellite();
-
-	/**
-	 * callback from the driver for the platform specific stuff
-	 */
-	static int callbackEntry(GPSCallbackType type, void *data1, int data2, void *user);
-
-	int callback(GPSCallbackType type, void *data1, int data2);
-
-    QString _device;
-    GPSType _type;
-    const std::atomic_bool& _requestStop;
-    double  _surveyInAccMeters;
-    int     _surveryInDurationSecs;
-    bool    _useFixedBaseLoction;
-    double  _fixedBaseLatitude;
-    double  _fixedBaseLongitude;
-    float   _fixedBaseAltitudeMeters;
-    float   _fixedBaseAccuracyMeters;
-    GPSHelper::GPSConfig _gpsConfig{};
-
-	struct sensor_gps_s        _reportGpsPos;
-	struct satellite_info_s    *_pReportSatInfo = nullptr;
-
-	QSerialPort *_serial = nullptr;
-};

src/GPS/RTCM/RTCMMavlink.cc

@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- * (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 "RTCMMavlink.h"
-
-#include "MultiVehicleManager.h"
-#include "Vehicle.h"
-
-#include <cstdio>
-
-RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox)
-    : _toolbox(toolbox)
-{
-    _bandwidthTimer.start();
-}
-
-void RTCMMavlink::RTCMDataUpdate(QByteArray message)
-{
-    /* statistics */
-    _bandwidthByteCounter += message.size();
-    qint64 elapsed = _bandwidthTimer.elapsed();
-    if (elapsed > 1000) {
-        printf("RTCM bandwidth: %.2f kB/s\n", (float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f);
-        _bandwidthTimer.restart();
-        _bandwidthByteCounter = 0;
-    }
-
-    const qsizetype maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN;
-    mavlink_gps_rtcm_data_t mavlinkRtcmData;
-    memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t));
-
-    if (message.size() < maxMessageLength) {
-        mavlinkRtcmData.len = message.size();
-        mavlinkRtcmData.flags = (_sequenceId & 0x1F) << 3;
-        memcpy(&mavlinkRtcmData.data, message.data(), message.size());
-        sendMessageToVehicle(mavlinkRtcmData);
-    } else {
-        // We need to fragment
-
-        uint8_t fragmentId = 0;         // Fragment id indicates the fragment within a set
-        int start = 0;
-        while (start < message.size()) {
-            int length = std::min(message.size() - start, maxMessageLength);
-            mavlinkRtcmData.flags = 1;                      // LSB set indicates message is fragmented
-            mavlinkRtcmData.flags |= fragmentId++ << 1;     // Next 2 bits are fragment id
-            mavlinkRtcmData.flags |= (_sequenceId & 0x1F) << 3;     // Next 5 bits are sequence id
-            mavlinkRtcmData.len = length;
-            memcpy(&mavlinkRtcmData.data, message.data() + start, length);
-            sendMessageToVehicle(mavlinkRtcmData);
-            start += length;
-        }
-    }
-    ++_sequenceId;
-}
-
-void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg)
-{
-    QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles();
-    MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol();
-    for (int i = 0; i < vehicles.count(); i++) {
-        Vehicle*                vehicle     = qobject_cast<Vehicle*>(vehicles[i]);
-        WeakLinkInterfacePtr    weakLink    = vehicle->vehicleLinkManager()->primaryLink();
-
-        if (!weakLink.expired()) {
-            mavlink_message_t       message;
-            SharedLinkInterfacePtr  sharedLink = weakLink.lock();
-
-            mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(),
-                                                  mavlinkProtocol->getComponentId(),
-                                                  sharedLink->mavlinkChannel(),
-                                                  &message,
-                                                  &msg);
-            vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
-        }
-    }
-}

src/GPS/RTCM/RTCMMavlink.h

@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- * (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 <QObject>
-#include <QElapsedTimer>
-
-#include "QGCToolbox.h"
-#include "MAVLinkProtocol.h"
-
-/**
- ** class RTCMMavlink
- * Receives RTCM updates and sends them via MAVLINK to the device
- */
-class RTCMMavlink : public QObject
-{
-    Q_OBJECT
-public:
-    RTCMMavlink(QGCToolbox& toolbox);
-    //TODO: API to select device(s)?
-
-public slots:
-    void RTCMDataUpdate(QByteArray message);
-
-private:
-    void sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg);
-
-    QGCToolbox& _toolbox;
-    QElapsedTimer _bandwidthTimer;
-    int _bandwidthByteCounter = 0;
-    uint8_t _sequenceId = 0;
-};

src/GPS/definitions.h

@@ -1,94 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file definitions.h
- * common platform-specific definitions & abstractions for gps
- * @author Beat Küng <beat-kueng@gmx.net>
- */
-
-#pragma once
-
-#include <QtGlobal>
-
-#define GPS_READ_BUFFER_SIZE 1024
-
-#define GPS_INFO(...) qInfo(__VA_ARGS__)
-#define GPS_WARN(...) qWarning(__VA_ARGS__)
-#define GPS_ERR(...) qCritical(__VA_ARGS__)
-
-#include "vehicle_gps_position.h"
-#include "satellite_info.h"
-
-#define M_DEG_TO_RAD 		(M_PI / 180.0)
-#define M_RAD_TO_DEG 		(180.0 / M_PI)
-#define M_DEG_TO_RAD_F 		0.01745329251994f
-#define M_RAD_TO_DEG_F 		57.2957795130823f
-
-#include <QThread>
-
-class Sleeper : public QThread
-{
-public:
-    static void usleep(unsigned long usecs) { QThread::usleep(usecs); }
-};
-
-static inline void gps_usleep(unsigned long usecs) {
-    Sleeper::usleep(usecs);
-}
-
-typedef uint64_t gps_abstime;
-
-#include <QDateTime>
-/**
- * Get the current time in us. Function signature:
- * uint64_t hrt_absolute_time()
- */
-static inline gps_abstime gps_absolute_time() {
-    //FIXME: is there something with microsecond accuracy?
-    return QDateTime::currentMSecsSinceEpoch() * 1000;
-}
-
-//timespec is UNIX-specific
-#ifdef _WIN32
-#if _MSC_VER < 1900
-struct timespec
-{
-    time_t tv_sec;
-    long tv_nsec;
-};
-#else
-#include <time.h>
-#endif
-#endif
-

src/GPS/satellite_info.h

@@ -1,55 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-#include <stdint.h>
-
-/*
- * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/satellite_info.msg
- * and was manually copied here.
- */
-
-struct satellite_info_s {
-	uint64_t timestamp;
-	uint8_t count;
-	uint8_t svid[20];
-	uint8_t used[20];
-	uint8_t elevation[20];
-	uint8_t azimuth[20];
-	uint8_t snr[20];
-	uint8_t prn[20];
-#ifdef __cplusplus
-	static const uint8_t SAT_INFO_MAX_SATELLITES = 20;
-
-#endif
-};

src/GPS/vehicle_gps_position.h

@@ -1,72 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/* Auto-generated by genmsg_cpp from file /home/beat/px4/src/Firmware/msg/vehicle_gps_position.msg */
-
-
-#pragma once
-#include <stdint.h>
-
-/*
- * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/vehicle_gps_position.msg
- * and was manually copied here.
- */
-
-struct sensor_gps_s {
-	uint64_t timestamp;
-	uint64_t time_utc_usec;
-	int32_t lat;
-	int32_t lon;
-	int32_t alt;
-	int32_t alt_ellipsoid;
-	uint16_t automatic_gain_control;
-	uint8_t jamming_state;
-	float s_variance_m_s;
-	float c_variance_rad;
-	float eph;
-	float epv;
-	float hdop;
-	float vdop;
-	int32_t noise_per_ms;
-	int32_t jamming_indicator;
-	float vel_m_s;
-	float vel_n_m_s;
-	float vel_e_m_s;
-	float vel_d_m_s;
-	float cog_rad;
-	int32_t timestamp_time_relative;
-	float heading;
-	uint8_t fix_type;
-	bool vel_ned_valid;
-	uint8_t satellites_used;
-};

src/GPSManager.cpp

@@ -1,11 +1,4 @@
-/****************************************************************************
- *
- * (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 "GPSManager.h"
@@ -14,36 +7,45 @@
 #include "SettingsManager.h"
 #include "RTKSettings.h"
 
+// GPSManager类的构造函数，它是一个工具类，用于管理GPS连接和数据处理。
 GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox)
-    : QGCTool(app, toolbox)
+    : QGCTool(app, toolbox) // 调用基类QGCTool的构造函数
 {
+    // 注册元类型，以便在信号和槽之间传递GPSPositionMessage和GPSSatelliteMessage类型的对象
     qRegisterMetaType<GPSPositionMessage>();
     qRegisterMetaType<GPSSatelliteMessage>();
 }
 
+// GPSManager类的析构函数
 GPSManager::~GPSManager()
 {
+    // 断开与GPS的连接
     disconnectGPS();
 }
 
+// 连接到指定的GPS设备
 void GPSManager::connectGPS(const QString& device, const QString& gps_type)
 {
+    // 获取RTK设置实例，用于访问RTK相关的配置参数
     RTKSettings* rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings();
 
+    // 根据传入的gps_type确定GPS设备的类型
     GPSProvider::GPSType type;
     if (gps_type.contains("trimble",  Qt::CaseInsensitive)) {
         type = GPSProvider::GPSType::trimble;
-        qCDebug(RTKGPSLog) << "Connecting Trimble device";
+        qCDebug(RTKGPSLog) << "Connecting Trimble device"; // 打印连接Trimble设备的调试信息
     } else if (gps_type.contains("septentrio",  Qt::CaseInsensitive)) {
         type = GPSProvider::GPSType::septentrio;
-        qCDebug(RTKGPSLog) << "Connecting Septentrio device";
+        qCDebug(RTKGPSLog) << "Connecting Septentrio device"; // 打印连接Septentrio设备的调试信息
     } else {
         type = GPSProvider::GPSType::u_blox;
-        qCDebug(RTKGPSLog) << "Connecting U-blox device";
+        qCDebug(RTKGPSLog) << "Connecting U-blox device"; // 打印连接U-blox设备的调试信息
     }
 
+    // 断开现有的GPS连接
     disconnectGPS();
-    _requestGpsStop = false;
+    _requestGpsStop = false; // 清除停止GPS请求的标志
+    // 创建一个新的GPSProvider实例，并传入设备名称和类型以及相关的RTK设置
     _gpsProvider = new GPSProvider(device,
                                    type,
                                    true,    /* enableSatInfo */
@@ -55,46 +57,58 @@ void GPSManager::connectGPS(const QString& device, const QString& gps_type)
                                    rtkSettings->fixedBasePositionAltitude()->rawValue().toFloat(),
                                    rtkSettings->fixedBasePositionAccuracy()->rawValue().toFloat(),
                                    _requestGpsStop);
-    _gpsProvider->start();
+    _gpsProvider->start(); // 启动GPSProvider
 
-    //create RTCM device
+    // 创建RTCM MAVLink设备
     _rtcmMavlink = new RTCMMavlink(*_toolbox);
 
+    // 连接GPSProvider的RTCM数据更新信号到RTCMMavlink的槽
     connect(_gpsProvider, &GPSProvider::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate);
 
-    //test: connect to position update
+    // 连接GPSProvider的位置更新信号到GPSManager的位置更新槽
     connect(_gpsProvider, &GPSProvider::positionUpdate,         this, &GPSManager::GPSPositionUpdate);
+    // 连接GPSProvider的卫星信息更新信号到GPSManager的卫星信息更新槽
     connect(_gpsProvider, &GPSProvider::satelliteInfoUpdate,    this, &GPSManager::GPSSatelliteUpdate);
+    // 连接GPSProvider的完成信号到GPSManager的断开连接槽
     connect(_gpsProvider, &GPSProvider::finished,               this, &GPSManager::onDisconnect);
+    // 连接GPSProvider的测量状态信号到GPSManager的测量状态槽
     connect(_gpsProvider, &GPSProvider::surveyInStatus,         this, &GPSManager::surveyInStatus);
 
+    // 发送连接事件
     emit onConnect();
 }
 
+
+// 断开与GPS的连接
 void GPSManager::disconnectGPS(void)
 {
     if (_gpsProvider) {
-        _requestGpsStop = true;
-        //Note that we need a relatively high timeout to be sure the GPS thread finished.
+        _requestGpsStop = true; // 设置请求停止GPS的标志
+        // 注意，我们需要一个相对较高的超时时间，以确保GPS线程完成。
         if (!_gpsProvider->wait(2000)) {
-            qWarning() << "Failed to wait for GPS thread exit. Consider increasing the timeout";
+            qWarning() << "Failed to wait for GPS thread exit. Consider increasing the timeout"; // 如果等待失败，打印警告信息
         }
-        delete(_gpsProvider);
+        delete(_gpsProvider); // 删除GPSProvider对象
     }
     if (_rtcmMavlink) {
-        delete(_rtcmMavlink);
+        delete(_rtcmMavlink); // 删除RTCMMavlink对象
     }
-    _gpsProvider = nullptr;
-    _rtcmMavlink = nullptr;
+    _gpsProvider = nullptr; // 将GPSProvider指针设置为nullptr
+    _rtcmMavlink = nullptr; // 将RTCMMavlink指针设置为nullptr
 }
 
-
+// 当接收到GPS位置更新时调用此函数
 void GPSManager::GPSPositionUpdate(GPSPositionMessage msg)
 {
     qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.alt).arg(msg.position_data.lon).arg(msg.position_data.lat);
+    // 打印接收到的GPS位置信息
 }
+
+// 当接收到GPS卫星信息更新时调用此函数
 void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg)
 {
     qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count);
-    emit satelliteUpdate(msg.satellite_data.count);
+    // 打印接收到的GPS卫星数量信息
+    emit satelliteUpdate(msg.satellite_data.count); // 发送卫星更新信号
 }
+

src/GPSProvider.cpp

@@ -1,11 +1,4 @@
-/****************************************************************************
- *
- * (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 "GPSProvider.h"

src/Joystick/CMakeLists.txt

@@ -0,0 +1,40 @@
+
+set(EXTRA_SRC)
+
+if (ANDROID)
+	list(APPEND EXTRA_SRC
+		JoystickAndroid.cc
+	)
+endif()
+
+add_library(Joystick
+	Joystick.cc
+	JoystickManager.cc
+	JoystickSDL.cc
+	JoystickMavCommand.cc
+	${EXTRA_SRC}
+)
+
+target_link_libraries(Joystick
+        PRIVATE
+            ui
+	PUBLIC
+		qgc
+		ui
+)
+
+target_include_directories(Joystick PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+if(WIN32)
+	target_link_libraries(Joystick PUBLIC sdl2)
+else()
+	find_package(SDL2 REQUIRED)
+	if (IS_DIRECTORY "${SDL2_INCLUDE_DIRS}")
+		include_directories(${SDL2_INCLUDE_DIRS})
+		string(STRIP "${SDL2_LIBRARIES}" SDL2_LIBRARIES)
+		target_link_libraries(Joystick PRIVATE ${SDL2_LIBRARIES})
+	else()
+		include_directories(${SDL2_DIR})
+		target_link_libraries(Joystick PRIVATE SDL2::SDL2)
+	endif()
+endif()

src/Joystick/Joystick.h

@@ -0,0 +1,370 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+/// @file
+/// @brief  Joystick Controller
+
+#pragma once
+
+#include <QObject>
+#include <QThread>
+#include <atomic>
+
+#include "QGCLoggingCategory.h"
+#include "Vehicle.h"
+#include "MultiVehicleManager.h"
+#include "JoystickMavCommand.h"
+
+// JoystickLog Category declaration moved to QGCLoggingCategory.cc to allow access in Vehicle
+Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog)
+Q_DECLARE_METATYPE(GRIPPER_ACTIONS)
+
+/// Action assigned to button
+class AssignedButtonAction : public QObject {
+    Q_OBJECT
+public:
+    AssignedButtonAction(QObject* parent, const QString name);
+    QString action;
+    QElapsedTimer buttonTime;
+    bool    repeat = false;
+};
+
+/// Assignable Button Action
+class AssignableButtonAction : public QObject {
+    Q_OBJECT
+public:
+    AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_ = false);
+    Q_PROPERTY(QString  action      READ action     CONSTANT)
+    Q_PROPERTY(bool     canRepeat   READ canRepeat  CONSTANT)
+    QString action      () { return _action; }
+    bool    canRepeat   () const{ return _repeat; }
+private:
+    QString _action;
+    bool    _repeat = false;
+};
+
+/// Joystick Controller
+class Joystick : public QThread
+{
+    Q_OBJECT
+public:
+    Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager);
+
+    virtual ~Joystick();
+
+    typedef struct Calibration_t {
+        int     min;
+        int     max;
+        int     center;
+        int     deadband;
+        bool    reversed;
+        Calibration_t()
+            : min(-32767)
+            , max(32767)
+            , center(0)
+            , deadband(0)
+            , reversed(false) {}
+    } Calibration_t;
+
+    typedef enum {
+        rollFunction,
+        pitchFunction,
+        yawFunction,
+        throttleFunction,
+        gimbalPitchFunction,
+        gimbalYawFunction,
+        maxFunction
+    } AxisFunction_t;
+
+    typedef enum {
+        ThrottleModeCenterZero,
+        ThrottleModeDownZero,
+        ThrottleModeMax
+    } ThrottleMode_t;
+
+    Q_PROPERTY(QString  name                    READ name                   CONSTANT)
+    Q_PROPERTY(bool     calibrated              MEMBER _calibrated          NOTIFY calibratedChanged)
+    Q_PROPERTY(int      totalButtonCount        READ totalButtonCount       CONSTANT)
+    Q_PROPERTY(int      axisCount               READ axisCount              CONSTANT)
+    Q_PROPERTY(bool     requiresCalibration     READ requiresCalibration    CONSTANT)
+
+    //-- Actions assigned to buttons
+    Q_PROPERTY(QStringList buttonActions        READ buttonActions          NOTIFY buttonActionsChanged)
+
+    //-- Actions that can be assigned to buttons
+    Q_PROPERTY(QmlObjectListModel* assignableActions    READ assignableActions          NOTIFY      assignableActionsChanged)
+    Q_PROPERTY(QStringList assignableActionTitles       READ assignableActionTitles     NOTIFY      assignableActionsChanged)
+    Q_PROPERTY(QString  disabledActionName              READ disabledActionName         CONSTANT)
+
+    Q_PROPERTY(int      throttleMode            READ throttleMode           WRITE setThrottleMode       NOTIFY throttleModeChanged)
+    Q_PROPERTY(float    axisFrequencyHz         READ axisFrequencyHz        WRITE setAxisFrequency      NOTIFY axisFrequencyHzChanged)
+    Q_PROPERTY(float    minAxisFrequencyHz      MEMBER _minAxisFrequencyHz                              CONSTANT)
+    Q_PROPERTY(float    maxAxisFrequencyHz      MEMBER _minAxisFrequencyHz                              CONSTANT)
+    Q_PROPERTY(float    buttonFrequencyHz       READ buttonFrequencyHz      WRITE setButtonFrequency    NOTIFY buttonFrequencyHzChanged)
+    Q_PROPERTY(float    minButtonFrequencyHz    MEMBER _minButtonFrequencyHz                            CONSTANT)
+    Q_PROPERTY(float    maxButtonFrequencyHz    MEMBER _maxButtonFrequencyHz                            CONSTANT)
+    Q_PROPERTY(bool     negativeThrust          READ negativeThrust         WRITE setNegativeThrust     NOTIFY negativeThrustChanged)
+    Q_PROPERTY(float    exponential             READ exponential            WRITE setExponential        NOTIFY exponentialChanged)
+    Q_PROPERTY(bool     accumulator             READ accumulator            WRITE setAccumulator        NOTIFY accumulatorChanged)
+    Q_PROPERTY(bool     circleCorrection        READ circleCorrection       WRITE setCircleCorrection   NOTIFY circleCorrectionChanged)
+
+    Q_INVOKABLE void    setButtonRepeat     (int button, bool repeat);
+    Q_INVOKABLE bool    getButtonRepeat     (int button);
+    Q_INVOKABLE void    setButtonAction     (int button, const QString& action);
+    Q_INVOKABLE QString getButtonAction     (int button);
+
+    // Property accessors
+
+    QString     name                () { return _name; }
+    int         totalButtonCount    () const{ return _totalButtonCount; }
+    int         axisCount           () const{ return _axisCount; }
+    QStringList buttonActions       ();
+
+    QmlObjectListModel* assignableActions   () { return &_assignableButtonActions; }
+    QStringList assignableActionTitles      () { return _availableActionTitles; }
+    QString     disabledActionName          () { return _buttonActionNone; }
+
+    /// Start the polling thread which will in turn emit joystick signals
+    void startPolling(Vehicle* vehicle);
+    void stopPolling(void);
+
+    void setCalibration(int axis, Calibration_t& calibration);
+    Calibration_t getCalibration(int axis);
+
+    void setFunctionAxis(AxisFunction_t function, int axis);
+    int getFunctionAxis(AxisFunction_t function);
+
+    void stop();
+
+/*
+    // Joystick index used by sdl library
+    // Settable because sdl library remaps indices after certain events
+    virtual int index(void) = 0;
+    virtual void setIndex(int index) = 0;
+*/
+	virtual bool requiresCalibration(void) { return true; }
+
+    int   throttleMode      ();
+    void  setThrottleMode   (int mode);
+
+    bool  negativeThrust    () const;
+    void  setNegativeThrust (bool allowNegative);
+
+    float exponential       () const;
+    void  setExponential    (float expo);
+
+    bool  accumulator       () const;
+    void  setAccumulator    (bool accu);
+
+    bool  deadband          () const;
+    void  setDeadband       (bool accu);
+
+    bool  circleCorrection  () const;
+    void  setCircleCorrection(bool circleCorrection);
+
+    void  setTXMode         (int mode);
+    int   getTXMode         () { return _transmitterMode; }
+
+    /// Set the current calibration mode
+    void  setCalibrationMode (bool calibrating);
+
+    /// Get joystick message rate (in Hz)
+    float axisFrequencyHz     () const{ return _axisFrequencyHz; }
+    /// Set joystick message rate (in Hz)
+    void  setAxisFrequency  (float val);
+
+    /// Get joystick button repeat rate (in Hz)
+    float buttonFrequencyHz   () const{ return _buttonFrequencyHz; }
+    /// Set joystick button repeat rate (in Hz)
+    void  setButtonFrequency(float val);
+
+signals:
+    // The raw signals are only meant for use by calibration
+    void rawAxisValueChanged        (int index, int value);
+    void rawButtonPressedChanged    (int index, int pressed);
+    void calibratedChanged          (bool calibrated);
+    void buttonActionsChanged       ();
+    void assignableActionsChanged   ();
+    void throttleModeChanged        (int mode);
+    void negativeThrustChanged      (bool allowNegative);
+    void exponentialChanged         (float exponential);
+    void accumulatorChanged         (bool accumulator);
+    void enabledChanged             (bool enabled);
+    void circleCorrectionChanged    (bool circleCorrection);
+    void axisValues                 (float roll, float pitch, float yaw, float throttle);
+
+    void axisFrequencyHzChanged     ();
+    void buttonFrequencyHzChanged   ();
+    void startContinuousZoom        (int direction);
+    void stopContinuousZoom         ();
+    void stepZoom                   (int direction);
+    void stepCamera                 (int direction);
+    void stepStream                 (int direction);
+    void triggerCamera              ();
+    void startVideoRecord           ();
+    void stopVideoRecord            ();
+    void toggleVideoRecord          ();
+    void gimbalPitchStep            (int direction);
+    void gimbalYawStep              (int direction);
+    void centerGimbal               ();
+    void gimbalControlValue         (double pitch, double yaw);
+    void setArmed                   (bool arm);
+    void setVtolInFwdFlight         (bool set);
+    void setFlightMode              (const QString& flightMode);
+    void emergencyStop              ();
+    /**
+     * @brief Send MAV_CMD_DO_GRIPPER command to the vehicle
+     * 
+     * @param gripperAction (Open / Close) Gripper action to command
+     */
+    void gripperAction              (GRIPPER_ACTIONS gripperAction);
+
+protected:
+    void    _setDefaultCalibration  ();
+    void    _saveSettings           ();
+    void    _saveButtonSettings     ();
+    void    _loadSettings           ();
+    float   _adjustRange            (int value, Calibration_t calibration, bool withDeadbands);
+    void    _executeButtonAction    (const QString& action, bool buttonDown);
+    int     _findAssignableButtonAction(const QString& action);
+    bool    _validAxis              (int axis) const;
+    bool    _validButton            (int button) const;
+    void    _handleAxis             ();
+    void    _handleButtons          ();
+    void    _buildActionList        (Vehicle* activeVehicle);
+
+    void    _pitchStep              (int direction);
+    void    _yawStep                (int direction);
+    double  _localYaw       = 0.0;
+    double  _localPitch     = 0.0;
+
+private:
+    virtual bool _open      ()          = 0;
+    virtual void _close     ()          = 0;
+    virtual bool _update    ()          = 0;
+
+    virtual bool _getButton (int i)      = 0;
+    virtual int  _getAxis   (int i)      = 0;
+    virtual bool _getHat    (int hat,int i) = 0;
+
+    void _updateTXModeSettingsKey(Vehicle* activeVehicle);
+    int _mapFunctionMode(int mode, int function);
+    void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]);
+
+    // Override from QThread
+    virtual void run();
+
+protected:
+
+    enum {
+        BUTTON_UP,
+        BUTTON_DOWN,
+        BUTTON_REPEAT
+    };
+
+    static const float  _defaultAxisFrequencyHz;
+    static const float  _defaultButtonFrequencyHz;
+
+    uint8_t*_rgButtonValues         = nullptr;
+
+    std::atomic<bool> _exitThread{false};    ///< true: signal thread to exit
+    bool    _calibrationMode        = false;
+    int*    _rgAxisValues           = nullptr;
+    Calibration_t* _rgCalibration   = nullptr;
+    ThrottleMode_t _throttleMode    = ThrottleModeDownZero;
+    bool    _negativeThrust         = false;
+    float   _exponential            = 0;
+    bool    _accumulator            = false;
+    bool    _deadband               = false;
+    bool    _circleCorrection       = true;
+    float   _axisFrequencyHz        = _defaultAxisFrequencyHz;
+    float   _buttonFrequencyHz      = _defaultButtonFrequencyHz;
+    Vehicle* _activeVehicle         = nullptr;
+
+    bool    _pollingStartedForCalibration = false;
+
+    QString _name;
+    bool    _calibrated;
+    int     _axisCount;
+    int     _buttonCount;
+    int     _hatCount;
+    int     _hatButtonCount;
+    int     _totalButtonCount;
+
+    static int          _transmitterMode;
+    int                 _rgFunctionAxis[maxFunction] = {};
+    QElapsedTimer       _axisTime;
+
+    QmlObjectListModel              _assignableButtonActions;
+    QList<AssignedButtonAction*>    _buttonActionArray;
+    QStringList                     _availableActionTitles;
+    MultiVehicleManager*            _multiVehicleManager = nullptr;
+
+    QList<JoystickMavCommand> _customMavCommands;
+
+    static const float  _minAxisFrequencyHz;
+    static const float  _maxAxisFrequencyHz;
+    static const float  _minButtonFrequencyHz;
+    static const float  _maxButtonFrequencyHz;
+
+private:
+    static const char*  _rgFunctionSettingsKey[maxFunction];
+
+    static const char* _settingsGroup;
+    static const char* _calibratedSettingsKey;
+    static const char* _buttonActionNameKey;
+    static const char* _buttonActionRepeatKey;
+    static const char* _throttleModeSettingsKey;
+    static const char* _negativeThrustSettingsKey;
+    static const char* _exponentialSettingsKey;
+    static const char* _accumulatorSettingsKey;
+    static const char* _deadbandSettingsKey;
+    static const char* _circleCorrectionSettingsKey;
+    static const char* _axisFrequencySettingsKey;
+    static const char* _buttonFrequencySettingsKey;
+    static const char* _txModeSettingsKey;
+    static const char* _fixedWingTXModeSettingsKey;
+    static const char* _multiRotorTXModeSettingsKey;
+    static const char* _roverTXModeSettingsKey;
+    static const char* _vtolTXModeSettingsKey;
+    static const char* _submarineTXModeSettingsKey;
+
+    static const char* _buttonActionNone;
+    static const char* _buttonActionArm;
+    static const char* _buttonActionDisarm;
+    static const char* _buttonActionToggleArm;
+    static const char* _buttonActionVTOLFixedWing;
+    static const char* _buttonActionVTOLMultiRotor;
+    static const char* _buttonActionStepZoomIn;
+    static const char* _buttonActionStepZoomOut;
+    static const char* _buttonActionContinuousZoomIn;
+    static const char* _buttonActionContinuousZoomOut;
+    static const char* _buttonActionNextStream;
+    static const char* _buttonActionPreviousStream;
+    static const char* _buttonActionNextCamera;
+    static const char* _buttonActionPreviousCamera;
+    static const char* _buttonActionTriggerCamera;
+    static const char* _buttonActionStartVideoRecord;
+    static const char* _buttonActionStopVideoRecord;
+    static const char* _buttonActionToggleVideoRecord;
+    static const char* _buttonActionGimbalDown;
+    static const char* _buttonActionGimbalUp;
+    static const char* _buttonActionGimbalLeft;
+    static const char* _buttonActionGimbalRight;
+    static const char* _buttonActionGimbalCenter;
+    static const char* _buttonActionEmergencyStop;
+    static const char* _buttonActionGripperGrab;
+    static const char* _buttonActionGripperRelease;
+
+
+private slots:
+    void _activeVehicleChanged(Vehicle* activeVehicle);
+    void _vehicleCountChanged(int count);
+    void _flightModesChanged();
+};

src/Joystick/JoystickAndroid.cc

@@ -0,0 +1,293 @@
+#include "JoystickAndroid.h"
+
+#include "QGCApplication.h"
+
+#include <QQmlEngine>
+
+int JoystickAndroid::_androidBtnListCount;
+int *JoystickAndroid::_androidBtnList;
+int JoystickAndroid::ACTION_DOWN;
+int JoystickAndroid::ACTION_UP;
+QMutex JoystickAndroid::m_mutex;
+
+static void clear_jni_exception()
+{
+    QAndroidJniEnvironment jniEnv;
+    if (jniEnv->ExceptionCheck()) {
+        jniEnv->ExceptionDescribe();
+        jniEnv->ExceptionClear();
+    }
+}
+
+JoystickAndroid::JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager)
+    : Joystick(name,axisCount,buttonCount,0,multiVehicleManager)
+    , deviceId(id)
+{
+    int i;
+    
+    QAndroidJniEnvironment env;
+    QAndroidJniObject inputDevice = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDevice", "(I)Landroid/view/InputDevice;", id);
+
+    //set button mapping (number->code)
+    jintArray b = env->NewIntArray(_androidBtnListCount);
+    env->SetIntArrayRegion(b,0,_androidBtnListCount,_androidBtnList);
+
+    QAndroidJniObject btns = inputDevice.callObjectMethod("hasKeys", "([I)[Z", b);
+    jbooleanArray jSupportedButtons = btns.object<jbooleanArray>();
+    jboolean* supportedButtons = env->GetBooleanArrayElements(jSupportedButtons, nullptr);
+    //create a mapping table (btnCode) that maps button number with button code
+    btnValue = new bool[_buttonCount];
+    btnCode = new int[_buttonCount];
+    int c = 0;
+    for (i = 0; i < _androidBtnListCount; i++) {
+        if (supportedButtons[i]) {
+            btnValue[c] = false;
+            btnCode[c] = _androidBtnList[i];
+            c++;
+        }
+    }
+
+    env->ReleaseBooleanArrayElements(jSupportedButtons, supportedButtons, 0);
+
+    // set axis mapping (number->code)
+    axisValue = new int[_axisCount];
+    axisCode = new int[_axisCount];
+    QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;");
+    for (i = 0; i < _axisCount; i++) {
+        QAndroidJniObject range = rangeListNative.callObjectMethod("get", "(I)Ljava/lang/Object;",i);
+        axisCode[i] = range.callMethod<jint>("getAxis");
+        // Don't allow two axis with the same code
+        for (int j = 0; j < i; j++) {
+            if (axisCode[i] == axisCode[j]) {
+                axisCode[i] = -1;
+                break;
+            }
+        }
+        axisValue[i] = 0;
+    }
+
+    qCDebug(JoystickLog) << "axis:" <<_axisCount << "buttons:" <<_buttonCount;
+    QtAndroidPrivate::registerGenericMotionEventListener(this);
+    QtAndroidPrivate::registerKeyEventListener(this);
+}
+
+JoystickAndroid::~JoystickAndroid() {
+    delete btnCode;
+    delete axisCode;
+    delete btnValue;
+    delete axisValue;
+
+    QtAndroidPrivate::unregisterGenericMotionEventListener(this);
+    QtAndroidPrivate::unregisterKeyEventListener(this);
+}
+
+
+QMap<QString, Joystick*> JoystickAndroid::discover(MultiVehicleManager* _multiVehicleManager) {
+    static QMap<QString, Joystick*> ret;
+
+    QMutexLocker lock(&m_mutex);
+
+    QAndroidJniEnvironment env;
+    QAndroidJniObject o = QAndroidJniObject::callStaticObjectMethod<jintArray>("android/view/InputDevice", "getDeviceIds");
+    jintArray jarr = o.object<jintArray>();
+    int sz = env->GetArrayLength(jarr);
+    jint *buff = env->GetIntArrayElements(jarr, nullptr);
+
+    int SOURCE_GAMEPAD = QAndroidJniObject::getStaticField<jint>("android/view/InputDevice", "SOURCE_GAMEPAD");
+    int SOURCE_JOYSTICK = QAndroidJniObject::getStaticField<jint>("android/view/InputDevice", "SOURCE_JOYSTICK");
+
+    QList<QString> names;
+
+    for (int i = 0; i < sz; ++i) {
+        QAndroidJniObject inputDevice = QAndroidJniObject::callStaticObjectMethod("android/view/InputDevice", "getDevice", "(I)Landroid/view/InputDevice;", buff[i]);
+        int sources = inputDevice.callMethod<jint>("getSources", "()I");
+        if (((sources & SOURCE_GAMEPAD) != SOURCE_GAMEPAD) //check if the input device is interesting to us
+                && ((sources & SOURCE_JOYSTICK) != SOURCE_JOYSTICK)) continue;
+
+        // get id and name
+        QString id = inputDevice.callObjectMethod("getDescriptor", "()Ljava/lang/String;").toString();
+        QString name = inputDevice.callObjectMethod("getName", "()Ljava/lang/String;").toString();
+
+        names.push_back(name);
+
+        if (ret.contains(name)) {
+            continue;
+        }
+
+        // get number of axis
+        QAndroidJniObject rangeListNative = inputDevice.callObjectMethod("getMotionRanges", "()Ljava/util/List;");
+        int axisCount = rangeListNative.callMethod<jint>("size");
+
+        // get number of buttons
+        jintArray a = env->NewIntArray(_androidBtnListCount);
+        env->SetIntArrayRegion(a,0,_androidBtnListCount,_androidBtnList);
+        QAndroidJniObject btns = inputDevice.callObjectMethod("hasKeys", "([I)[Z", a);
+        jbooleanArray jSupportedButtons = btns.object<jbooleanArray>();
+        jboolean* supportedButtons = env->GetBooleanArrayElements(jSupportedButtons, nullptr);
+        int buttonCount = 0;
+        for (int j=0;j<_androidBtnListCount;j++)
+            if (supportedButtons[j]) buttonCount++;
+        env->ReleaseBooleanArrayElements(jSupportedButtons, supportedButtons, 0);
+
+        qCDebug(JoystickLog) << "\t" << name << "id:" << buff[i] << "axes:" << axisCount << "buttons:" << buttonCount;
+
+        ret[name] = new JoystickAndroid(name, axisCount, buttonCount, buff[i], _multiVehicleManager);
+    }
+
+    for (auto i = ret.begin(); i != ret.end();) {
+        if (!names.contains(i.key())) {
+            i = ret.erase(i);
+        } else {
+            i++;
+        }
+    }
+
+    env->ReleaseIntArrayElements(jarr, buff, 0);
+
+    return ret;
+}
+
+
+bool JoystickAndroid::handleKeyEvent(jobject event) {
+    QJNIObjectPrivate ev(event);
+    QMutexLocker lock(&m_mutex);
+    const int _deviceId = ev.callMethod<jint>("getDeviceId", "()I");
+    if (_deviceId!=deviceId) return false;
+ 
+    const int action = ev.callMethod<jint>("getAction", "()I");
+    const int keyCode = ev.callMethod<jint>("getKeyCode", "()I");
+
+    for (int i = 0; i <_buttonCount; i++) {
+        if (btnCode[i] == keyCode) {
+            if (action == ACTION_DOWN) btnValue[i] = true;
+            if (action == ACTION_UP)   btnValue[i] = false;
+            return true;
+        }
+    }
+    return false;
+}
+
+bool JoystickAndroid::handleGenericMotionEvent(jobject event) {
+    QJNIObjectPrivate ev(event);
+    QMutexLocker lock(&m_mutex);
+    const int _deviceId = ev.callMethod<jint>("getDeviceId", "()I");
+    if (_deviceId!=deviceId) return false;
+ 
+    for (int i = 0; i <_axisCount; i++) {
+        const float v = ev.callMethod<jfloat>("getAxisValue", "(I)F",axisCode[i]);
+        axisValue[i] = static_cast<int>((v*32767.f));
+    }
+    return true;
+}
+
+bool JoystickAndroid::_open(void) {
+    return true;
+}
+
+void JoystickAndroid::_close(void) {
+}
+
+bool JoystickAndroid::_update(void)
+{
+    return true;
+}
+
+bool JoystickAndroid::_getButton(int i) {
+    return btnValue[ i ];
+}
+
+int JoystickAndroid::_getAxis(int i) {
+    return axisValue[ i ];
+}
+
+bool JoystickAndroid::_getHat(int hat,int i) {
+    Q_UNUSED(hat);
+    Q_UNUSED(i);
+    return false;
+}
+
+static JoystickManager *_manager = nullptr;
+
+//helper method
+bool JoystickAndroid::init(JoystickManager *manager) {
+    _manager = manager;
+
+    //this gets list of all possible buttons - this is needed to check how many buttons our gamepad supports
+    //instead of the whole logic below we could have just a simple array of hardcoded int values as these 'should' not change
+
+    //int JoystickAndroid::_androidBtnListCount;
+    _androidBtnListCount = 31;
+    static int ret[31]; //there are 31 buttons in total accordingy to the API
+    int i;
+    //int *JoystickAndroid::
+    _androidBtnList = ret;
+
+    clear_jni_exception();
+    for (i = 1; i <= 16; i++) {
+        QString name = "KEYCODE_BUTTON_"+QString::number(i);
+        ret[i-1] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", name.toStdString().c_str());
+    }
+    i--;
+
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_A");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_B");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_C");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_L1");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_L2");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_R1");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_R2");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_MODE");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_SELECT");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_START");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBL");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_THUMBR");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_X");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_Y");
+    ret[i++] = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "KEYCODE_BUTTON_Z");
+
+    ACTION_DOWN = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "ACTION_DOWN");
+    ACTION_UP = QAndroidJniObject::getStaticField<jint>("android/view/KeyEvent", "ACTION_UP");
+
+    return true;
+}
+
+static const char kJniClassName[] {"org/mavlink/qgroundcontrol/QGCActivity"};
+
+static void jniUpdateAvailableJoysticks(JNIEnv *envA, jobject thizA)
+{
+    Q_UNUSED(envA);
+    Q_UNUSED(thizA);
+
+    if (_manager != nullptr) {
+        qCDebug(JoystickLog) << "jniUpdateAvailableJoysticks triggered";
+        emit _manager->updateAvailableJoysticksSignal();
+    }
+}
+
+void JoystickAndroid::setNativeMethods()
+{
+    qCDebug(JoystickLog) << "Registering Native Functions";
+
+    //  REGISTER THE C++ FUNCTION WITH JNI
+    JNINativeMethod javaMethods[] {
+        {"nativeUpdateAvailableJoysticks", "()V", reinterpret_cast<void *>(jniUpdateAvailableJoysticks)}
+    };
+
+    clear_jni_exception();
+    QAndroidJniEnvironment jniEnv;
+    jclass objectClass = jniEnv->FindClass(kJniClassName);
+    if(!objectClass) {
+        clear_jni_exception();
+        qWarning() << "Couldn't find class:" << kJniClassName;
+        return;
+    }
+
+    jint val = jniEnv->RegisterNatives(objectClass, javaMethods, sizeof(javaMethods) / sizeof(javaMethods[0]));
+
+    if (val < 0) {
+        qWarning() << "Error registering methods: " << val;
+    } else {
+        qCDebug(JoystickLog) << "Native Functions Registered";
+    }
+    clear_jni_exception();
+}

src/Joystick/JoystickAndroid.h

@@ -0,0 +1,54 @@
+#ifndef JOYSTICKANDROID_H
+#define JOYSTICKANDROID_H
+
+#include "Joystick.h"
+#include "Vehicle.h"
+#include "MultiVehicleManager.h"
+
+#include <jni.h>
+#include <QtCore/private/qjni_p.h>
+#include <QtCore/private/qjnihelpers_p.h>
+#include <QtAndroidExtras/QtAndroidExtras>
+#include <QtAndroidExtras/QAndroidJniObject>
+
+
+class JoystickAndroid : public Joystick, public QtAndroidPrivate::GenericMotionEventListener, public QtAndroidPrivate::KeyEventListener
+{
+public:
+    JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager);
+
+    ~JoystickAndroid();
+
+    static bool init(JoystickManager *manager);
+
+    static void setNativeMethods();
+
+    static QMap<QString, Joystick*> discover(MultiVehicleManager* _multiVehicleManager);
+
+private:
+    bool handleKeyEvent(jobject event);
+    bool handleGenericMotionEvent(jobject event);
+
+    virtual bool _open          ();
+    virtual void _close         ();
+    virtual bool _update        ();
+
+    virtual bool _getButton     (int i);
+    virtual int  _getAxis       (int i);
+    virtual bool _getHat        (int hat,int i);
+
+    int *btnCode;
+    int *axisCode;
+    bool *btnValue;
+    int *axisValue;
+
+    static int * _androidBtnList; //list of all possible android buttons
+    static int _androidBtnListCount;
+
+    static int ACTION_DOWN, ACTION_UP;
+    static QMutex m_mutex;
+
+    int deviceId;
+};
+
+#endif // JOYSTICKANDROID_H

src/Joystick/JoystickManager.cc

@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ * (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 "JoystickManager.h"
+#include "QGCApplication.h"
+
+#include <QQmlEngine>
+
+#ifndef __mobile__
+    #include "JoystickSDL.h"
+    #define __sdljoystick__
+#endif
+
+#ifdef __android__
+    #include "JoystickAndroid.h"
+#endif
+
+QGC_LOGGING_CATEGORY(JoystickManagerLog, "JoystickManagerLog")
+
+const char * JoystickManager::_settingsGroup =              "JoystickManager";
+const char * JoystickManager::_settingsKeyActiveJoystick =  "ActiveJoystick";
+
+JoystickManager::JoystickManager(QGCApplication* app, QGCToolbox* toolbox)
+    : QGCTool(app, toolbox)
+    , _activeJoystick(nullptr)
+    , _multiVehicleManager(nullptr)
+{
+}
+
+JoystickManager::~JoystickManager() {
+    QMap<QString, Joystick*>::iterator i;
+    for (i = _name2JoystickMap.begin(); i != _name2JoystickMap.end(); ++i) {
+        qCDebug(JoystickManagerLog) << "Releasing joystick:" << i.key();
+        i.value()->stop();
+        delete i.value();
+    }
+    qDebug() << "Done";
+}
+
+void JoystickManager::setToolbox(QGCToolbox *toolbox)
+{
+    QGCTool::setToolbox(toolbox);
+
+    _multiVehicleManager = _toolbox->multiVehicleManager();
+
+    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+}
+
+void JoystickManager::init() {
+#ifdef __sdljoystick__
+    if (!JoystickSDL::init()) {
+        return;
+    }
+    _setActiveJoystickFromSettings();
+#elif defined(__android__)
+    if (!JoystickAndroid::init(this)) {
+        return;
+    }
+    connect(this, &JoystickManager::updateAvailableJoysticksSignal, this, &JoystickManager::restartJoystickCheckTimer);
+#endif
+    connect(&_joystickCheckTimer, &QTimer::timeout, this, &JoystickManager::_updateAvailableJoysticks);
+    _joystickCheckTimerCounter = 5;
+    _joystickCheckTimer.start(1000);
+}
+
+void JoystickManager::_setActiveJoystickFromSettings(void)
+{
+    QMap<QString,Joystick*> newMap;
+
+#ifdef __sdljoystick__
+    // Get the latest joystick mapping
+    newMap = JoystickSDL::discover(_multiVehicleManager);
+#elif defined(__android__)
+    newMap = JoystickAndroid::discover(_multiVehicleManager);
+#endif
+
+    if (_activeJoystick && !newMap.contains(_activeJoystick->name())) {
+        qCDebug(JoystickManagerLog) << "Active joystick removed";
+        setActiveJoystick(nullptr);
+    }
+
+    // Check to see if our current mapping contains any joysticks that are not in the new mapping
+    // If so, those joysticks have been unplugged, and need to be cleaned up
+    QMap<QString, Joystick*>::iterator i;
+    for (i = _name2JoystickMap.begin(); i != _name2JoystickMap.end(); ++i) {
+        if (!newMap.contains(i.key())) {
+            qCDebug(JoystickManagerLog) << "Releasing joystick:" << i.key();
+            i.value()->stopPolling();
+            i.value()->wait(1000);
+            i.value()->deleteLater();
+        }
+    }
+
+    _name2JoystickMap = newMap;
+    emit availableJoysticksChanged();
+
+    if (!_name2JoystickMap.count()) {
+        setActiveJoystick(nullptr);
+        return;
+    }
+
+    QSettings settings;
+
+    settings.beginGroup(_settingsGroup);
+    QString name = settings.value(_settingsKeyActiveJoystick).toString();
+
+    if (name.isEmpty()) {
+        name = _name2JoystickMap.first()->name();
+    }
+
+    setActiveJoystick(_name2JoystickMap.value(name, _name2JoystickMap.first()));
+    settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
+}
+
+Joystick* JoystickManager::activeJoystick(void)
+{
+    return _activeJoystick;
+}
+
+void JoystickManager::setActiveJoystick(Joystick* joystick)
+{
+    QSettings settings;
+
+    if (joystick != nullptr && !_name2JoystickMap.contains(joystick->name())) {
+        qCWarning(JoystickManagerLog) << "Set active not in map" << joystick->name();
+        return;
+    }
+
+    if (_activeJoystick == joystick) {
+        return;
+    }
+
+    if (_activeJoystick) {
+        _activeJoystick->stopPolling();
+    }
+
+    _activeJoystick = joystick;
+
+    if (_activeJoystick != nullptr) {
+        qCDebug(JoystickManagerLog) << "Set active:" << _activeJoystick->name();
+
+        settings.beginGroup(_settingsGroup);
+        settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
+    }
+
+    emit activeJoystickChanged(_activeJoystick);
+    emit activeJoystickNameChanged(_activeJoystick?_activeJoystick->name():"");
+}
+
+QVariantList JoystickManager::joysticks(void)
+{
+    QVariantList list;
+
+    for (const QString &name: _name2JoystickMap.keys()) {
+        list += QVariant::fromValue(_name2JoystickMap[name]);
+    }
+
+    return list;
+}
+
+QStringList JoystickManager::joystickNames(void)
+{
+    return _name2JoystickMap.keys();
+}
+
+QString JoystickManager::activeJoystickName(void)
+{
+    return _activeJoystick ? _activeJoystick->name() : QString();
+}
+
+bool JoystickManager::setActiveJoystickName(const QString& name)
+{
+    if (_name2JoystickMap.contains(name)) {
+        setActiveJoystick(_name2JoystickMap[name]);
+        return true;
+    } else {
+        qCWarning(JoystickManagerLog) << "Set active not in map" << name;
+        return false;
+    }
+}
+
+/*
+ * TODO: move this to the right place: JoystickSDL.cc and JoystickAndroid.cc respectively and call through Joystick.cc
+ */
+void JoystickManager::_updateAvailableJoysticks()
+{
+#ifdef __sdljoystick__
+    SDL_Event event;
+    while (SDL_PollEvent(&event)) {
+        switch(event.type) {
+        case SDL_QUIT:
+            qCDebug(JoystickManagerLog) << "SDL ERROR:" << SDL_GetError();
+            break;
+        case SDL_JOYDEVICEADDED:
+            qCDebug(JoystickManagerLog) << "Joystick added:" << event.jdevice.which;
+            _setActiveJoystickFromSettings();
+            break;
+        case SDL_JOYDEVICEREMOVED:
+            qCDebug(JoystickManagerLog) << "Joystick removed:" << event.jdevice.which;
+            _setActiveJoystickFromSettings();
+            break;
+        default:
+            break;
+        }
+    }
+#elif defined(__android__)
+    _joystickCheckTimerCounter--;
+    _setActiveJoystickFromSettings();
+    if (_joystickCheckTimerCounter <= 0) {
+        _joystickCheckTimer.stop();
+    }
+#endif
+}
+
+void JoystickManager::restartJoystickCheckTimer()
+{
+    _joystickCheckTimerCounter = 5;
+    _joystickCheckTimer.start(1000);
+}

src/Joystick/JoystickManager.h

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+/// @file
+/// @brief  Joystick Manager
+
+#pragma once
+
+#include "QGCLoggingCategory.h"
+#include "Joystick.h"
+#include "MultiVehicleManager.h"
+#include "QGCToolbox.h"
+
+#include <QVariantList>
+
+Q_DECLARE_LOGGING_CATEGORY(JoystickManagerLog)
+
+/// Joystick Manager
+class JoystickManager : public QGCTool
+{
+    Q_OBJECT
+
+public:
+    JoystickManager(QGCApplication* app, QGCToolbox* toolbox);
+    ~JoystickManager();
+
+    Q_PROPERTY(QVariantList joysticks READ joysticks NOTIFY availableJoysticksChanged)
+    Q_PROPERTY(QStringList  joystickNames READ joystickNames NOTIFY availableJoysticksChanged)
+
+    Q_PROPERTY(Joystick* activeJoystick READ activeJoystick WRITE setActiveJoystick NOTIFY activeJoystickChanged)
+    Q_PROPERTY(QString activeJoystickName READ activeJoystickName WRITE setActiveJoystickName NOTIFY activeJoystickNameChanged)
+
+    /// List of available joysticks
+    QVariantList joysticks();
+    /// List of available joystick names
+    QStringList joystickNames(void);
+
+    /// Get active joystick
+    Joystick* activeJoystick(void);
+    /// Set active joystick
+    void setActiveJoystick(Joystick* joystick);
+
+    QString activeJoystickName(void);
+    bool setActiveJoystickName(const QString& name);
+
+    void restartJoystickCheckTimer(void);
+
+    // Override from QGCTool
+    virtual void setToolbox(QGCToolbox *toolbox);
+
+public slots:
+    void init();
+
+signals:
+    void activeJoystickChanged(Joystick* joystick);
+    void activeJoystickNameChanged(const QString& name);
+    void availableJoysticksChanged(void);
+    void updateAvailableJoysticksSignal();
+
+private slots:
+    void _updateAvailableJoysticks(void);
+
+private:
+    void _setActiveJoystickFromSettings(void);
+
+private:
+    Joystick*                   _activeJoystick;
+    QMap<QString, Joystick*>    _name2JoystickMap;
+    MultiVehicleManager*        _multiVehicleManager;
+
+    static const char * _settingsGroup;
+    static const char * _settingsKeyActiveJoystick;
+
+    int _joystickCheckTimerCounter;
+    QTimer _joystickCheckTimer;
+};

src/Joystick/JoystickMavCommand.cc

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ * (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 "JoystickMavCommand.h"
+#include "QGCLoggingCategory.h"
+#include "Vehicle.h"
+#include <QJsonDocument>
+#include <QJsonParseError>
+#include <QJsonArray>
+
+QGC_LOGGING_CATEGORY(JoystickMavCommandLog, "JoystickMavCommandLog")
+
+static void parseJsonValue(const QJsonObject& jsonObject, const QString& key, float& param)
+{
+    if (jsonObject.contains(key))
+        param = static_cast<float>(jsonObject.value(key).toDouble());
+}
+
+QList<JoystickMavCommand> JoystickMavCommand::load(const QString& jsonFilename)
+{
+    qCDebug(JoystickMavCommandLog) << "Loading" << jsonFilename;
+    QList<JoystickMavCommand> result;
+
+    QFile jsonFile(jsonFilename);
+    if (!jsonFile.open(QIODevice::ReadOnly | QIODevice::Text)) {
+        qCDebug(JoystickMavCommandLog) << "Could not open" << jsonFilename;
+        return result;
+    }
+
+    QByteArray bytes = jsonFile.readAll();
+    jsonFile.close();
+    QJsonParseError jsonParseError;
+    QJsonDocument doc = QJsonDocument::fromJson(bytes, &jsonParseError);
+    if (jsonParseError.error != QJsonParseError::NoError) {
+        qWarning() << jsonFilename << "Unable to open json document" << jsonParseError.errorString();
+        return result;
+    }
+
+    QJsonObject json = doc.object();
+
+    const int version = json.value("version").toInt();
+    if (version != 1) {
+        qWarning() << jsonFilename << ": invalid version" << version;
+        return result;
+    }
+
+    QJsonValue jsonValue = json.value("commands");
+    if (!jsonValue.isArray()) {
+        qWarning() << jsonFilename << ": 'commands' is not an array";
+        return result;
+    }
+
+    QJsonArray jsonArray = jsonValue.toArray();
+    for (QJsonValue info: jsonArray) {
+        if (!info.isObject()) {
+            qWarning() << jsonFilename << ": 'commands' should contain objects";
+            return result;
+        }
+
+        auto jsonObject = info.toObject();
+        JoystickMavCommand item;
+        if (!jsonObject.contains("id")) {
+            qWarning() << jsonFilename << ": 'id' is required";
+            continue;
+        }
+        item._id = jsonObject.value("id").toInt();
+        if (!jsonObject.contains("name")) {
+            qWarning() << jsonFilename << ": 'name' is required";
+            continue;
+        }
+        item._name = jsonObject.value("name").toString();
+        item._showError = jsonObject.value("showError").toBool();
+        parseJsonValue(jsonObject, "param1", item._param1);
+        parseJsonValue(jsonObject, "param2", item._param2);
+        parseJsonValue(jsonObject, "param3", item._param3);
+        parseJsonValue(jsonObject, "param4", item._param4);
+        parseJsonValue(jsonObject, "param5", item._param5);
+        parseJsonValue(jsonObject, "param6", item._param6);
+        parseJsonValue(jsonObject, "param7", item._param7);
+
+        qCDebug(JoystickMavCommandLog) << jsonObject;
+
+        result.append(item);
+    }
+
+    return result;
+}
+
+void JoystickMavCommand::send(Vehicle* vehicle)
+{
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            static_cast<MAV_CMD>(_id),
+                            _showError,
+                            _param1, _param2, _param3, _param4, _param5, _param6, _param7);
+}

src/Joystick/JoystickMavCommand.h

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+/// @file
+/// @brief  Custom Joystick MAV command
+
+#pragma once
+
+#include <QString>
+#include <QList>
+
+class Vehicle;
+
+/// Custom MAV command
+class JoystickMavCommand
+{
+public:
+    static QList<JoystickMavCommand> load(const QString& jsonFilename);
+    QString name() const { return _name; }
+
+    void send(Vehicle* vehicle);
+private:
+    QString _name;
+    int _id = 0;
+    bool _showError = false;
+    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;
+};
+

src/Joystick/JoystickMavCommands.json

@@ -0,0 +1,17 @@
+{
+    "comment": "Joystick MAV commands",
+    "version": 1,
+
+    "commands": [
+        {
+            "id":                   31010,
+            "name":                 "MAV_CMD_USER_1",
+            "param1":               1.0
+        },
+        {
+            "id":                   31011,
+            "name":                 "MAV_CMD_USER_2",
+            "param1":               0.0
+        }
+    ]
+}

src/Joystick/JoystickSDL.cc

@@ -0,0 +1,178 @@
+#include "JoystickSDL.h"
+
+#include "QGCApplication.h"
+
+#include <QQmlEngine>
+#include <QTextStream>
+
+JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager)
+    : Joystick(name,axisCount,buttonCount,hatCount,multiVehicleManager)
+    , _isGameController(isGameController)
+    , _index(index)
+{
+    if(_isGameController) _setDefaultCalibration();
+}
+
+bool JoystickSDL::init(void) {
+    if (SDL_InitSubSystem(SDL_INIT_GAMECONTROLLER | SDL_INIT_JOYSTICK) < 0) {
+        SDL_JoystickEventState(SDL_ENABLE);
+        qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError();
+        return false;
+    }
+    _loadGameControllerMappings();
+    return true;
+}
+
+QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) {
+    static QMap<QString, Joystick*> ret;
+
+    QMap<QString,Joystick*> newRet;
+
+    // Load available joysticks
+
+    qCDebug(JoystickLog) << "Available joysticks";
+
+    for (int i=0; i<SDL_NumJoysticks(); i++) {
+        QString name = SDL_JoystickNameForIndex(i);
+
+        if (!ret.contains(name)) {
+            int axisCount, buttonCount, hatCount;
+            bool isGameController = SDL_IsGameController(i);
+
+            if (SDL_Joystick* sdlJoystick = SDL_JoystickOpen(i)) {
+                SDL_ClearError();
+                axisCount = SDL_JoystickNumAxes(sdlJoystick);
+                buttonCount = SDL_JoystickNumButtons(sdlJoystick);
+                hatCount = SDL_JoystickNumHats(sdlJoystick);
+                if (axisCount < 0 || buttonCount < 0 || hatCount < 0) {
+                    qCWarning(JoystickLog) << "\t libsdl error parsing joystick features:" << SDL_GetError();
+                }
+                SDL_JoystickClose(sdlJoystick);
+            } else {
+                qCWarning(JoystickLog) << "\t libsdl failed opening joystick" << qPrintable(name) << "error:" << SDL_GetError();
+                continue;
+            }
+
+            qCDebug(JoystickLog) << "\t" << name << "axes:" << axisCount << "buttons:" << buttonCount << "hats:" << hatCount << "isGC:" << isGameController;
+
+            // Check for joysticks with duplicate names and differentiate the keys when necessary.
+            // This is required when using an Xbox 360 wireless receiver that always identifies as
+            // 4 individual joysticks, regardless of how many joysticks are actually connected to the
+            // receiver. Using GUID does not help, all of these devices present the same GUID.
+            QString originalName = name;
+            uint8_t duplicateIdx = 1;
+            while (newRet[name]) {
+                name = QString("%1 %2").arg(originalName).arg(duplicateIdx++);
+            }
+
+            newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager);
+        } else {
+            newRet[name] = ret[name];
+            JoystickSDL *j = static_cast<JoystickSDL*>(newRet[name]);
+            if (j->index() != i) {
+                j->setIndex(i); // This joystick index has been remapped by SDL
+            }
+            // Anything left in ret after we exit the loop has been removed (unplugged) and needs to be cleaned up.
+            // We will handle that in JoystickManager in case the removed joystick was in use.
+            ret.remove(name);
+            qCDebug(JoystickLog) << "\tSkipping duplicate" << name;
+        }
+    }
+
+    if (!newRet.count()) {
+        qCDebug(JoystickLog) << "\tnone found";
+    }
+
+    ret = newRet;
+    return ret;
+}
+
+void JoystickSDL::_loadGameControllerMappings(void) {
+    QFile file(":/db/mapping/joystick/gamecontrollerdb.txt");
+    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
+    {
+        qWarning() << "Couldn't load GameController mapping database.";
+        return;
+    }
+
+    QTextStream s(&file);
+
+    while (!s.atEnd()) {
+        SDL_GameControllerAddMapping(s.readLine().toStdString().c_str());
+    }
+}
+
+bool JoystickSDL::_open(void) {
+    if ( _isGameController ) {
+        sdlController = SDL_GameControllerOpen(_index);
+        sdlJoystick = SDL_GameControllerGetJoystick(sdlController);
+    } else {
+        sdlJoystick = SDL_JoystickOpen(_index);
+    }
+
+    if (!sdlJoystick) {
+        qCWarning(JoystickLog) << "SDL_JoystickOpen failed:" << SDL_GetError();
+        return false;
+    }
+
+    qCDebug(JoystickLog) << "Opened joystick at" << sdlJoystick;
+
+    return true;
+}
+
+void JoystickSDL::_close(void) {
+    if (sdlJoystick == nullptr) {
+        qCDebug(JoystickLog) << "Attempt to close null joystick!";
+        return;
+    }
+
+    qCDebug(JoystickLog) << "Closing" << SDL_JoystickName(sdlJoystick) << "at" << sdlJoystick;
+
+    // We get a segfault if we try to close a joystick that has been detached
+    if (SDL_JoystickGetAttached(sdlJoystick) == SDL_FALSE) {
+        qCDebug(JoystickLog) << "\tJoystick is not attached!";
+    } else {
+
+        if (SDL_JoystickInstanceID(sdlJoystick) != -1) {
+            qCDebug(JoystickLog) << "\tID:" << SDL_JoystickInstanceID(sdlJoystick);
+            // This segfaults so often, and I've spent so much time trying to find the cause and fix it
+            // I think this might be an SDL bug
+            // We are much more stable just commenting this out
+            //SDL_JoystickClose(sdlJoystick);
+        }
+    }
+
+    sdlJoystick   = nullptr;
+    sdlController = nullptr;
+}
+
+bool JoystickSDL::_update(void)
+{
+    SDL_JoystickUpdate();
+    SDL_GameControllerUpdate();
+    return true;
+}
+
+bool JoystickSDL::_getButton(int i) {
+    if (_isGameController) {
+        return SDL_GameControllerGetButton(sdlController, SDL_GameControllerButton(i)) == 1;
+    } else {
+        return SDL_JoystickGetButton(sdlJoystick, i) == 1;
+    }
+}
+
+int JoystickSDL::_getAxis(int i) {
+    if (_isGameController) {
+        return SDL_GameControllerGetAxis(sdlController, SDL_GameControllerAxis(i));
+    } else {
+        return SDL_JoystickGetAxis(sdlJoystick, i);
+    }
+}
+
+bool JoystickSDL::_getHat(int hat, int i) {
+    uint8_t hatButtons[] = {SDL_HAT_UP,SDL_HAT_DOWN,SDL_HAT_LEFT,SDL_HAT_RIGHT};
+    if (i < int(sizeof(hatButtons))) {
+        return (SDL_JoystickGetHat(sdlJoystick, hat) & hatButtons[i]) != 0;
+    }
+    return false;
+}

src/Joystick/JoystickSDL.h

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+/// @file
+/// @brief SDL Joystick Interface
+
+#pragma once
+
+#include "Joystick.h"
+#include "Vehicle.h"
+#include "MultiVehicleManager.h"
+
+#include <SDL.h>
+
+/// @brief SDL Joystick Interface
+class JoystickSDL : public Joystick
+{
+public:
+    JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager);
+
+    static QMap<QString, Joystick*> discover(MultiVehicleManager* _multiVehicleManager); 
+    static bool init(void);
+
+    int index(void) const { return _index; }
+    void setIndex(int index) { _index = index; }
+
+    // This can be uncommented to hide the calibration buttons for gamecontrollers in the future
+    // bool requiresCalibration(void) final { return !_isGameController; }
+
+private:
+    static void _loadGameControllerMappings();
+
+    bool _open      () final;
+    void _close     () final;
+    bool _update    () final;
+
+    bool _getButton (int i) final;
+    int  _getAxis   (int i) final;
+    bool _getHat    (int hat,int i) final;
+
+    SDL_Joystick*       sdlJoystick;
+    SDL_GameController* sdlController;
+
+    bool    _isGameController;
+    int     _index;      ///< Index for SDL_JoystickOpen
+
+};

src/JsonHelper.cpp

@@ -0,0 +1,520 @@
+/****************************************************************************
+ *此代码是QGC开发的重要部分 
+ ****************************************************************************/
+
+#include "JsonHelper.h"
+#include "QGCQGeoCoordinate.h"
+#include "QmlObjectListModel.h"
+#include "MissionCommandList.h"
+#include "FactMetaData.h"
+#include "QGCApplication.h"
+
+#include <QJsonArray>
+#include <QJsonParseError>
+#include <QObject>
+#include <QRegularExpression>
+#include <QRegularExpressionMatch>
+#include <QFile>
+#include <QTranslator>
+
+const char* JsonHelper::jsonVersionKey                      = "version";
+const char* JsonHelper::jsonGroundStationKey                = "groundStation";
+const char* JsonHelper::jsonGroundStationValue              = "QGroundControl";
+const char* JsonHelper::jsonFileTypeKey                     = "fileType";
+const char* JsonHelper::_translateKeysKey                   = "translateKeys";
+const char* JsonHelper::_arrayIDKeysKey                     = "_arrayIDKeys";
+
+bool JsonHelper::validateRequiredKeys(const QJsonObject& jsonObject, const QStringList& keys, QString& errorString)
+{
+    QString missingKeys;
+
+    foreach(const QString& key, keys) {
+        if (!jsonObject.contains(key)) {
+            if (!missingKeys.isEmpty()) {
+                missingKeys += QStringLiteral(", ");
+            }
+            missingKeys += key;
+        }
+    }
+
+    if (missingKeys.length() != 0) {
+        errorString = QObject::tr("The following required keys are missing: %1").arg(missingKeys);
+        return false;
+    }
+
+    return true;
+}
+
+bool JsonHelper::_loadGeoCoordinate(const QJsonValue&   jsonValue,
+                                    bool                altitudeRequired,
+                                    QGeoCoordinate&     coordinate,
+                                    QString&            errorString,
+                                    bool                geoJsonFormat)
+{
+    if (!jsonValue.isArray()) {
+        errorString = QObject::tr("value for coordinate is not array");
+        return false;
+    }
+
+    QJsonArray coordinateArray = jsonValue.toArray();
+    int requiredCount = altitudeRequired ? 3 : 2;
+    if (coordinateArray.count() != requiredCount) {
+        errorString = QObject::tr("Coordinate array must contain %1 values").arg(requiredCount);
+        return false;
+    }
+
+    for (const auto& jsonValue: coordinateArray) {
+        if (jsonValue.type() != QJsonValue::Double && jsonValue.type() != QJsonValue::Null) {
+            errorString = QObject::tr("Coordinate array may only contain double values, found: %1").arg(jsonValue.type());
+            return false;
+        }
+    }
+
+    if (geoJsonFormat) {
+        coordinate = QGeoCoordinate(coordinateArray[1].toDouble(), coordinateArray[0].toDouble());
+    } else {
+        coordinate = QGeoCoordinate(possibleNaNJsonValue(coordinateArray[0]), possibleNaNJsonValue(coordinateArray[1]));
+    }
+    if (altitudeRequired) {
+        coordinate.setAltitude(possibleNaNJsonValue(coordinateArray[2]));
+    }
+
+    return true;
+}
+
+void JsonHelper::_saveGeoCoordinate(const QGeoCoordinate&   coordinate,
+                                    bool                    writeAltitude,
+                                    QJsonValue&             jsonValue,
+                                    bool                    geoJsonFormat)
+{
+    QJsonArray coordinateArray;
+
+    if (geoJsonFormat) {
+        coordinateArray << coordinate.longitude() << coordinate.latitude();
+    } else {
+        coordinateArray << coordinate.latitude() << coordinate.longitude();
+    }
+    if (writeAltitude) {
+        coordinateArray << coordinate.altitude();
+    }
+
+    jsonValue = QJsonValue(coordinateArray);
+}
+
+bool JsonHelper::loadGeoCoordinate(const QJsonValue&    jsonValue,
+                                   bool                 altitudeRequired,
+                                   QGeoCoordinate&      coordinate,
+                                   QString&             errorString,
+                                   bool                 geoJsonFormat)
+{
+    return _loadGeoCoordinate(jsonValue, altitudeRequired, coordinate, errorString, geoJsonFormat);
+}
+
+void JsonHelper::saveGeoCoordinate(const QGeoCoordinate&    coordinate,
+                                   bool                     writeAltitude,
+                                   QJsonValue&              jsonValue)
+{
+    _saveGeoCoordinate(coordinate, writeAltitude, jsonValue, false /* geoJsonFormat */);
+}
+
+bool JsonHelper::loadGeoJsonCoordinate(const QJsonValue& jsonValue,
+                                       bool              altitudeRequired,
+                                       QGeoCoordinate&   coordinate,
+                                       QString&          errorString)
+{
+    return _loadGeoCoordinate(jsonValue, altitudeRequired, coordinate, errorString, true /* geoJsonFormat */);
+}
+
+void JsonHelper::saveGeoJsonCoordinate(const QGeoCoordinate& coordinate,
+                                       bool                  writeAltitude,
+                                       QJsonValue&           jsonValue)
+{
+    _saveGeoCoordinate(coordinate, writeAltitude, jsonValue, true /* geoJsonFormat */);
+}
+
+bool JsonHelper::validateKeyTypes(const QJsonObject& jsonObject, const QStringList& keys, const QList<QJsonValue::Type>& types, QString& errorString)
+{
+    for (int i=0; i<types.count(); i++) {
+        QString valueKey = keys[i];
+        if (jsonObject.contains(valueKey)) {
+            const QJsonValue& jsonValue = jsonObject[valueKey];
+            if (jsonValue.type() == QJsonValue::Null &&  types[i] == QJsonValue::Double) {
+                // Null type signals a NaN on a double value
+                continue;
+            }
+            if (jsonValue.type() != types[i]) {
+                errorString  = QObject::tr("Incorrect value type - key:type:expected %1:%2:%3").arg(valueKey).arg(_jsonValueTypeToString(jsonValue.type())).arg(_jsonValueTypeToString(types[i]));
+                return false;
+            }
+        }
+    }
+
+    return true;
+}
+
+bool JsonHelper::isJsonFile(const QByteArray& bytes, QJsonDocument& jsonDoc, QString& errorString)
+{
+    QJsonParseError parseError;
+
+    jsonDoc = QJsonDocument::fromJson(bytes, &parseError);
+
+    if (parseError.error == QJsonParseError::NoError) {
+        return true;
+    } else {
+        int startPos = qMax(0, parseError.offset - 100);
+        int length = qMin(bytes.length() - startPos, 200);
+        qDebug() << QStringLiteral("Json read error '%1'").arg(bytes.mid(startPos, length).constData());
+        errorString = parseError.errorString();
+        return false;
+    }
+}
+
+bool JsonHelper::isJsonFile(const QString& fileName, QJsonDocument& jsonDoc, QString& errorString)
+{
+    QFile jsonFile(fileName);
+    if (!jsonFile.open(QFile::ReadOnly)) {
+        errorString = tr("File open failed: file:error %1 %2").arg(jsonFile.fileName()).arg(jsonFile.errorString());
+        return false;
+    }
+    QByteArray jsonBytes = jsonFile.readAll();
+    jsonFile.close();
+
+    return isJsonFile(jsonBytes, jsonDoc, errorString);
+}
+
+bool JsonHelper::validateInternalQGCJsonFile(const QJsonObject& jsonObject,
+                                             const QString&     expectedFileType,
+                                             int                minSupportedVersion,
+                                             int                maxSupportedVersion,
+                                             int&               version,
+                                             QString&           errorString)
+{
+    // Validate required keys
+    QList<JsonHelper::KeyValidateInfo> requiredKeys = {
+        { jsonFileTypeKey,       QJsonValue::String, true },
+        { jsonVersionKey,        QJsonValue::Double, true },
+    };
+    if (!JsonHelper::validateKeys(jsonObject, requiredKeys, errorString)) {
+        return false;
+    }
+
+    // Make sure file type is correct
+    QString fileTypeValue = jsonObject[jsonFileTypeKey].toString();
+    if (fileTypeValue != expectedFileType) {
+        errorString = QObject::tr("Incorrect file type key expected:%1 actual:%2").arg(expectedFileType).arg(fileTypeValue);
+        return false;
+    }
+
+    // Version check
+    version = jsonObject[jsonVersionKey].toInt();
+    if (version < minSupportedVersion) {
+        errorString = QObject::tr("File version %1 is no longer supported").arg(version);
+        return false;
+    }
+    if (version > maxSupportedVersion) {
+        errorString = QObject::tr("File version %1 is newer than current supported version %2").arg(version).arg(maxSupportedVersion);
+        return false;
+    }
+
+    return true;
+}
+
+bool JsonHelper::validateExternalQGCJsonFile(const QJsonObject& jsonObject,
+                                             const QString&     expectedFileType,
+                                             int                minSupportedVersion,
+                                             int                maxSupportedVersion,
+                                             int&               version,
+                                             QString&           errorString)
+{
+    // Validate required keys
+    QList<JsonHelper::KeyValidateInfo> requiredKeys = {
+        { jsonGroundStationKey, QJsonValue::String, true },
+    };
+    if (!JsonHelper::validateKeys(jsonObject, requiredKeys, errorString)) {
+        return false;
+    }
+
+    return validateInternalQGCJsonFile(jsonObject, expectedFileType, minSupportedVersion, maxSupportedVersion, version, errorString);
+}
+
+QStringList JsonHelper::_addDefaultLocKeys(QJsonObject& jsonObject)
+{
+    QString translateKeys;
+    QString fileType = jsonObject[jsonFileTypeKey].toString();
+    if (!fileType.isEmpty()) {
+        if (fileType == MissionCommandList::qgcFileType) {
+            if (jsonObject.contains(_translateKeysKey)) {
+                translateKeys = jsonObject[_translateKeysKey].toString();
+            } else {
+                translateKeys = "label,enumStrings,friendlyName,description,category";
+                jsonObject[_translateKeysKey] = translateKeys;
+            }
+            if (!jsonObject.contains(_arrayIDKeysKey)) {
+                jsonObject[_arrayIDKeysKey] = "rawName,comment";
+            }
+        } else if (fileType == FactMetaData::qgcFileType) {
+            if (jsonObject.contains(_translateKeysKey)) {
+                translateKeys = jsonObject[_translateKeysKey].toString();
+            } else {
+                translateKeys = "shortDescription,longDescription,enumStrings";
+                jsonObject[_translateKeysKey] = "shortDescription,longDescription,enumStrings";
+            }
+            if (!jsonObject.contains(_arrayIDKeysKey)) {
+                jsonObject[_arrayIDKeysKey] = "name";
+            }
+        }
+    }
+    return translateKeys.split(",");
+}
+
+QJsonObject JsonHelper::_translateObject(QJsonObject& jsonObject, const QString& translateContext, const QStringList& translateKeys)
+{
+    for (const QString& key: jsonObject.keys()) {
+        if (jsonObject[key].isString()) {
+            QString locString = jsonObject[key].toString();
+            if (translateKeys.contains(key)) {
+                QString disambiguation;
+                QString disambiguationPrefix("#loc.disambiguation#");
+
+                if (locString.startsWith(disambiguationPrefix)) {
+                    locString = locString.right(locString.length() - disambiguationPrefix.length());
+                    int commentEndIndex = locString.indexOf("#");
+                    if (commentEndIndex != -1) {
+                        disambiguation = locString.left(commentEndIndex);
+                        locString = locString.right(locString.length() - disambiguation.length() - 1);
+                    }
+                }
+
+                QString xlatString = qgcApp()->qgcJSONTranslator().translate(translateContext.toUtf8().constData(), locString.toUtf8().constData(), disambiguation.toUtf8().constData());
+                if (!xlatString.isNull()) {
+                    jsonObject[key] = xlatString;
+                }
+            }
+        } else if (jsonObject[key].isArray()) {
+            QJsonArray childJsonArray = jsonObject[key].toArray();
+            jsonObject[key] = _translateArray(childJsonArray, translateContext, translateKeys);
+        } else if (jsonObject[key].isObject()) {
+            QJsonObject childJsonObject = jsonObject[key].toObject();
+            jsonObject[key] = _translateObject(childJsonObject, translateContext, translateKeys);
+        }
+    }
+
+    return jsonObject;
+}
+
+QJsonArray JsonHelper::_translateArray(QJsonArray& jsonArray, const QString& translateContext, const QStringList& translateKeys)
+{
+    for (int i=0; i<jsonArray.count(); i++) {
+        QJsonObject childJsonObject = jsonArray[i].toObject();
+        jsonArray[i] = _translateObject(childJsonObject, translateContext, translateKeys);
+    }
+
+    return jsonArray;
+}
+
+QJsonObject JsonHelper::_translateRoot(QJsonObject& jsonObject, const QString& translateContext, const QStringList& translateKeys)
+{
+    return _translateObject(jsonObject, translateContext, translateKeys);
+}
+
+QJsonObject JsonHelper::openInternalQGCJsonFile(const QString&  jsonFilename,
+                                                const QString&  expectedFileType,
+                                                int             minSupportedVersion,
+                                                int             maxSupportedVersion,
+                                                int             &version,
+                                                QString&        errorString)
+{
+    QFile jsonFile(jsonFilename);
+    if (!jsonFile.open(QIODevice::ReadOnly | QIODevice::Text)) {
+        errorString = tr("Unable to open file: '%1', error: %2").arg(jsonFilename).arg(jsonFile.errorString());
+        return QJsonObject();
+    }
+
+    QByteArray bytes = jsonFile.readAll();
+    jsonFile.close();
+    QJsonParseError jsonParseError;
+    QJsonDocument doc = QJsonDocument::fromJson(bytes, &jsonParseError);
+    if (jsonParseError.error != QJsonParseError::NoError) {
+        errorString = tr("Unable to parse json file: %1 error: %2 offset: %3").arg(jsonFilename).arg(jsonParseError.errorString()).arg(jsonParseError.offset);
+        return QJsonObject();
+    }
+
+    if (!doc.isObject()) {
+        errorString = tr("Root of json file is not object: %1").arg(jsonFilename);
+        return QJsonObject();
+    }
+
+    QJsonObject jsonObject = doc.object();
+    bool success = validateInternalQGCJsonFile(jsonObject, expectedFileType, minSupportedVersion, maxSupportedVersion, version, errorString);
+    if (!success) {
+        errorString = tr("Json file: '%1'. %2").arg(jsonFilename).arg(errorString);
+        return QJsonObject();
+    }
+
+    QStringList translateKeys = _addDefaultLocKeys(jsonObject);
+    QString context = QFileInfo(jsonFile).fileName();
+    return _translateRoot(jsonObject, context, translateKeys);
+}
+
+void JsonHelper::saveQGCJsonFileHeader(QJsonObject&     jsonObject,
+                                       const QString&   fileType,
+                                       int              version)
+{
+    jsonObject[jsonGroundStationKey] = jsonGroundStationValue;
+    jsonObject[jsonFileTypeKey] = fileType;
+    jsonObject[jsonVersionKey] = version;
+}
+
+bool JsonHelper::loadGeoCoordinateArray(const QJsonValue&   jsonValue,
+                                        bool                altitudeRequired,
+                                        QVariantList&       rgVarPoints,
+                                        QString&            errorString)
+{
+    if (!jsonValue.isArray()) {
+        errorString = QObject::tr("value for coordinate array is not array");
+        return false;
+    }
+    QJsonArray rgJsonPoints = jsonValue.toArray();
+
+    rgVarPoints.clear();
+    for (int i=0; i<rgJsonPoints.count(); i++) {
+        QGeoCoordinate coordinate;
+
+        if (!JsonHelper::loadGeoCoordinate(rgJsonPoints[i], altitudeRequired, coordinate, errorString)) {
+            return false;
+        }
+        rgVarPoints.append(QVariant::fromValue(coordinate));
+    }
+
+    return true;
+}
+
+bool JsonHelper::loadGeoCoordinateArray(const QJsonValue&       jsonValue,
+                                        bool                    altitudeRequired,
+                                        QList<QGeoCoordinate>&  rgPoints,
+                                        QString&                errorString)
+{
+    QVariantList rgVarPoints;
+
+    if (!loadGeoCoordinateArray(jsonValue, altitudeRequired, rgVarPoints, errorString)) {
+        return false;
+    }
+
+    rgPoints.clear();
+    for (int i=0; i<rgVarPoints.count(); i++) {
+        rgPoints.append(rgVarPoints[i].value<QGeoCoordinate>());
+    }
+
+    return true;
+}
+
+void JsonHelper::saveGeoCoordinateArray(const QVariantList& rgVarPoints,
+                                        bool                writeAltitude,
+                                        QJsonValue&         jsonValue)
+{
+    QJsonArray rgJsonPoints;
+
+    // Add all points to the array
+    for (int i=0; i<rgVarPoints.count(); i++) {
+        QJsonValue jsonPoint;
+
+        JsonHelper::saveGeoCoordinate(rgVarPoints[i].value<QGeoCoordinate>(), writeAltitude, jsonPoint);
+        rgJsonPoints.append(jsonPoint);
+    }
+
+    jsonValue = rgJsonPoints;
+}
+
+void JsonHelper::saveGeoCoordinateArray(const QList<QGeoCoordinate>&    rgPoints,
+                                        bool                            writeAltitude,
+                                        QJsonValue&                     jsonValue)
+{
+    QVariantList rgVarPoints;
+
+    for (int i=0; i<rgPoints.count(); i++) {
+        rgVarPoints.append(QVariant::fromValue(rgPoints[i]));
+    }
+    return saveGeoCoordinateArray(rgVarPoints, writeAltitude, jsonValue);
+}
+
+bool JsonHelper::validateKeys(const QJsonObject& jsonObject, const QList<JsonHelper::KeyValidateInfo>& keyInfo, QString& errorString)
+{
+    QStringList             keyList;
+    QList<QJsonValue::Type> typeList;
+
+    for (int i=0; i<keyInfo.count(); i++) {
+        if (keyInfo[i].required) {
+            keyList.append(keyInfo[i].key);
+        }
+    }
+    if (!validateRequiredKeys(jsonObject, keyList, errorString)) {
+        return false;
+    }
+
+    keyList.clear();
+    for (int i=0; i<keyInfo.count(); i++) {
+        keyList.append(keyInfo[i].key);
+        typeList.append(keyInfo[i].type);
+    }
+    return validateKeyTypes(jsonObject, keyList, typeList, errorString);
+}
+
+QString JsonHelper::_jsonValueTypeToString(QJsonValue::Type type)
+{
+    const struct {
+        QJsonValue::Type    type;
+        const char*         string;
+    } rgTypeToString[] = {
+    { QJsonValue::Null,         "NULL" },
+    { QJsonValue::Bool,         "Bool" },
+    { QJsonValue::Double,       "Double" },
+    { QJsonValue::String,       "String" },
+    { QJsonValue::Array,        "Array" },
+    { QJsonValue::Object,       "Object" },
+    { QJsonValue::Undefined,    "Undefined" },
+};
+
+    for (size_t i=0; i<sizeof(rgTypeToString)/sizeof(rgTypeToString[0]); i++) {
+        if (type == rgTypeToString[i].type) {
+            return rgTypeToString[i].string;
+        }
+    }
+
+    return QObject::tr("Unknown type: %1").arg(type);
+}
+
+bool JsonHelper::loadPolygon(const QJsonArray& polygonArray, QmlObjectListModel& list, QObject* parent, QString& errorString)
+{
+    for (int i=0; i<polygonArray.count(); i++) {
+        const QJsonValue& pointValue = polygonArray[i];
+
+        QGeoCoordinate pointCoord;
+        if (!JsonHelper::loadGeoCoordinate(pointValue, false /* altitudeRequired */, pointCoord, errorString, true)) {
+            list.clearAndDeleteContents();
+            return false;
+        }
+        list.append(new QGCQGeoCoordinate(pointCoord, parent));
+    }
+
+    return true;
+}
+
+void JsonHelper::savePolygon(QmlObjectListModel& list, QJsonArray& polygonArray)
+{
+    for (int i=0; i<list.count(); i++) {
+        QGeoCoordinate vertex = list.value<QGCQGeoCoordinate*>(i)->coordinate();
+
+        QJsonValue jsonValue;
+        JsonHelper::saveGeoCoordinate(vertex, false /* writeAltitude */, jsonValue);
+        polygonArray.append(jsonValue);
+    }
+}
+
+double JsonHelper::possibleNaNJsonValue(const  QJsonValue& value)
+{
+    if (value.type() == QJsonValue::Null) {
+        return std::numeric_limits<double>::quiet_NaN();
+    } else {
+        return value.toDouble();
+    }
+}

src/PairingManager/PairingManager.cc

@@ -0,0 +1,575 @@
+/****************************************************************************
+ *
+ *   (c) 2019 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 "PairingManager.h"
+#include "SettingsManager.h"
+#include "MicrohardManager.h"
+#include "QGCApplication.h"
+#include "QGCCorePlugin.h"
+
+#include <QSettings>
+#include <QJsonObject>
+#include <QStandardPaths>
+#include <QMutexLocker>
+
+QGC_LOGGING_CATEGORY(PairingManagerLog, "PairingManagerLog")
+
+static const char* jsonFileName = "pairing.json";
+
+//-----------------------------------------------------------------------------
+static QString
+random_string(uint length)
+{
+    auto randchar = []() -> char
+    {
+        const char charset[] =
+            "0123456789"
+            "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+            "abcdefghijklmnopqrstuvwxyz";
+        const uint max_index = (sizeof(charset) - 1);
+        return charset[static_cast<uint>(rand()) % max_index];
+    };
+    std::string str(length, 0);
+    std::generate_n(str.begin(), length, randchar);
+    return QString::fromStdString(str);
+}
+
+//-----------------------------------------------------------------------------
+PairingManager::PairingManager(QGCApplication* app, QGCToolbox* toolbox)
+    : QGCTool(app, toolbox)
+    , _aes("J6+KuWh9K2!hG(F'", 0x368de30e8ec063ce)
+{
+    _jsonFileName = QDir::temp().filePath(jsonFileName);
+    connect(this, &PairingManager::parsePairingJson, this, &PairingManager::_parsePairingJson);
+    connect(this, &PairingManager::setPairingStatus, this, &PairingManager::_setPairingStatus);
+    connect(this, &PairingManager::startUpload, this, &PairingManager::_startUpload);
+}
+
+//-----------------------------------------------------------------------------
+PairingManager::~PairingManager()
+{
+}
+
+//-----------------------------------------------------------------------------
+
+void
+PairingManager::setToolbox(QGCToolbox *toolbox)
+{
+    QGCTool::setToolbox(toolbox);
+    _updatePairedDeviceNameList();
+    emit pairedListChanged();
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_pairingCompleted(QString name)
+{
+    _writeJson(_jsonDoc, _pairingCacheFile(name));
+    _remotePairingMap["NM"] = name;
+    _lastPaired = name;
+    _updatePairedDeviceNameList();
+    emit pairedListChanged();
+    emit pairedVehicleChanged();
+    //_app->informationMessageBoxOnMainThread("", tr("Paired with %1").arg(name));
+    setPairingStatus(PairingSuccess, tr("Pairing Successfull"));
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_connectionCompleted(QString /*name*/)
+{
+    //QString pwd = _remotePairingMap["PWD"].toString();
+    //_toolbox->microhardManager()->switchToConnectionEncryptionKey(pwd);
+    //_app->informationMessageBoxOnMainThread("", tr("Connected to %1").arg(name));
+    setPairingStatus(PairingConnected, tr("Connection Successfull"));
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_startUpload(QString pairURL, QJsonDocument jsonDoc)
+{
+    QMutexLocker lock(&_uploadMutex);
+    if (_uploadManager != nullptr) {
+        return;
+    }
+    _uploadManager = new QNetworkAccessManager(this);
+
+    QString str = jsonDoc.toJson(QJsonDocument::JsonFormat::Compact);
+    qCDebug(PairingManagerLog) << "Starting upload to: " << pairURL << " " << str;
+    _uploadData = QString::fromStdString(_aes.encrypt(str.toStdString()));
+    _uploadURL = pairURL;
+    _startUploadRequest();
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_startUploadRequest()
+{
+    QNetworkRequest req;
+    req.setUrl(QUrl(_uploadURL));
+    req.setHeader(QNetworkRequest::ContentTypeHeader, "application/x-www-form-urlencoded");
+    QNetworkReply *reply = _uploadManager->post(req, _uploadData.toUtf8());
+    connect(reply, &QNetworkReply::finished, this, &PairingManager::_uploadFinished);
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_stopUpload()
+{
+    QMutexLocker lock(&_uploadMutex);
+    if (_uploadManager != nullptr) {
+        delete _uploadManager;
+        _uploadManager = nullptr;
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_uploadFinished()
+{
+    QMutexLocker lock(&_uploadMutex);
+    QNetworkReply* reply = qobject_cast<QNetworkReply*>(QObject::sender());
+    if (reply) {
+        if (_uploadManager != nullptr) {
+            if (reply->error() == QNetworkReply::NoError) {
+                qCDebug(PairingManagerLog) << "Upload finished.";
+                QByteArray bytes = reply->readAll();
+                QString str = QString::fromUtf8(bytes.data(), bytes.size());
+                qCDebug(PairingManagerLog) << "Reply: " << str;
+                auto a = str.split(QRegExp("\\s+"));
+                if (a[0] == "Accepted" && a.length() > 1) {
+                    _pairingCompleted(a[1]);
+                } else if (a[0] == "Connected" && a.length() > 1) {
+                    _connectionCompleted(a[1]);
+                } else if (a[0] == "Connection" && a.length() > 1) {
+                    setPairingStatus(PairingConnectionRejected, tr("Connection Rejected"));
+                    qCDebug(PairingManagerLog) << "Connection error: " << str;
+                } else {
+                    setPairingStatus(PairingRejected, tr("Pairing Rejected"));
+                    qCDebug(PairingManagerLog) << "Pairing error: " << str;
+                }
+                _uploadManager->deleteLater();
+                _uploadManager = nullptr;
+            } else {
+                if(++_pairRetryCount > 3) {
+                    qCDebug(PairingManagerLog) << "Giving up";
+                    setPairingStatus(PairingError, tr("No Response From Vehicle"));
+                    _uploadManager->deleteLater();
+                    _uploadManager = nullptr;
+                } else {
+                    qCDebug(PairingManagerLog) << "Upload error: " + reply->errorString();
+                    _startUploadRequest();
+                }
+            }
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_parsePairingJsonFile()
+{
+    QFile file(_jsonFileName);
+    file.open(QIODevice::ReadOnly | QIODevice::Text);
+    QString json = file.readAll();
+    file.remove();
+    file.close();
+
+    jsonReceived(json);
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::connectToPairedDevice(QString name)
+{
+    setPairingStatus(PairingConnecting, tr("Connecting to %1").arg(name));
+    QFile file(_pairingCacheFile(name));
+    file.open(QIODevice::ReadOnly | QIODevice::Text);
+    QString json = file.readAll();
+    jsonReceived(json);
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::removePairedDevice(QString name)
+{
+    QFile file(_pairingCacheFile(name));
+    file.remove();
+    _updatePairedDeviceNameList();
+    emit pairedListChanged();
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_updatePairedDeviceNameList()
+{
+    _deviceList.clear();
+    QDirIterator it(_pairingCacheDir().absolutePath(), QDir::Files);
+    while (it.hasNext()) {
+        QFileInfo fileInfo(it.next());
+        _deviceList.append(fileInfo.fileName());
+        qCDebug(PairingManagerLog) << "Listing: " << fileInfo.fileName();
+    }
+}
+
+//-----------------------------------------------------------------------------
+QString
+PairingManager::_assumeMicrohardPairingJson()
+{
+    QJsonDocument json;
+    QJsonObject   jsonObject;
+
+    jsonObject.insert("LT", "MH");
+    jsonObject.insert("IP", "192.168.168.10");
+    jsonObject.insert("AIP", _toolbox->microhardManager()->remoteIPAddr());
+    jsonObject.insert("CU",  _toolbox->microhardManager()->configUserName());
+    jsonObject.insert("CP",  _toolbox->microhardManager()->configPassword());
+    jsonObject.insert("EK",  _toolbox->microhardManager()->encryptionKey());
+    json.setObject(jsonObject);
+
+    return QString(json.toJson(QJsonDocument::Compact));
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_parsePairingJson(QString jsonEnc)
+{
+    QString json = QString::fromStdString(_aes.decrypt(jsonEnc.toStdString()));
+    if (json == "") {
+        json = jsonEnc;
+    }
+    qCDebug(PairingManagerLog) << "Parsing JSON: " << json;
+
+    _jsonDoc = QJsonDocument::fromJson(json.toUtf8());
+
+    if (_jsonDoc.isNull()) {
+        setPairingStatus(PairingError, tr("Invalid Pairing File"));
+        qCDebug(PairingManagerLog) << "Failed to create Pairing JSON doc.";
+        return;
+    }
+    if (!_jsonDoc.isObject()) {
+        setPairingStatus(PairingError, tr("Error Parsing Pairing File"));
+        qCDebug(PairingManagerLog) << "Pairing JSON is not an object.";
+        return;
+    }
+
+    QJsonObject jsonObj = _jsonDoc.object();
+
+    if (jsonObj.isEmpty()) {
+        setPairingStatus(PairingError, tr("Error Parsing Pairing File"));
+        qCDebug(PairingManagerLog) << "Pairing JSON object is empty.";
+        return;
+    }
+
+    _remotePairingMap = jsonObj.toVariantMap();
+    QString linkType  = _remotePairingMap["LT"].toString();
+    QString pport     = _remotePairingMap["PP"].toString();
+    if (pport.length()==0) {
+        pport = "29351";
+    }
+
+    if (linkType.length()==0) {
+        setPairingStatus(PairingError, tr("Error Parsing Pairing File"));
+        qCDebug(PairingManagerLog) << "Pairing JSON is malformed.";
+        return;
+    }
+
+    _toolbox->microhardManager()->switchToPairingEncryptionKey();
+
+    QString pairURL = "http://" + _remotePairingMap["IP"].toString() + ":" + pport;
+    bool connecting = jsonObj.contains("PWD");
+    QJsonDocument jsonDoc;
+
+    if (!connecting) {
+        pairURL +=  + "/pair";
+        QString pwd = random_string(8);
+        // TODO generate certificates
+        QString cert1 = "";
+        QString cert2 = "";
+        jsonObj.insert("PWD", pwd);
+        jsonObj.insert("CERT1", cert1);
+        jsonObj.insert("CERT2", cert2);
+        _jsonDoc.setObject(jsonObj);
+        if (linkType == "ZT") {
+            jsonDoc = _createZeroTierPairingJson(cert1);
+        } else if (linkType == "MH") {
+            jsonDoc = _createMicrohardPairingJson(pwd, cert1);
+        }
+    } else {
+        pairURL +=  + "/connect";
+        QString cert2 = _remotePairingMap["CERT2"].toString();
+        if (linkType == "ZT") {
+            jsonDoc = _createZeroTierConnectJson(cert2);
+        } else if (linkType == "MH") {
+            jsonDoc = _createMicrohardConnectJson(cert2);
+        }
+    }
+
+    if (linkType == "ZT") {
+        _toolbox->settingsManager()->appSettings()->enableMicrohard()->setRawValue(false);
+        _toolbox->settingsManager()->appSettings()->enableTaisync()->setRawValue(false);
+        emit startUpload(pairURL, jsonDoc);
+    } else if (linkType == "MH") {
+        _toolbox->settingsManager()->appSettings()->enableMicrohard()->setRawValue(true);
+        _toolbox->settingsManager()->appSettings()->enableTaisync()->setRawValue(false);
+        if (_remotePairingMap.contains("AIP")) {
+            _toolbox->microhardManager()->setRemoteIPAddr(_remotePairingMap["AIP"].toString());
+        }
+        if (_remotePairingMap.contains("CU")) {
+            _toolbox->microhardManager()->setConfigUserName(_remotePairingMap["CU"].toString());
+        }
+        if (_remotePairingMap.contains("CP")) {
+            _toolbox->microhardManager()->setConfigPassword(_remotePairingMap["CP"].toString());
+        }
+        if (_remotePairingMap.contains("EK") && !connecting) {
+            _toolbox->microhardManager()->setEncryptionKey(_remotePairingMap["EK"].toString());
+        }
+        _toolbox->microhardManager()->updateSettings();
+        emit startUpload(pairURL, jsonDoc);
+    }
+}
+
+//-----------------------------------------------------------------------------
+QString
+PairingManager::_getLocalIPInNetwork(QString remoteIP, int num)
+{
+    QStringList pieces = remoteIP.split(".");
+    QString ipPrefix = "";
+    for (int i = 0; i<num && i<pieces.length(); i++) {
+        ipPrefix += pieces[i] + ".";
+    }
+
+    const QHostAddress &localhost = QHostAddress(QHostAddress::LocalHost);
+    for (const QHostAddress &address: QNetworkInterface::allAddresses()) {
+        if (address.protocol() == QAbstractSocket::IPv4Protocol && address != localhost) {
+            if (address.toString().startsWith(ipPrefix)) {
+                return address.toString();
+            }
+        }
+    }
+
+    return "";
+}
+
+//-----------------------------------------------------------------------------
+QDir
+PairingManager::_pairingCacheDir()
+{
+    const QString spath(QFileInfo(QSettings().fileName()).dir().absolutePath());
+    QDir dir = spath + QDir::separator() + "PairingCache";
+    if (!dir.exists()) {
+        dir.mkpath(".");
+    }
+
+    return dir;
+}
+
+//-----------------------------------------------------------------------------
+QString
+PairingManager::_pairingCacheFile(QString uavName)
+{
+    return _pairingCacheDir().filePath(uavName);
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_writeJson(QJsonDocument &jsonDoc, QString fileName)
+{
+    QString val = jsonDoc.toJson(QJsonDocument::JsonFormat::Compact);
+    qCDebug(PairingManagerLog) << "Write json " << val;
+    QString enc = QString::fromStdString(_aes.encrypt(val.toStdString()));
+
+    QFile file(fileName);
+    file.open(QIODevice::WriteOnly | QIODevice::Text | QIODevice::Truncate);
+    file.write(enc.toUtf8());
+    file.close();
+}
+
+//-----------------------------------------------------------------------------
+QJsonDocument
+PairingManager::_createZeroTierPairingJson(QString cert1)
+{
+    QString localIP = _getLocalIPInNetwork(_remotePairingMap["IP"].toString(), 2);
+
+    QJsonObject jsonObj;
+    jsonObj.insert("LT", "ZT");
+    jsonObj.insert("IP", localIP);
+    jsonObj.insert("P", 14550);
+    jsonObj.insert("CERT1", cert1);
+    return QJsonDocument(jsonObj);
+}
+
+//-----------------------------------------------------------------------------
+QJsonDocument
+PairingManager::_createMicrohardPairingJson(QString pwd, QString cert1)
+{
+    QString localIP = _getLocalIPInNetwork(_remotePairingMap["IP"].toString(), 3);
+
+    QJsonObject jsonObj;
+    jsonObj.insert("LT", "MH");
+    jsonObj.insert("IP", localIP);
+    jsonObj.insert("P", 14550);
+    jsonObj.insert("PWD", pwd);
+    jsonObj.insert("CERT1", cert1);
+    return QJsonDocument(jsonObj);
+}
+
+//-----------------------------------------------------------------------------
+QJsonDocument
+PairingManager::_createZeroTierConnectJson(QString cert2)
+{
+    QString localIP = _getLocalIPInNetwork(_remotePairingMap["IP"].toString(), 2);
+
+    QJsonObject jsonObj;
+    jsonObj.insert("LT", "ZT");
+    jsonObj.insert("IP", localIP);
+    jsonObj.insert("P", 14550);
+    jsonObj.insert("CERT2", cert2);
+    return QJsonDocument(jsonObj);
+}
+
+//-----------------------------------------------------------------------------
+QJsonDocument
+PairingManager::_createMicrohardConnectJson(QString cert2)
+{
+    QString localIP = _getLocalIPInNetwork(_remotePairingMap["IP"].toString(), 3);
+
+    QJsonObject jsonObj;
+    jsonObj.insert("LT", "MH");
+    jsonObj.insert("IP", localIP);
+    jsonObj.insert("P", 14550);
+    jsonObj.insert("CERT2", cert2);
+    return QJsonDocument(jsonObj);
+}
+
+//-----------------------------------------------------------------------------
+
+QStringList
+PairingManager::pairingLinkTypeStrings()
+{
+    //-- Must follow same order as enum LinkType in LinkConfiguration.h
+    static QStringList list;
+    int i = 0;
+    if (!list.size()) {
+#if defined QGC_ENABLE_QTNFC
+        list += tr("NFC");
+        _nfcIndex = i++;
+#endif
+#if defined QGC_GST_MICROHARD_ENABLED
+        list += tr("Microhard");
+        _microhardIndex = i++;
+#endif
+    }
+    return list;
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::_setPairingStatus(PairingStatus status, QString statusStr)
+{
+    _status = status;
+    _statusString = statusStr;
+    emit pairingStatusChanged();
+}
+
+//-----------------------------------------------------------------------------
+QString
+PairingManager::pairingStatusStr() const
+{
+    return _statusString;
+}
+
+#if QGC_GST_MICROHARD_ENABLED
+//-----------------------------------------------------------------------------
+void
+PairingManager::startMicrohardPairing()
+{
+    stopPairing();
+    _pairRetryCount = 0;
+    setPairingStatus(PairingActive, tr("Pairing..."));
+    _parsePairingJson(_assumeMicrohardPairingJson());
+}
+#endif
+
+//-----------------------------------------------------------------------------
+void
+PairingManager::stopPairing()
+{
+#if defined QGC_ENABLE_QTNFC
+    pairingNFC.stop();
+#endif
+    _stopUpload();
+    setPairingStatus(PairingIdle, "");
+}
+
+#if defined QGC_ENABLE_QTNFC
+//-----------------------------------------------------------------------------
+void
+PairingManager::startNFCScan()
+{
+    stopPairing();
+    setPairingStatus(PairingActive, tr("Pairing..."));
+    pairingNFC.start();
+}
+
+#endif
+
+//-----------------------------------------------------------------------------
+#ifdef __android__
+static const char kJniClassName[] {"org/mavlink/qgroundcontrol/QGCActivity"};
+
+//-----------------------------------------------------------------------------
+static void jniNFCTagReceived(JNIEnv *envA, jobject thizA, jstring messageA)
+{
+    Q_UNUSED(thizA);
+
+    const char *stringL = envA->GetStringUTFChars(messageA, nullptr);
+    QString ndef = QString::fromUtf8(stringL);
+    envA->ReleaseStringUTFChars(messageA, stringL);
+    if (envA->ExceptionCheck())
+        envA->ExceptionClear();
+    qCDebug(PairingManagerLog) << "NDEF Tag Received: " << ndef;
+    qgcApp()->toolbox()->pairingManager()->jsonReceived(ndef);
+}
+
+//-----------------------------------------------------------------------------
+void PairingManager::setNativeMethods(void)
+{
+    //  REGISTER THE C++ FUNCTION WITH JNI
+    JNINativeMethod javaMethods[] {
+        {"nativeNFCTagReceived", "(Ljava/lang/String;)V", reinterpret_cast<void *>(jniNFCTagReceived)}
+    };
+
+    QAndroidJniEnvironment jniEnv;
+    if (jniEnv->ExceptionCheck()) {
+        jniEnv->ExceptionDescribe();
+        jniEnv->ExceptionClear();
+    }
+
+    jclass objectClass = jniEnv->FindClass(kJniClassName);
+    if(!objectClass) {
+        qWarning() << "Couldn't find class:" << kJniClassName;
+        return;
+    }
+
+    jint val = jniEnv->RegisterNatives(objectClass, javaMethods, sizeof(javaMethods) / sizeof(javaMethods[0]));
+
+    if (val < 0) {
+        qWarning() << "Error registering methods: " << val;
+    } else {
+        qCDebug(PairingManagerLog) << "Native Functions Registered";
+    }
+
+    if (jniEnv->ExceptionCheck()) {
+        jniEnv->ExceptionDescribe();
+        jniEnv->ExceptionClear();
+    }
+}
+#endif
+//-----------------------------------------------------------------------------

src/PairingManager/PairingManager.h

@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   (c) 2019 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 <QJsonDocument>
+#include <QMutex>
+#include <QNetworkReply>
+#include <QTimer>
+#include <QTime>
+#include <QVariantMap>
+
+#include "aes.h"
+#include "QGCToolbox.h"
+#include "QGCLoggingCategory.h"
+#include "Fact.h"
+#if defined QGC_ENABLE_QTNFC
+#include "QtNFC.h"
+#endif
+#ifdef __android__
+#include <jni.h>
+#include <QtAndroidExtras/QtAndroidExtras>
+#include <QtAndroidExtras/QAndroidJniObject>
+#endif
+
+Q_DECLARE_LOGGING_CATEGORY(PairingManagerLog)
+
+class AppSettings;
+class QGCApplication;
+
+//-----------------------------------------------------------------------------
+class PairingManager : public QGCTool
+{
+    Q_OBJECT
+public:
+    explicit PairingManager (QGCApplication* app, QGCToolbox* toolbox);
+    ~PairingManager         () override;
+
+    // Override from QGCTool
+    virtual void setToolbox(QGCToolbox *toolbox) override;
+
+    enum PairingStatus {
+        PairingIdle,
+        PairingActive,
+        PairingSuccess,
+        PairingConnecting,
+        PairingConnected,
+        PairingRejected,
+        PairingConnectionRejected,
+        PairingError
+    };
+
+    Q_ENUM(PairingStatus)
+
+    QStringList     pairingLinkTypeStrings      ();
+    QString         pairingStatusStr            () const;
+    QStringList     pairedDeviceNameList        () { return _deviceList; }
+    PairingStatus   pairingStatus               () { return _status; }
+    QString         pairedVehicle               () { return _lastPaired; }
+    int             nfcIndex                    () { return _nfcIndex; }
+    int             microhardIndex              () { return _microhardIndex; }
+    bool            firstBoot                   () { return _firstBoot; }
+    bool            errorState                  () { return _status == PairingRejected || _status == PairingConnectionRejected || _status == PairingError; }
+    void            setStatusMessage            (PairingStatus status, QString statusStr) { emit setPairingStatus(status, statusStr); }
+    void            jsonReceived                (QString json) { emit parsePairingJson(json); }
+    void            setFirstBoot                (bool set) { _firstBoot = set; emit firstBootChanged(); }
+#ifdef __android__
+    static void     setNativeMethods            (void);
+#endif
+    Q_INVOKABLE void connectToPairedDevice      (QString name);
+    Q_INVOKABLE void removePairedDevice         (QString name);
+
+#if defined defined QGC_ENABLE_QTNFC
+    Q_INVOKABLE void startNFCScan();
+#endif    
+#if QGC_GST_MICROHARD_ENABLED
+    Q_INVOKABLE void startMicrohardPairing();
+#endif
+    Q_INVOKABLE void stopPairing();
+
+    Q_PROPERTY(QString          pairingStatusStr        READ pairingStatusStr       NOTIFY pairingStatusChanged)
+    Q_PROPERTY(PairingStatus    pairingStatus           READ pairingStatus          NOTIFY pairingStatusChanged)
+    Q_PROPERTY(QStringList      pairedDeviceNameList    READ pairedDeviceNameList   NOTIFY pairedListChanged)
+    Q_PROPERTY(QStringList      pairingLinkTypeStrings  READ pairingLinkTypeStrings CONSTANT)
+    Q_PROPERTY(QString          pairedVehicle           READ pairedVehicle          NOTIFY pairedVehicleChanged)
+    Q_PROPERTY(bool             errorState              READ errorState             NOTIFY pairingStatusChanged)
+    Q_PROPERTY(int              nfcIndex                READ nfcIndex               CONSTANT)
+    Q_PROPERTY(int              microhardIndex          READ microhardIndex         CONSTANT)
+    Q_PROPERTY(bool             firstBoot               READ firstBoot              WRITE setFirstBoot  NOTIFY firstBootChanged)
+
+signals:
+    void startUpload                            (QString pairURL, QJsonDocument);
+    void closeConnection                        ();
+    void pairingConfigurationsChanged           ();
+    void nameListChanged                        ();
+    void pairingStatusChanged                   ();
+    void parsePairingJson                       (QString json);
+    void setPairingStatus                       (PairingStatus status, QString pairingStatus);
+    void pairedListChanged                      ();
+    void pairedVehicleChanged                   ();
+    void firstBootChanged                       ();
+
+private slots:
+    void _startUpload                           (QString pairURL, QJsonDocument);
+    void _stopUpload                            ();
+    void _startUploadRequest                    ();
+    void _parsePairingJson                      (QString jsonEnc);
+    void _setPairingStatus                      (PairingStatus status, QString pairingStatus);
+
+private:
+    QString                 _statusString;
+    QString                 _jsonFileName;
+    QString                 _lastPaired;
+    QVariantMap             _remotePairingMap;
+    int                     _nfcIndex = -1;
+    int                     _microhardIndex = -1;
+    int                     _pairRetryCount = 0;
+    PairingStatus           _status = PairingIdle;
+    AES                     _aes;
+    QJsonDocument           _jsonDoc{};
+    QMutex                  _uploadMutex{};
+    QNetworkAccessManager*  _uploadManager = nullptr;
+    QString                 _uploadURL{};
+    QString                 _uploadData{};
+    bool                    _firstBoot = true;
+    QStringList             _deviceList;
+
+    void                    _parsePairingJsonFile       ();
+    QJsonDocument           _createZeroTierConnectJson  (QString cert2);
+    QJsonDocument           _createMicrohardConnectJson (QString cert2);
+    QJsonDocument           _createZeroTierPairingJson  (QString cert1);
+    QJsonDocument           _createMicrohardPairingJson (QString pwd, QString cert1);
+    QString                 _assumeMicrohardPairingJson ();
+    void                    _writeJson                  (QJsonDocument &jsonDoc, QString fileName);
+    QString                 _getLocalIPInNetwork        (QString remoteIP, int num);
+    void                    _uploadFinished             ();
+    void                    _uploadError                (QNetworkReply::NetworkError code);
+    void                    _pairingCompleted           (QString name);
+    void                    _connectionCompleted        (QString name);
+    QDir                    _pairingCacheDir            ();
+    QString                 _pairingCacheFile           (QString uavName);
+    void                    _updatePairedDeviceNameList ();
+
+#if defined QGC_ENABLE_QTNFC
+    PairingNFC              pairingNFC;
+#endif
+};

src/PairingManager/QtNFC.cc

@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ *   (c) 2019 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 "PairingManager.h"
+#include "QtNFC.h"
+#include "QGCApplication.h"
+#include <QSoundEffect>
+
+QGC_LOGGING_CATEGORY(PairingNFCLog, "PairingNFCLog")
+
+#include <QNdefNfcTextRecord>
+
+//-----------------------------------------------------------------------------
+PairingNFC::PairingNFC()
+{
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingNFC::start()
+{
+    if (manager != nullptr) {
+        return;
+    }
+    qgcApp()->toolbox()->pairingManager()->setStatusMessage(tr("Waiting for NFC connection"));
+    qCDebug(PairingNFCLog) << "Waiting for NFC connection";
+
+    manager = new QNearFieldManager(this);
+    if (!manager->isAvailable()) {
+        qWarning() << "NFC not available";
+        delete manager;
+        manager = nullptr;
+        return;
+    }
+
+    QNdefFilter filter;
+    filter.setOrderMatch(false);
+    filter.appendRecord<QNdefNfcTextRecord>(1, UINT_MAX);
+    // type parameter cannot specify substring so filter for "image/" below
+    filter.appendRecord(QNdefRecord::Mime, QByteArray(), 0, 1);
+
+    int result = manager->registerNdefMessageHandler(filter, this, SLOT(handleMessage(QNdefMessage, QNearFieldTarget*)));
+
+    if (result < 0)
+        qWarning() << "Platform does not support NDEF message handler registration";
+
+    manager->startTargetDetection();
+    connect(manager, &QNearFieldManager::targetDetected, this, &PairingNFC::targetDetected);
+    connect(manager, &QNearFieldManager::targetLost, this, &PairingNFC::targetLost);
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingNFC::stop()
+{
+    if (manager != nullptr) {
+        qgcApp()->toolbox()->pairingManager()->setStatusMessage("");
+        qCDebug(PairingNFCLog) << "NFC: Stop";
+        manager->stopTargetDetection();
+        delete manager;
+        manager = nullptr;
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingNFC::targetDetected(QNearFieldTarget *target)
+{
+    if (!target) {
+        return;
+    }
+
+    qgcApp()->toolbox()->pairingManager()->setStatusMessage(tr("Device detected"));
+    qCDebug(PairingNFCLog) << "NFC: Device detected";
+    connect(target, &QNearFieldTarget::ndefMessageRead, this, &PairingNFC::handlePolledNdefMessage);
+    connect(target, SIGNAL(error(QNearFieldTarget::Error,QNearFieldTarget::RequestId)),
+            this, SLOT(targetError(QNearFieldTarget::Error,QNearFieldTarget::RequestId)));
+    connect(target, &QNearFieldTarget::requestCompleted, this, &PairingNFC::handleRequestCompleted);
+
+    manager->setTargetAccessModes(QNearFieldManager::NdefReadTargetAccess);
+    QNearFieldTarget::RequestId id = target->readNdefMessages();
+    if (target->waitForRequestCompleted(id)) {
+        qCDebug(PairingNFCLog) << "requestCompleted ";
+        QVariant res = target->requestResponse(id);
+        qCDebug(PairingNFCLog) << "Response:  " << res.toString();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingNFC::handleRequestCompleted(const QNearFieldTarget::RequestId& id)
+{
+    Q_UNUSED(id);
+    qCDebug(PairingNFCLog) << "handleRequestCompleted ";
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingNFC::targetError(QNearFieldTarget::Error error, const QNearFieldTarget::RequestId& id)
+{
+    Q_UNUSED(id);
+    qCDebug(PairingNFCLog) << "Error: " << error;
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingNFC::targetLost(QNearFieldTarget *target)
+{
+    qgcApp()->toolbox()->pairingManager()->setStatusMessage(tr("Device removed"));
+    qCDebug(PairingNFCLog) << "NFC: Device removed";
+    if (target) {
+        target->deleteLater();
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+PairingNFC::handlePolledNdefMessage(QNdefMessage message)
+{
+    qCDebug(PairingNFCLog) << "NFC: Handle NDEF message";
+//    QNearFieldTarget *target = qobject_cast<QNearFieldTarget *>(sender());
+    for (const QNdefRecord &record : message) {
+        if (record.isRecordType<QNdefNfcTextRecord>()) {
+            QNdefNfcTextRecord textRecord(record);
+            qgcApp()->toolbox()->pairingManager()->jsonReceived(textRecord.text());
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------

src/PairingManager/QtNFC.h

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   (c) 2019 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 <QObject>
+#include <QNdefMessage>
+#include <QNearFieldManager>
+#include <QNearFieldTarget>
+
+#include "QGCLoggingCategory.h"
+
+Q_DECLARE_LOGGING_CATEGORY(PairingNFCLog)
+
+class PairingNFC : public QObject
+{
+    Q_OBJECT
+
+public:
+    PairingNFC();
+
+    void start();
+
+    void stop();
+
+signals:
+    void parsePairingJson(QString json);
+
+public slots:
+    void targetDetected(QNearFieldTarget *target);
+    void targetLost(QNearFieldTarget *target);
+    void handlePolledNdefMessage(QNdefMessage message);
+    void targetError(QNearFieldTarget::Error error, const QNearFieldTarget::RequestId& id);
+    void handleRequestCompleted(const QNearFieldTarget::RequestId& id);
+
+private:
+    bool _exitThread = false;    ///< true: signal thread to exit
+    QNearFieldManager *manager = nullptr;
+};

src/PairingManager/aes.cpp

@@ -0,0 +1,154 @@
+#include "aes.h"
+
+#include <memory>
+#include <openssl/aes.h>
+#include <openssl/bio.h>
+#include <zlib.h>
+
+//-----------------------------------------------------------------------------
+AES::AES(std::string password, unsigned long long salt)
+{
+    int nrounds = 5;
+    unsigned char key[32], iv[32];
+
+    /*
+     * Gen key & IV for AES 256 CBC mode. A SHA1 digest is used to hash the supplied key material.
+     * nrounds is the number of times the we hash the material. More rounds are more secure but
+     * slower.
+     */
+    EVP_BytesToKey(EVP_aes_256_cbc(), EVP_sha1(),
+                   reinterpret_cast<const unsigned char*>(&salt),
+                   reinterpret_cast<const unsigned char*>(password.c_str()),
+                   static_cast<int>(password.length()),
+                   nrounds, key, iv);
+
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+    encCipherContext = EVP_CIPHER_CTX_new();
+    decCipherContext = EVP_CIPHER_CTX_new();
+
+    EVP_CIPHER_CTX_init(encCipherContext);
+    EVP_EncryptInit_ex(encCipherContext, EVP_aes_256_cbc(), nullptr, key, iv);
+    EVP_CIPHER_CTX_init(decCipherContext);
+    EVP_DecryptInit_ex(decCipherContext, EVP_aes_256_cbc(), nullptr, key, iv);
+#else
+    EVP_CIPHER_CTX_init(&encCipherContext);
+    EVP_EncryptInit_ex(&encCipherContext, EVP_aes_256_cbc(), nullptr, key, iv);
+    EVP_CIPHER_CTX_init(&decCipherContext);
+    EVP_DecryptInit_ex(&decCipherContext, EVP_aes_256_cbc(), nullptr, key, iv);
+#endif
+}
+
+//-----------------------------------------------------------------------------
+AES::~AES()
+{
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+    EVP_CIPHER_CTX_free(encCipherContext);
+    EVP_CIPHER_CTX_free(decCipherContext);
+#else
+    EVP_CIPHER_CTX_cleanup(&encCipherContext);
+    EVP_CIPHER_CTX_cleanup(&decCipherContext);
+#endif
+}
+
+//-----------------------------------------------------------------------------
+std::string
+AES::encrypt(std::string plainText)
+{
+    unsigned long sourceLen = static_cast<unsigned long>(plainText.length() + 1);
+    unsigned long destLen = sourceLen * 2;
+    unsigned char* compressed = new unsigned char[destLen];
+    int err = compress2(compressed, &destLen,
+                        reinterpret_cast<const unsigned char *>(plainText.c_str()),
+                        sourceLen, 9);
+    if (err != Z_OK) {
+        return {};
+    }
+
+    int pLen = static_cast<int>(destLen);
+    int cLen = pLen + AES_BLOCK_SIZE;
+    int fLen = 0;
+    unsigned char* cipherText = new unsigned char[cLen];
+
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+    EVP_EncryptInit_ex(encCipherContext, nullptr, nullptr, nullptr, nullptr);
+    EVP_EncryptUpdate(encCipherContext, cipherText, &cLen, compressed, pLen);
+    EVP_EncryptFinal_ex(encCipherContext, cipherText + cLen, &fLen);
+#else
+    EVP_EncryptInit_ex(&encCipherContext, nullptr, nullptr, nullptr, nullptr);
+    EVP_EncryptUpdate(&encCipherContext, cipherText, &cLen, compressed, pLen);
+    EVP_EncryptFinal_ex(&encCipherContext, cipherText + cLen, &fLen);
+#endif
+
+    std::vector<unsigned char> data(cipherText, cipherText + cLen + fLen);
+    std::string res = base64Encode(data);
+    delete[] cipherText;
+    delete[] compressed;
+
+    return res;
+}
+//-----------------------------------------------------------------------------
+std::string
+AES::decrypt(std::string cipherText)
+{
+    int fLen = 0;
+    std::vector<unsigned char> text = base64Decode(cipherText);
+    int pLen = static_cast<int>(text.size());
+    unsigned char* plainText = new unsigned char[pLen];
+
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+    EVP_DecryptInit_ex(decCipherContext, nullptr, nullptr, nullptr, nullptr);
+    EVP_DecryptUpdate(decCipherContext, plainText, &pLen, text.data(), pLen);
+    EVP_DecryptFinal_ex(decCipherContext, plainText + pLen, &fLen);
+#else
+    EVP_DecryptInit_ex(&decCipherContext, nullptr, nullptr, nullptr, nullptr);
+    EVP_DecryptUpdate(&decCipherContext, plainText, &pLen, text.data(), pLen);
+    EVP_DecryptFinal_ex(&decCipherContext, plainText + pLen, &fLen);
+#endif
+
+    unsigned long destLen = static_cast<unsigned long>((pLen + fLen) * 2);
+    unsigned char* uncompressed = new unsigned char[destLen];
+    int err = uncompress(uncompressed, &destLen, plainText, static_cast<unsigned long>(pLen + fLen));
+    if (err != Z_OK) {
+        return {};
+    }
+
+    std::string res(reinterpret_cast<char*>(uncompressed));
+    delete[] uncompressed;
+
+    return res;
+}
+
+//-----------------------------------------------------------------------------
+struct BIOFreeAll { void operator()(BIO* p) { BIO_free_all(p); } };
+
+std::string
+AES::base64Encode(const std::vector<unsigned char>& binary)
+{
+    std::unique_ptr<BIO, BIOFreeAll> b64(BIO_new(BIO_f_base64()));
+    BIO_set_flags(b64.get(), BIO_FLAGS_BASE64_NO_NL);
+    BIO* sink = BIO_new(BIO_s_mem());
+    BIO_push(b64.get(), sink);
+    BIO_write(b64.get(), binary.data(), static_cast<int>(binary.size()));
+    BIO_ctrl(b64.get(), BIO_CTRL_FLUSH, 0, nullptr);
+    const char* encoded;
+    const unsigned long len = static_cast<unsigned long>(BIO_ctrl(sink, BIO_CTRL_INFO, 0, &encoded));
+
+    return std::string(encoded, len);
+}
+
+//-----------------------------------------------------------------------------
+std::vector<unsigned char>
+AES::base64Decode(std::string encoded)
+{
+    std::unique_ptr<BIO, BIOFreeAll> b64(BIO_new(BIO_f_base64()));
+    BIO_set_flags(b64.get(), BIO_FLAGS_BASE64_NO_NL);
+    BIO* source = BIO_new_mem_buf(encoded.c_str(), -1); // read-only source
+    BIO_push(b64.get(), source);
+    const unsigned long maxlen = encoded.length() / 4 * 3 + 1;
+    std::vector<unsigned char> decoded(maxlen);
+    const unsigned long len = static_cast<unsigned long>(BIO_read(b64.get(), decoded.data(), static_cast<int>(maxlen)));
+    decoded.resize(len);
+    return decoded;
+}
+
+//-----------------------------------------------------------------------------

src/PairingManager/aes.h

@@ -0,0 +1,35 @@
+#ifndef AES_H
+#define AES_H
+
+#pragma once
+
+#include <string>
+#include <vector>
+#include <openssl/evp.h>
+
+class AES
+{
+public:
+    AES(std::string password, unsigned long long salt);
+
+    ~AES();
+
+    std::string encrypt(std::string plainText);
+
+    std::string decrypt(std::string cipherText);
+
+private:
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+    EVP_CIPHER_CTX *encCipherContext = nullptr;
+    EVP_CIPHER_CTX *decCipherContext = nullptr;
+#else
+    EVP_CIPHER_CTX encCipherContext;
+    EVP_CIPHER_CTX decCipherContext;
+#endif
+
+    std::string base64Encode(const std::vector<unsigned char>& binary);
+
+    std::vector<unsigned char> base64Decode(std::string encoded);
+};
+
+#endif // AES_H

src/PositionManager/CMakeLists.txt

@@ -0,0 +1,16 @@
+
+add_library(PositionManager
+	PositionManager.cpp
+	SimulatedPosition.cc
+)
+
+target_link_libraries(PositionManager
+	PUBLIC
+		qgc
+)
+
+target_include_directories(PositionManager
+	PUBLIC
+		${CMAKE_CURRENT_SOURCE_DIR}
+	)
+

src/PositionManager/PositionManager.cpp

@@ -0,0 +1,202 @@
+/****************************************************************************
+ *
+ * (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 "PositionManager.h"
+#include "QGCApplication.h"
+#include "QGCCorePlugin.h"
+
+#if !defined(NO_SERIAL_LINK) && !defined(__android__)
+#include <QSerialPortInfo>
+#endif
+
+#include <QtPositioning/private/qgeopositioninfosource_p.h>
+
+QGCPositionManager::QGCPositionManager(QGCApplication* app, QGCToolbox* toolbox)
+    : QGCTool           (app, toolbox)
+{
+
+}
+
+QGCPositionManager::~QGCPositionManager()
+{
+    delete(_simulatedSource);
+    delete(_nmeaSource);
+}
+
+void QGCPositionManager::setToolbox(QGCToolbox *toolbox)
+{
+   QGCTool::setToolbox(toolbox);
+   //-- First see if plugin provides a position source
+   _defaultSource = toolbox->corePlugin()->createPositionSource(this);
+   if (_defaultSource) {
+       _usingPluginSource = true;
+   }
+
+   if (qgcApp()->runningUnitTests()) {
+       // Units test on travis fail due to lack of position source
+       return;
+   }
+
+   if (!_defaultSource) {
+       //-- Otherwise, create a default one
+#if 1
+       // Calling this can end up falling through a path which tries to instantiate a serialnmea source.
+       // The Qt code for this will pop a qWarning if there are no serial ports available. This in turn
+       // causes you to be unable to run with QT_FATAL_WARNINGS=1 to debug stuff.
+       _defaultSource = QGeoPositionInfoSource::createDefaultSource(this);
+#else
+       // So instead we create our own version of QGeoPositionInfoSource::createDefaultSource which isn't as stupid.
+       QList<QCborMap> plugins = QGeoPositionInfoSourcePrivate::pluginsSorted();
+       foreach (const auto &obj, plugins) {
+           if (obj.value("Position").isBool() && obj.value("Position").toBool()) {
+               QString pluginName = obj.value("Keys").toArray()[0].toString();
+               if (pluginName == "serialnmea") {
+#if !defined(NO_SERIAL_LINK) && !defined(__android__)
+                   if (QSerialPortInfo::availablePorts().isEmpty()) {
+                       // This prevents the qWarning from popping
+                       continue;
+                   }
+#else
+                   continue;
+#endif
+               }
+               _defaultSource = QGeoPositionInfoSource::createSource(pluginName, this);
+               if (_defaultSource) {
+                   break;
+               }
+           }
+       }
+#endif
+   }
+   _simulatedSource = new SimulatedPosition();
+
+#if 1
+   setPositionSource(QGCPositionSource::InternalGPS);
+#else
+   setPositionSource(QGCPositionManager::Simulated);
+#endif
+}
+
+void QGCPositionManager::setNmeaSourceDevice(QIODevice* device)
+{
+    // stop and release _nmeaSource
+    if (_nmeaSource) {
+        _nmeaSource->stopUpdates();
+        disconnect(_nmeaSource);
+
+        // if _currentSource is pointing there, point to null
+        if (_currentSource == _nmeaSource){
+            _currentSource = nullptr;
+        }
+
+        delete _nmeaSource;
+        _nmeaSource = nullptr;
+
+    }
+    _nmeaSource = new QNmeaPositionInfoSource(QNmeaPositionInfoSource::RealTimeMode, this);
+    _nmeaSource->setDevice(device);
+    // set equivalent range error to enable position accuracy reporting
+    _nmeaSource->setUserEquivalentRangeError(5.1);
+    setPositionSource(QGCPositionManager::NmeaGPS);
+}
+
+void QGCPositionManager::_positionUpdated(const QGeoPositionInfo &update)
+{
+    _geoPositionInfo = update;
+
+    QGeoCoordinate newGCSPosition = QGeoCoordinate();
+    qreal newGCSHeading = update.attribute(QGeoPositionInfo::Direction);
+
+    if (update.isValid() && update.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) {
+        // Note that gcsPosition filters out possible crap values
+        if (qAbs(update.coordinate().latitude()) > 0.001 &&
+            qAbs(update.coordinate().longitude()) > 0.001 ) {
+            _gcsPositionHorizontalAccuracy = update.attribute(QGeoPositionInfo::HorizontalAccuracy);
+            if (_gcsPositionHorizontalAccuracy <= MinHorizonalAccuracyMeters) {
+                newGCSPosition = update.coordinate();
+            }
+            emit gcsPositionHorizontalAccuracyChanged();
+        }
+    }
+    if (newGCSPosition != _gcsPosition) {
+        _gcsPosition = newGCSPosition;
+        emit gcsPositionChanged(_gcsPosition);
+    }
+
+    // At this point only plugins support gcs heading. The reason is that the quality of heading information from a local
+    // position device (not a compass) is unknown. In many cases it can only be trusted if the GCS location is moving above
+    // a certain rate of speed. When it is not, or the gcs is standing still the heading is just random. We don't want these
+    // random heading to be shown on the fly view. So until we can get a true compass based heading or some smarted heading quality
+    // information which takes into account the speed of movement we normally don't set a heading. We do use the heading though
+    // if the plugin overrides the position source. In that case we assume that it hopefully know what it is doing.
+    if (_usingPluginSource && newGCSHeading != _gcsHeading) {
+        _gcsHeading = newGCSHeading;
+        emit gcsHeadingChanged(_gcsHeading);
+    }
+
+    emit positionInfoUpdated(update);
+}
+
+int QGCPositionManager::updateInterval() const
+{
+    return _updateInterval;
+}
+
+void QGCPositionManager::setPositionSource(QGCPositionManager::QGCPositionSource source)
+{
+    if (_currentSource != nullptr) {
+        _currentSource->stopUpdates();
+        disconnect(_currentSource);
+
+        // Reset all values so we dont get stuck on old values
+        _geoPositionInfo = QGeoPositionInfo();
+        _gcsPosition = QGeoCoordinate();
+        _gcsHeading =       qQNaN();
+        _gcsPositionHorizontalAccuracy = std::numeric_limits<qreal>::infinity();
+
+        emit gcsPositionChanged(_gcsPosition);
+        emit gcsHeadingChanged(_gcsHeading);
+        emit positionInfoUpdated(_geoPositionInfo);
+        emit gcsPositionHorizontalAccuracyChanged();
+    }
+
+    if (qgcApp()->runningUnitTests()) {
+        // Units test on travis fail due to lack of position source
+        return;
+    }
+
+    switch(source) {
+    case QGCPositionManager::Log:
+        break;
+    case QGCPositionManager::Simulated:
+        _currentSource = _simulatedSource;
+        break;
+    case QGCPositionManager::NmeaGPS:
+        _currentSource = _nmeaSource;
+        break;
+    case QGCPositionManager::InternalGPS:
+    default:
+        _currentSource = _defaultSource;
+        break;
+    }
+
+    if (_currentSource != nullptr) {
+        _updateInterval = _currentSource->minimumUpdateInterval();
+        _currentSource->setPreferredPositioningMethods(QGeoPositionInfoSource::SatellitePositioningMethods);
+        _currentSource->setUpdateInterval(_updateInterval);
+        connect(_currentSource, &QGeoPositionInfoSource::positionUpdated, this, &QGCPositionManager::_positionUpdated);
+        connect(_currentSource, &QGeoPositionInfoSource::errorOccurred, this, &QGCPositionManager::_error);
+        _currentSource->startUpdates();
+    }
+}
+
+void QGCPositionManager::_error(QGeoPositionInfoSource::Error positioningError)
+{
+    qWarning() << "QGCPositionManager error" << positioningError;
+}

src/PositionManager/PositionManager.h

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ * (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 <QGeoPositionInfoSource>
+#include <QNmeaPositionInfoSource>
+
+#include <QVariant>
+
+#include "QGCToolbox.h"
+#include "SimulatedPosition.h"
+
+class QGCPositionManager : public QGCTool {
+    Q_OBJECT
+
+public:
+    static constexpr size_t MinHorizonalAccuracyMeters = 100;
+    QGCPositionManager(QGCApplication* app, QGCToolbox* toolbox);
+    ~QGCPositionManager();
+
+    Q_PROPERTY(QGeoCoordinate gcsPosition  READ gcsPosition  NOTIFY gcsPositionChanged)
+    Q_PROPERTY(qreal          gcsHeading   READ gcsHeading   NOTIFY gcsHeadingChanged)
+    Q_PROPERTY(qreal          gcsPositionHorizontalAccuracy  READ gcsPositionHorizontalAccuracy
+                                                             NOTIFY gcsPositionHorizontalAccuracyChanged)
+
+    enum QGCPositionSource {
+        Simulated,
+        InternalGPS,
+        Log,
+        NmeaGPS
+    };
+
+    QGeoCoordinate      gcsPosition         (void) { return _gcsPosition; }
+    qreal               gcsHeading          (void) const{ return _gcsHeading; }
+    qreal               gcsPositionHorizontalAccuracy(void) const { return _gcsPositionHorizontalAccuracy; }
+    QGeoPositionInfo    geoPositionInfo     (void) const { return _geoPositionInfo; }
+    void                setPositionSource   (QGCPositionSource source);
+    int                 updateInterval      (void) const;
+    void                setNmeaSourceDevice (QIODevice* device);
+
+    // Overrides from QGCTool
+    void setToolbox(QGCToolbox* toolbox) override;
+
+
+private slots:
+    void _positionUpdated(const QGeoPositionInfo &update);
+    void _error(QGeoPositionInfoSource::Error positioningError);
+
+signals:
+    void gcsPositionChanged(QGeoCoordinate gcsPosition);
+    void gcsHeadingChanged(qreal gcsHeading);
+    void positionInfoUpdated(QGeoPositionInfo update);
+    void gcsPositionHorizontalAccuracyChanged();
+
+private:
+    int                 _updateInterval =   0;
+    QGeoPositionInfo    _geoPositionInfo;
+    QGeoCoordinate      _gcsPosition;
+    qreal               _gcsHeading =       qQNaN();
+    qreal               _gcsPositionHorizontalAccuracy = std::numeric_limits<qreal>::infinity();
+
+    QGeoPositionInfoSource*     _currentSource =        nullptr;
+    QGeoPositionInfoSource*     _defaultSource =        nullptr;
+    QNmeaPositionInfoSource*    _nmeaSource =           nullptr;
+    QGeoPositionInfoSource*     _simulatedSource =      nullptr;
+    bool                        _usingPluginSource =    false;
+};

src/PositionManager/SimulatedPosition.cc

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * (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 <QtCore>
+#include <QDateTime>
+#include <QDate>
+
+#include "SimulatedPosition.h"
+#include "QGCApplication.h"
+#include "MultiVehicleManager.h"
+
+SimulatedPosition::SimulatedPosition()
+    : QGeoPositionInfoSource(nullptr)
+{
+    _updateTimer.setSingleShot(false);
+
+    // Initialize position to normal PX4 Gazebo home position
+    _lastPosition.setTimestamp(QDateTime::currentDateTime());
+    _lastPosition.setCoordinate(QGeoCoordinate(47.3977420, 8.5455941, 488));
+    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::Direction, _heading);
+    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::GroundSpeed, _horizontalVelocityMetersPerSec);
+    _lastPosition.setAttribute(QGeoPositionInfo::Attribute::VerticalSpeed, _verticalVelocityMetersPerSec);
+
+    // When a vehicle shows up we switch location to the vehicle home position
+    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::vehicleAdded, this, &SimulatedPosition::_vehicleAdded);
+
+    connect(&_updateTimer, &QTimer::timeout, this, &SimulatedPosition::_updatePosition);
+}
+
+QGeoPositionInfo SimulatedPosition::lastKnownPosition(bool /*fromSatellitePositioningMethodsOnly*/) const
+{
+    return _lastPosition;
+}
+
+SimulatedPosition::PositioningMethods SimulatedPosition::supportedPositioningMethods() const
+{
+    return AllPositioningMethods;
+}
+
+void SimulatedPosition::startUpdates(void)
+{
+    _updateTimer.start(qMax(updateInterval(), minimumUpdateInterval()));
+}
+
+void SimulatedPosition::stopUpdates(void)
+{
+    _updateTimer.stop();
+}
+
+void SimulatedPosition::requestUpdate(int /*timeout*/)
+{
+    emit errorOccurred(QGeoPositionInfoSource::UpdateTimeoutError);
+}
+
+void SimulatedPosition::_updatePosition(void)
+{
+    int intervalMsecs = _updateTimer.interval();
+
+    QGeoCoordinate  coord =                 _lastPosition.coordinate();
+    double          horizontalDistance =    _horizontalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
+    double          verticalDistance =      _verticalVelocityMetersPerSec * (1000.0 / static_cast<double>(intervalMsecs));
+
+    _lastPosition.setCoordinate(coord.atDistanceAndAzimuth(horizontalDistance, _heading, verticalDistance));
+
+    emit positionUpdated(_lastPosition);
+}
+
+QGeoPositionInfoSource::Error SimulatedPosition::error() const
+{
+    return QGeoPositionInfoSource::NoError;
+}
+
+void SimulatedPosition::_vehicleAdded(Vehicle* vehicle)
+{
+    if (vehicle->homePosition().isValid()) {
+        _lastPosition.setCoordinate(vehicle->homePosition());
+    } else {
+        connect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged);
+    }
+}
+
+void SimulatedPosition::_vehicleHomePositionChanged(QGeoCoordinate homePosition)
+{
+    Vehicle* vehicle = qobject_cast<Vehicle*>(sender());
+
+    if (homePosition.isValid()) {
+        _lastPosition.setCoordinate(homePosition);
+        disconnect(vehicle, &Vehicle::homePositionChanged, this, &SimulatedPosition::_vehicleHomePositionChanged);
+    }
+}

src/PositionManager/SimulatedPosition.h

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ * (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 <QtPositioning/qgeopositioninfosource.h>
+#include "QGCToolbox.h"
+#include <QTimer>
+
+class Vehicle;
+
+class SimulatedPosition : public QGeoPositionInfoSource
+{
+   Q_OBJECT
+
+public:
+    SimulatedPosition();
+
+    QGeoPositionInfo lastKnownPosition(bool fromSatellitePositioningMethodsOnly = false) const override;
+
+    PositioningMethods  supportedPositioningMethods (void) const override;
+    int                 minimumUpdateInterval       (void) const override { return _updateIntervalMsecs; }
+    Error               error                       (void) const override;
+
+public slots:
+    void startUpdates   (void) override;
+    void stopUpdates    (void) override;
+    void requestUpdate  (int timeout = 5000) override;
+
+private slots:
+    void _updatePosition                (void);
+    void _vehicleAdded                  (Vehicle* vehicle);
+    void _vehicleHomePositionChanged    (QGeoCoordinate homePosition);
+
+private:
+    QTimer              _updateTimer;
+    QGeoPositionInfo    _lastPosition;
+
+    static constexpr int    _updateIntervalMsecs =              1000;
+    static constexpr double _horizontalVelocityMetersPerSec =   0.5;
+    static constexpr double _verticalVelocityMetersPerSec =     0.1;
+    static constexpr double _heading =                          45;
+};

src/ViewLink.h

@@ -0,0 +1,822 @@
+
+
+/**
+ * @file ViewLink.h
+ * @brief ViewLink SDK header file
+ * @details This header file defines ViewLink SDK interfaces and the necessary data structures
+ * @author Edwin
+ * @version 3.0.7
+ * @date    2020-06-28
+ * @copyright Copyright (c) 2020 Shenzhen Viewpro Technology Co., Ltd
+ */
+
+#ifndef __VIEW_LINK_H__
+#define __VIEW_LINK_H__
+
+#if (defined _WIN32) || (defined _WIN64) // windows
+	#ifdef VLK_EXPORTS
+		#define VLK_API extern "C" __declspec(dllexport)
+	#else
+		#define VLK_API extern "C" __declspec(dllimport)
+	#endif
+	#define VLK_CALL __stdcall
+#else
+    #define VLK_API
+	#define VLK_CALL
+#endif
+
+/** @name error code
+ *  @brief usually used as return value of interface function to indicate the operation success or not
+ * @{
+ */
+#define VLK_ERROR_NO_ERROR 0
+#define VLK_ERROR_INVALID_PARAM -1
+/** @} error code */
+
+/** @name ViewLink SDK constant
+ * @{
+ */
+/** the max value of yaw */
+#define VLK_YAW_MAX 180.0
+/** the min value of yaw */
+#define VLK_YAW_MIN -180.0
+/** the max value of pitch */
+#define VLK_PITCH_MAX 90.0
+/** the min value of pitch */
+#define VLK_PITCH_MIN -90.0
+
+/** the yaw max moving speed (20 degrees per second)*/
+#define VLK_MAX_YAW_SPEED 2000.0
+/** the pitch max moving speed (20 degrees per second)*/
+#define VLK_MAX_PITCH_SPEED 2000.0
+
+/** the min zoom (or focus) speed*/
+#define VLK_MIN_ZOOM_SPEED 1
+/** the max zoom (or focus) speed*/
+#define VLK_MAX_ZOOM_SPEED 8
+/** @} ViewLink SDK constant */
+
+/** @name ViewLink SDK data structures
+ * @{
+ */
+
+/**
+ * device connection type
+ */
+typedef enum _VLK_CONN_TYPE
+{
+    VLK_CONN_TYPE_SERIAL_PORT = 0x00, ///< serial port
+    VLK_CONN_TYPE_TCP = 0x01, ///< tcp
+    VLK_CONN_TYPE_UDP = 0x02, ///< udp, not currently supported
+	VLK_CONN_TYPE_BUTT
+}VLK_CONN_TYPE;
+
+/**
+ * TCP connect information
+ */
+typedef struct _VLK_DEV_IPADDR VLK_DEV_IPADDR;
+struct _VLK_DEV_IPADDR
+{
+    char szIPV4[16]; ///< device ipv4 address, e.g: 192.168.2.119
+    int iPort; ///< tcp port, e.g: 2000
+};
+
+/**
+ * serial port connect information
+ */
+typedef struct _VLK_DEV_SERIAL_PORT VLK_DEV_SERIAL_PORT;
+struct _VLK_DEV_SERIAL_PORT
+{
+    char szSerialPortName[16]; ///< serial port name, e.g: /dev/ttyS0 on linux or COM1 on windows
+    int iBaudRate; ///< baudrate, e.g: 115200
+};
+
+/**
+ * device connect parameter
+ */
+typedef struct _VLK_CONN_PARAM VLK_CONN_PARAM;
+struct _VLK_CONN_PARAM
+{
+	VLK_CONN_TYPE emType;
+	union {
+		VLK_DEV_IPADDR IPAddr;
+		VLK_DEV_SERIAL_PORT SerialPort;
+	} ConnParam;
+};
+
+/**
+ * device connection status
+ */
+typedef enum _VLK_CONN_STATUS
+{
+    VLK_CONN_STATUS_CONNECTED = 0x00, ///< deprecated
+    VLK_CONN_STATUS_DISCONNECTED = 0x01, ///< deprecated
+    VLK_CONN_STATUS_TCP_CONNECTED = 0x02, ///< TCP connected
+    VLK_CONN_STATUS_TCP_DISCONNECTED = 0x03, ///< TCP disconnected
+    VLK_CONN_STATUS_SERIAL_PORT_CONNECTED = 0x04, ///< serial port connected
+    VLK_CONN_STATUS_SERIAL_PORT_DISCONNECTED = 0x05, ///< serial port disconnectd
+	VLK_CONN_STATUS_BUTT
+}VLK_CONN_STATUS;
+
+/**
+ * track template size
+ */
+typedef enum _VLK_TRACK_TEMPLATE_SIZE
+{
+	VLK_TRACK_TEMPLATE_SIZE_AUTO = 0,
+	VLK_TRACK_TEMPLATE_SIZE_32 = 32,
+	VLK_TRACK_TEMPLATE_SIZE_64 = 64,
+	VLK_TRACK_TEMPLATE_SIZE_128 = 128,
+	VLK_TRACK_TEMPLATE_SIZE_BUTT
+}VLK_TRACK_TEMPLATE_SIZE;
+
+/**
+ * sensor type
+ */
+typedef enum _VLK_SENSOR
+{
+    VLK_SENSOR_VISIBLE1 = 0, ///< visible light only
+    VLK_SENSOR_IR = 1, ///< IR only
+    VLK_SENSOR_VISIBLE_IR = 2, ///< visible light + IR PIP
+    VLK_SENSOR_IR_VISIBLE = 3, ///< IR + visible light PIP
+    VLK_SENSOR_VISIBLE2 = 4, ///< prime lens visible light
+	VLK_SENSOR_BUTT
+}VLK_SENSOR;
+
+/**
+ * track mode parameter
+ */
+typedef struct _VLK_TRACK_MODE_PARAM VLK_TRACK_MODE_PARAM;
+struct _VLK_TRACK_MODE_PARAM
+{
+	VLK_TRACK_TEMPLATE_SIZE emTrackTempSize;
+	VLK_SENSOR emTrackSensor;
+};
+
+/**
+ * Record mode
+ */
+typedef enum _VLK_RECORD_MODE
+{
+    VLK_RECORD_MODE_NONE = 0, ///< none mode, neither photo nor record mode
+    VLK_RECORD_MODE_PHOTO = 1, ///< photo mode, take photos only
+    VLK_RECORD_MODE_RECORD = 2, ///< record mode, video record only
+	VLK_RECORD_MODE_BUTT
+}VLK_RECORD_MODE;
+
+/**
+ * Focus mode
+ */
+typedef enum _VLK_FOCUS_MODE
+{
+    VLK_FOCUS_MODE_AUTO = 0, ///< automatic focus
+    VLK_FOCUS_MODE_MANU = 1, ///< manual focus
+	VLK_FOCUS_MODE_BUTT
+}VLK_FOCUS_MODE;
+
+/**
+ * Laser zoom mode
+ */
+typedef enum _VLK_LASER_ZOOM_MODE
+{
+    VLK_LASER_ZOOM_MODE_FOLLOW_EO = 0, ///< follow visible light zoom
+    VLK_LASER_ZOOM_MODE_MANU = 1, ///< manual control
+	VLK_LASER_ZOOM_MODE_BUTT
+}VLK_LASER_ZOOM_MODE;
+
+/**
+ * OSD mask
+ */
+typedef enum _VLK_OSD_MASK
+{
+    VLK_OSD_MASK_ENABLE_OSD = 0x1, ///< enable OSD
+    VLK_OSD_MASK_CROSS = 0x2, ///< enable cross frame in center of image
+    VLK_OSD_MASK_PITCH_YAW = 0x4, ///< show pitch and yaw
+    VLK_OSD_MASK_XYSHIFT = 0x8, ///< enable xy shift
+    VLK_OSD_MASK_GPS = 0x10, ///< enable GPS
+    VLK_OSD_MASK_TIME = 0x20, ///< enable time
+    VLK_OSD_MASK_VL_MAG = 0x40, ///< enable VL-MAG
+    VLK_OSD_MASK_BIG_FONT = 0x80 ///< enable big font
+}VLK_OSD_MASK;
+
+/**
+ * OSD input mask
+ */
+typedef enum _VLK_OSD_INPUT_MASK
+{
+    VLK_OSD_INPUT_MASK_PERMANENT_SAVE = 0x1, ///< save OSD configuration, still work after reboot
+    VLK_OSD_INPUT_MASK_TIME = 0x2, ///< enalbe input time
+    VLK_OSD_INPUT_MASK_GPS = 0x4, ///< enable input GPS
+    VLK_OSD_INPUT_MASK_MGRS = 0x8,///< enable input MGRS
+    VLK_OSD_INPUT_MASK_PITCH_YAW = 0x10, ///< enable input pitch and yaw
+    VLK_OSD_INPUT_MASK_VL_MAG = 0x20, ///< enalbe input VL-MAG
+    VLK_OSD_INPUT_MASK_ZOOM_TIMES_OR_FOV = 0x40, ///< display zoom times or FOV
+    VLK_OSD_INPUT_MASK_CHAR_BLACK_BORDER = 0x80 ///< enable character black frame
+}VLK_OSD_INPUT_MASK;
+
+/**
+ * OSD configuration parameter
+ */
+typedef struct _VLK_OSD_PARAM VLK_OSD_PARAM;
+struct _VLK_OSD_PARAM
+{
+    char cOSD; ///< mask of VLK_OSD_MASK, e.g: VLK_OSD_MASK_ENABLE_OSD | VLK_OSD_MASK_CROSS
+    char cOSDInput; ///< mask of VLK_OSD_INPUT_MASK, e.g: VLK_OSD_INPUT_MASK_TIME | VLK_OSD_INPUT_MASK_GPS
+};
+
+/**
+ * Image type
+ */
+typedef enum _VLK_IMAGE_TYPE
+{
+    VLK_IMAGE_TYPE_VISIBLE1, ///< visible light zoom lens
+    VLK_IMAGE_TYPE_VISIBLE2, ///< visible light prime lens
+    VLK_IMAGE_TYPE_IR1, ///< IR zoom lens
+    VLK_IMAGE_TYPE_IR2, ///< IR prime lens
+    VLK_IMAGE_TYPE_FUSION, ///< visible light + IR fusion
+	VLK_IMAGE_TYPE_BUTT
+}VLK_IMAGE_TYPE;
+
+/**
+ * IR color type
+ */
+typedef enum _VLK_IR_COLOR
+{
+	VLK_IR_COLOR_WHITEHOT,
+	VLK_IR_COLOR_BLACKHOT,
+	VLK_IR_COLOR_PSEUDOHOT,
+	VLK_IR_COLOR_RUSTY,
+	VLK_IR_COLOR_BUTT
+}VLK_IR_COLOR;
+
+/**
+ * 2.4G wireless control channels map
+ */
+typedef struct _VLK_CHANNELS_MAP VLK_CHANNELS_MAP;
+struct _VLK_CHANNELS_MAP
+{
+    char cYW; ///< high 4 bits save yaw left channel index, low 4 bits save yaw right channel index
+    char cPT; ///< high 4 bits save pitch up channel index, low 4 bits save pitch down channel index
+    char cMO; ///< high 4 bits save adjust speed channel index, low 4 bits save recenter channel index
+    char cZM; ///< high 4 bits save zoom out channel index, low 4 bits save zoom in channel index
+    char cFC; ///< high 4 bits save focus out channel index, low 4 bits save focus in channel index
+    char cRP; ///< high 4 bits save take photo channel index, low 4 bits save Gimbal record channel index
+    char cMU; ///< high 4 bits save start track channel index, low 4 bits save stop track channel index
+};
+
+/**
+ * Device configuration
+ */
+typedef struct _VLK_DEV_CONFIG VLK_DEV_CONFIG;
+struct _VLK_DEV_CONFIG
+{
+    char cTimeZone; ///< time zone, e.g: 8 (Beijing), 9 (Seoul)
+    char cOSDCfg; ///< OSD configuration, equivalent to cOSD in VLK_OSD_PARAM
+    char cMagneticVariation; ///< Magnetic Variation set https://skyvector.com/   http://www.magnetic-declination.com/
+    char cOSDInput; ///< OSD input configuration, equivalent to cOSDInput in VLK_OSD_PARAM
+
+    /**
+     * @brief Gimbal serial port baudrate \n
+     * 0: 2400 \n
+     * 1: 4800 \n
+     * 2: 9600 \n
+     * 3: 19200 \n
+     * 4: 38400 \n
+     * 5: 57600 \n
+     * 6: 115200 \n
+     * 7: S.BUS mode
+     */
+    char cBaudRate;
+
+    char cEODigitalZoom; ///< EO digital zoom, 1: on; 0: off
+    short sTemperatureAlarmLine;///< temperature alarm, low 8 bits save lower limit, high 8 bits save upper limit
+    char cTrack; ///< track status, 1: enabled, 0: disabled
+
+    /**
+     * @brief laser work mode \n
+     * 0: stop LRF \n
+     * 1: 1HZ get LRF data \n
+     * 2: continuosly get LRF \n
+     * 3: one time get LRF data
+     */
+    char cLaser;
+
+    char cRecordDefinition; ///< record definition, 1: 4k 25fps; 2: 1080P 25fps
+    char cOSDGPS; ///< OSD GPS, 0: OSD GPS is UAV; 1: OSD GPS is target
+
+    /**
+     * @brief s.bus/mavlink channels map set \n
+     * 1: 1~7 \n
+     * 2: 6~12 \n
+     * 3: 8~14 \n
+     * 4: custom channels \n
+     */
+    char cSBUSChnlMap;
+
+    VLK_CHANNELS_MAP ChnlsMap; ///< custom channels map
+    char cFocusHoldSet; ///< deprecated
+    char cCameraType; ///< deprecated
+    char cReserved1[5]; ///< reserved
+    char cRestoreIP; ///< deprecated
+    char cReserved2[5]; ///< reserved
+    char cReserved3[43]; ///< reserved
+    char cVersionNO[20]; ///< firmware version NO
+    char cDeviceID[10]; ///< device model
+    char cSerialNO[22]; ///< serial NO
+};
+
+
+/**
+ * Device model
+ */
+typedef struct _VLK_DEV_MODEL VLK_DEV_MODEL;
+struct _VLK_DEV_MODEL
+{
+    char cModelCode; ///< Gimbal model code
+    char szModelName[32]; ///< Gimbal model name
+};
+
+/**
+ * Tracker status
+ */
+typedef enum _VLK_TRACKER_STATUS
+{
+	VLK_TRACKER_STATUS_STOPPED,
+	VLK_TRACKER_STATUS_SEARCHING,
+	VLK_TRACKER_STATUS_TRACKING,
+	VLK_TRACKER_STATUS_LOST,
+	VLK_TRACKER_STATUS_BUTT
+}VLK_TRACKER_STATUS;
+
+/**
+ * Device telemetry
+ */
+typedef struct _VLK_DEV_TELEMETRY VLK_DEV_TELEMETRY;
+struct _VLK_DEV_TELEMETRY
+{
+    double dYaw; ///< Gimbal current yaw
+    double dPitch; ///< Gimbal current pitch
+    VLK_SENSOR emSensorType; ///< sensor type
+    VLK_TRACKER_STATUS emTrackerStatus; ///< tracker status
+    double dTargetLat; ///< target latitude
+    double dTargetLng; ///< target longitude
+    double dTargetAlt; ///< target altitude
+    double dZoomMagTimes; ///< camera magnification times
+    short sLaserDistance; ///< laser distance
+    VLK_IR_COLOR emIRColor; ///< IR color
+    VLK_RECORD_MODE emRecordMode; ///< record mode
+};
+
+/**
+ * Device status type \n
+ * device status is passed by VLK_DevStatus_CB, \n
+ * VLK_DEV_STATUS_TYPE will be passed to formal paramter iType, \n
+ * and the device status data will be passed to szBuffer, \n
+ * different iType matches different status data struct
+ */
+typedef enum _VLK_DEV_STATUS_TYPE
+{
+    VLK_DEV_STATUS_TYPE_MODEL, ///< matches VLK_DEV_MODEL
+    VLK_DEV_STATUS_TYPE_CONFIG, ///< matches VLK_DEV_CONFIG
+    VLK_DEV_STATUS_TYPE_TELEMETRY, ///< matches VLK_DEV_TELEMETRY
+	VLK_DEV_STATUS_TYPE_BUTT,
+}VLK_DEV_STATUS_TYPE;
+/** @} ViewLink SDK data structures */
+
+/** @name ViewLink SDK callback functions
+ * @{
+ */
+/** @brief connection status callback
+ *  @param iConnStatus connection status, it must be one of VLK_CONN_STATUS
+ *  @param szMessage extended string message
+ *  @param iMsgLen extended string message length
+ *  @return return 0 anyway
+ *  @details VLK_ConnStatus_CB will be called by SDK once the connection status changed,
+ *           for example, developers should check connect result in this callback function
+ * @see VLK_Connect
+ */
+typedef int (*VLK_ConnStatus_CB)(int iConnStatus, const char* szMessage, int iMsgLen, void* pUserParam);
+
+/** @brief device status callback
+ *  @param iType device status type, it must be one of VLK_DEV_STATUS_TYPE
+ *  @param szBuffer device status data, it is an address of a struct matchs iType, e.g: VLK_DEV_MODEL
+ *  @param iBufLen device status data length, it must be equal to the size of struct matchs iType, e.g: iBufLen == sizeof(VLK_DEV_MODEL)
+ *  @return return 0 anyway
+ *  @details VLK_DevStatus_CB will be called by SDK once device new status is received, developers can easily get Gimbal latest status
+ *  @see VLK_RegisterDevStatusCB
+ */
+typedef int (*VLK_DevStatus_CB)(int iType, const char* szBuffer, int iBufLen, void* pUserParam);
+/** @} ViewLink SDK callback functions */
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @brief Get SDK version
+ *  @return SDK version number, e.g: 3.0.6
+ *  @details we recommend printing the SDK version number before you use SDK,
+ *           this is the only interface can be called before VLK_Init()
+ */
+VLK_API	const char* VLK_CALL    GetSDKVersion();
+
+/** @brief Initialize SDK
+ *  @return VLK_ERROR_NO_ERROR on success, error code otherwise
+ *  @details it should be called somewhere at the beginning of your application,
+ *           and it should be called just once during the lifecirle of your application,
+ *           all interfaces are unavailbe before initialized
+ */
+VLK_API	int     VLK_CALL    VLK_Init();
+
+/** @brief Uninitialize SDK
+ *  @details it should be called somewhere at the end of your application,
+ *           and it should be called just once during the lifecirle of your application,
+ *           once you uninitialize sdk, all interfaces are nomore available
+ */
+VLK_API	void    VLK_CALL    VLK_UnInit();
+
+/** @brief connect Gimbal with specific type, TCP and serial port are available
+ *  @param pConnParam connect information, including connection type (TCP or serial port), ip address, port, serial port name, baudrate
+ *  @param pConnStatusCB  connnection status callback function, it will be called by SDK when device is connected or disconnected
+ *  @param pUserParam user parameters, will be passed back when pConnStatusCB is called
+ *  @return VLK_ERROR_NO_ERROR on success, error code otherwise
+ *  @details this function is asynchronous, the connect result will be passed back by callback function, do not use return value to judge connect result.
+ *           SDK allows only one connection, it means if you create a new connection, the previous one will be disconnected automatically
+ */
+VLK_API	int     VLK_CALL    VLK_Connect(const VLK_CONN_PARAM* pConnParam, VLK_ConnStatus_CB pConnStatusCB, void* pUserParam);
+
+/** @brief Check if Gimbal is connected
+ *  @return
+ *  @retval 1 if either TCP or serial port is connected
+ *  @retval 0 if neither TCP nor serial port is connected
+ */
+VLK_API	int     VLK_CALL    VLK_IsConnected();
+
+/** @brief Check if Gimbal is TCP connected
+ *  @return
+ *  @retval 1 if TCP is connected
+ *  @retval 0 if TCP is disconnected
+ */
+VLK_API	int     VLK_CALL    VLK_IsTCPConnected();
+
+/** @brief Check if Gimbal is serial port connected
+ *  @return
+ *  @retval 1 if serial port is connected
+ *  @retval 0 if serial port is disconnected
+ */
+VLK_API	int     VLK_CALL    VLK_IsSerialPortConnected();
+
+/** @brief Disconnect current connection no matter it is TCP, serial port or other
+ */
+VLK_API	void	VLK_CALL    VLK_Disconnect();
+
+/** @brief Disconnect current TCP connection
+ */
+VLK_API	void	VLK_CALL    VLK_DisconnectTCP();
+
+/** @brief Disconnect current serial port connection
+ */
+VLK_API	void	VLK_CALL    VLK_DisconnectSerialPort();
+
+/** @brief Set keep alive interval
+ *  @param iMS keep alive interval in milliseconds range from 100 ms to 5000 ms
+ *  @details keep alive interval determines the frequncy of querying Gimbal telemetry data,
+ *           SDK default keep alive interval is 500 ms, no need to change it if not necessary
+ */
+VLK_API void	VLK_CALL    VLK_SetKeepAliveInterval(int iMS);
+
+/** @brief Register device status callback
+ *  @param pDevStatusCB receive device status callback function
+ *  @param pUserParam user parameter, it will be passed back when pDevStatusCB is called
+ *  @details pDevStatusCB will be called once SDK received new Gimbal,
+ *           specifically, the telemetry data will keep updating once device is connected,
+ *           ignore it if you don't need it
+ */
+VLK_API	void	VLK_CALL    VLK_RegisterDevStatusCB(VLK_DevStatus_CB pDevStatusCB, void* pUserParam);
+
+/** @brief Control Gimbal yaw and pitch
+ *  @param sHorizontalSpeed the speed of changing yaw (0.01 degrees/s), for example, 2000 means 20 degrees per second
+ *  @param sVeritcalSpeed the speed of changing pitch (0.01 degrees/s)
+ *  @details considering a small angle adjustment will result in a huge visual field changing in sky view, we limit
+ *           speed in a proper range: \n
+ *           -VLK_MAX_YAW_SPEED <= sHorizontalSpeed <= VLK_MAX_YAW_SPEED \n
+ *           -VLK_MAX_PITCH_SPEED <= sVeritcalSpeed <= VLK_MAX_PITCH_SPEED \n
+ *           some example: \n
+ *           move up: VLK_Move(0, 1000); \n
+ *           move left: VLK_Move(-1000, 0); \n
+ *           move right: VLK_Move(1000, 0); \n
+ *           move down: VLK_Move(0, -1000);
+ */
+VLK_API	void	VLK_CALL    VLK_Move(short sHorizontalSpeed, short sVeritcalSpeed);
+
+/** @brief Stop moving
+ *  @details once you call VLK_Move, Gimbal will keep moving until it reach the yaw and pitch limitation or VLK_Stop is called
+ */
+VLK_API	void	VLK_CALL    VLK_Stop();
+
+/** @brief Zoom in
+ *  @param sSpeed the speed of zoom in
+ *  @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster
+ */
+VLK_API	void	VLK_CALL    VLK_ZoomIn(short sSpeed);
+
+/** @brief Zoom out
+ *  @param sSpeed the speed of zoom out
+ *  @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster
+ */
+VLK_API	void	VLK_CALL    VLK_ZoomOut(short sSpeed);
+
+/** @brief Stop zoom
+ *  @details once you call VLK_ZoomIn or VLK_ZoomOut, Camera will keep zooming until it reach the limitation or VLK_StopZoom is called
+ */
+VLK_API	void	VLK_CALL    VLK_StopZoom();
+
+/** @brief Enable track mode
+ *  @param Param track mode parameters
+ *  @see VLK_TrackTargetPositionEx
+ *  @deprecated
+ */
+VLK_API	void	VLK_CALL    VLK_EnableTrackMode(const VLK_TRACK_MODE_PARAM* pParam);
+
+/** @brief Track target by it's position
+ *  @param iX target position, number of pixels from top-left corner in horizontal direction
+ *  @param iY taget position, number of  pixels from top-left corner in vertical direction
+ *  @param iVideoWidth video image width, e.g: 1920, 1280
+ *  @param iVideoHeight video image height, e.g: 1080, 720
+ *  @deprecated
+ *  @see VLK_TrackTargetPositionEx
+ */
+VLK_API	void	VLK_CALL    VLK_TrackTargetPosition(int iX, int iY, int iVideoWidth, int iVideoHeight);
+
+/** @brief Track target by it's position
+ *  @param pParam track mode parameters,
+ *  @param iX target position, number of pixels from top-left corner in horizontal direction
+ *  @param iY taget position, number of  pixels from top-left corner in vertical direction
+ *  @param iVideoWidth video image width, e.g: 1920, 1280
+ *  @param iVideoHeight video image height, e.g: 1080, 720
+ *  @details we usually use VLK_TRACK_TEMPLATE_SIZE_AUTO for pParam->emTrackTempSize and VLK_SENSOR_VISIBLE1 for pParam->emTrackSensor
+ */
+VLK_API	void	VLK_CALL    VLK_TrackTargetPositionEx(const VLK_TRACK_MODE_PARAM* pParam, int iX, int iY, int iVideoWidth, int iVideoHeight);
+
+/** @brief Disable track mode
+ */
+VLK_API	void	VLK_CALL    VLK_DisableTrackMode();
+
+/** @brief Focus in
+ *  @param sSpeed the speed of focus in
+ *  @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster
+ *           this function is available only when the Gimbal is in Manual focusing mode, call VLK_GetFocusMode to
+ *           get current focus mode, call VLK_SetFocusMode to set focus mode
+ */
+VLK_API	void	VLK_CALL    VLK_FocusIn(short sSpeed);
+
+/** @brief Focus out
+ *  @param sSpeed the speed of focus out
+ *  @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster
+ *           this function is available only when the Gimbal is in Manual focusing mode, call VLK_GetFocusMode to
+ *           get current focus mode, call VLK_SetFocusMode to set focus mode
+ */
+VLK_API	void	VLK_CALL    VLK_FocusOut(short sSpeed);
+
+/** @brief Stop focus
+ *  @details  once you call VLK_FocusIn or VLK_FocusOut, Camera will keep focusing until it reach the limitation or VLK_StopFocus is called
+ */
+VLK_API	void	VLK_CALL    VLK_StopFocus();
+
+/** @brief Move to home position
+ */
+VLK_API	void	VLK_CALL    VLK_Home();
+
+/** @brief Switch motor
+ *  @param iOn turn on/turn off \n
+ *         1 turn on \n
+ *         0 turn off
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchMotor(int iOn);
+
+/** @brief Check motor status
+ *  @return current motor status
+ *  @retval 1 on
+ *  @retval 0 off
+ */
+VLK_API	int     VLK_CALL    VLK_IsMotorOn();
+
+/** @brief Enable Gimbal follow drone
+ *  @param iEnable enable or disable \n
+ *         1 enable \n
+ *         0 disable
+ */
+VLK_API	void	VLK_CALL    VLK_EnableFollowMode(int iEnable);
+
+/** @brief Check follow mode
+ *  @return current follow status
+ *  @retval 1 enabled
+ *  @retval 0 disabled
+ */
+VLK_API	int     VLK_CALL    VLK_IsFollowMode();
+
+/** @brief Turn to specific yaw and pitch
+ *  @param dYaw specific yaw, must be a double value from VLK_YAW_MIN to VLK_YAW_MAX
+ *  @param dPitch specific pitch, must be a double value from VLK_PITCH_MIN to VLK_PITCH_MAX
+ */
+VLK_API	void	VLK_CALL    VLK_TurnTo(double dYaw, double dPitch);
+
+/** @brief Enable track
+ */
+VLK_API	void	VLK_CALL    VLK_StartTrack();
+
+
+/** @brief Disable track
+ */
+VLK_API	void	VLK_CALL    VLK_StopTrack();
+
+/** @brief Check if track is enabled
+ *  @return current track status
+ *  @retval 1 enabled
+ *  @retval 0 disabled
+ */
+VLK_API	int     VLK_CALL    VLK_IsTracking();
+
+/** @brief Set track template size
+ *  @param emSize track template size enumeration
+ *  @see VLK_TRACK_TEMPLATE_SIZE
+ */
+VLK_API	void	VLK_CALL    VLK_SetTrackTemplateSize(VLK_TRACK_TEMPLATE_SIZE emSize);
+
+/** @brief Set image color
+ *  @param emImgType image type enumeration
+ *  @param iEnablePIP enable picture in picture \n
+ *         1 enable \n
+ *         0 disable
+ * @param emIRColor IR color enumeration
+ *  @see VLK_IMAGE_TYPE
+ */
+VLK_API	void	VLK_CALL    VLK_SetImageColor(VLK_IMAGE_TYPE emImgType, int iEnablePIP, VLK_IR_COLOR emIRColor);
+
+/** @brief Ask Gimbal take a photograph
+ *  @details make sure there is SD card in the Gimbal
+ */
+VLK_API	void	VLK_CALL    VLK_Photograph();
+
+/** @brief Gimbal start or stop recording
+ *  @param iOn \n
+ *          1 start recording \n
+ *          0 stop recording
+ *  @details make sure there is SD card in the Gimbal
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchRecord(int iOn);
+
+/** @brief Check if Gimbal is recording
+ *  @return flag of recording status
+ *  @retval 1 recording
+ *  @retval 0 not recording
+ *  @details make sure there is SD card in the Gimbal
+ */
+VLK_API	int     VLK_CALL    VLK_IsRecording();
+
+/** @brief Enable defog
+ *  @param iOn \n
+ *          1 defog is enabled \n
+ *          0 defog is disabled
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchDefog(int iOn);
+
+/** @brief Check if defog is enabled
+ *  @return flag of defog status
+ *  @retval 1 defog is enabled
+ *  @retval 0 defog is disabled
+ */
+VLK_API	int     VLK_CALL    VLK_IsDefogOn();
+
+/** @brief Set record mode
+ *  @param emMode record mode enumeration
+ *  @details some models could not take photo while it is recording,
+ *           they must be switched to a certain
+ * @see VLK_RECORD_MODE
+ */
+VLK_API	void	VLK_CALL    VLK_SetRecordMode(VLK_RECORD_MODE emMode);
+
+/** @brief Get current record mode
+ *  @return record mode enumeration
+ *  @see VLK_RECORD_MODE
+ */
+VLK_API	int     VLK_CALL    VLK_GetRecordMode();
+
+/** @brief Set focus mode ( manual focus or automatic focus)
+ *  @param focus mode enumeration
+ *  @see VLK_FOCUS_MODE
+ */
+VLK_API	void	VLK_CALL    VLK_SetFocusMode(VLK_FOCUS_MODE emMode);
+
+/** @brief Get current focus mode ( manual focus or automatic focus)
+ *  @return focus mode enumeration
+ *  @see VLK_FOCUS_MODE
+ */
+VLK_API	int     VLK_CALL    VLK_GetFocusMode();
+
+/** @brief Zoom to a specific magnification
+ *  @param fMag specific magnification
+ *  @details the camera will reach the limitation if the specific magnification over it's capability
+ */
+VLK_API	void	VLK_CALL    VLK_ZoomTo(float fMag);
+
+/** @brief IR digital zoom in
+ *  @param sSpeed deprecated, pass 0
+ *  @details zoom in x1 then stop automatically
+ */
+VLK_API	void	VLK_CALL    VLK_IRDigitalZoomIn(short sSpeed);
+
+/** @brief IR digital zoom out
+ *  @param sSpeed deprecated, pass 0
+ *  @details zoom out x1 then stop automatically
+ */
+VLK_API	void	VLK_CALL    VLK_IRDigitalZoomOut(short sSpeed);
+
+/** @brief Switch EO digital zoom status
+ *  @param iOn \n
+ *         1 enable \n
+ *         0 disable
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchEODigitalZoom(int iOn);
+
+/** @brief Enter S-BUS mode
+ *  @deprecated
+ */
+VLK_API	void	VLK_CALL    VLK_EnterSBUSMode();
+
+/** @brief Exit S-BUS mode
+ *  @deprecated
+ */
+VLK_API	void	VLK_CALL    VLK_ExitSBUSMode();
+
+/** @brief Query device configuration
+ *  @details this function is asynchronous, configuration data will be passed back throw VLK_DevStatus_CB
+ *  @see VLK_DevStatus_CB
+ */
+VLK_API	void	VLK_CALL    VLK_QueryDevConfiguration();
+
+/** @brief Set OSD
+ *  @param pParam OSG configuration
+ *  @details we don't provide VLK_GetOSD because OSD is included in device configuration
+ *  @see VLK_OSD_PARAM
+ */
+VLK_API	void	VLK_CALL    VLK_SetOSD(const VLK_OSD_PARAM* pParam);
+
+/** @brief Set wireless control channels map
+ *  @param pChnlsMap wireless control channels map configuration
+ *  @details we don't provide VLK_GetWirelessCtrlChnlMap because channels map is included in device configuration
+ *  @see VLK_CHANNELS_MAP
+ */
+VLK_API	void	VLK_CALL    VLK_SetWirelessCtrlChnlMap(const VLK_CHANNELS_MAP* pChnlsMap);
+
+/** @brief Send extent command
+ *  @param szCmd command data pointer
+ *  @param iLen command data length
+ *  @details for some seldom-used Gimbal control command which we didn't provide specific interface,
+ *           you should call this function to send command data directly
+ */
+VLK_API	void	VLK_CALL    VLK_SendExtentCmd(const char* szCmd, int iLen);
+
+
+/** @name Laser Control functions
+ * @{
+ */
+
+/** @brief Switch laser status
+ *  @param iOn \n
+ *         1 turn on \n
+ *         0 turn off
+ *  @details make sure your device has laser capability
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchLaser(int iOn);
+
+/** @brief Laser single ranging
+ */
+VLK_API	void	VLK_CALL    VLK_LaserSingle();
+
+
+/** @brief Laser zoom in
+ *  @param sSpeed deprecated, pass 0
+ */
+VLK_API	void	VLK_CALL    VLK_LaserZoomIn(short sSpeed);
+
+/** @brief Laser zoom out
+ *  @param sSpeed deprecated, pass 0
+ */
+VLK_API	void	VLK_CALL    VLK_LaserZoomOut(short sSpeed);
+
+/** @brief Laser stop zoom
+ */
+VLK_API	void	VLK_CALL    VLK_LaserStopZoom();
+
+/** @brief Set laser zoom mode
+ *  @param emMode laser zoom mode enumeration
+ */
+VLK_API	void	VLK_CALL    VLK_SetLaserZoomMode(VLK_LASER_ZOOM_MODE emMode);
+/** @} Laser Control functions */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //__VIEW_LINK_H__

src/api/QGCCorePlugin.cc

@@ -0,0 +1,502 @@
+/****************************************************************************
+ *
+ * (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 "QGCApplication.h"
+#include "QGCCorePlugin.h"
+#include "QGCOptions.h"
+#include "QmlComponentInfo.h"
+#include "FactMetaData.h"
+#include "SettingsManager.h"
+#include "AppMessages.h"
+#include "QmlObjectListModel.h"
+#include "VideoManager.h"
+#if defined(QGC_GST_STREAMING)
+#include "GStreamer.h"
+#include "VideoReceiver.h"
+#endif
+#include "QGCLoggingCategory.h"
+#include "QGCCameraManager.h"
+#include "HorizontalFactValueGrid.h"
+#include "InstrumentValueData.h"
+
+#include <QtQml>
+#include <QQmlEngine>
+
+/// @file
+///     @brief Core Plugin Interface for QGroundControl - Default Implementation
+///     @author Gus Grubba <gus@auterion.com>
+
+class QGCCorePlugin_p
+{
+public:
+    QGCCorePlugin_p()
+    {
+    }
+
+    ~QGCCorePlugin_p()
+    {
+        if(pGeneral)
+            delete pGeneral;
+        if(pCommLinks)
+            delete pCommLinks;
+        if(pOfflineMaps)
+            delete pOfflineMaps;
+#if defined(QGC_GST_TAISYNC_ENABLED)
+        if(pTaisync)
+            delete pTaisync;
+#endif
+#if defined(QGC_GST_MICROHARD_ENABLED)
+        if(pMicrohard)
+            delete pMicrohard;
+#endif
+        if(pMAVLink)
+            delete pMAVLink;
+        if(pConsole)
+            delete pConsole;
+#if defined(QT_DEBUG)
+        if(pMockLink)
+            delete pMockLink;
+        if(pDebug)
+            delete pDebug;
+        if(pQmlTest)
+            delete pQmlTest;
+#endif
+        if(pRemoteID)
+            delete pRemoteID;
+        if(defaultOptions)
+            delete defaultOptions;
+    }
+
+    QmlComponentInfo* pGeneral                  = nullptr;
+    QmlComponentInfo* pCommLinks                = nullptr;
+    QmlComponentInfo* pOfflineMaps              = nullptr;
+#if defined(QGC_GST_TAISYNC_ENABLED)
+    QmlComponentInfo* pTaisync                  = nullptr;
+#endif
+#if defined(QGC_GST_MICROHARD_ENABLED)
+    QmlComponentInfo* pMicrohard                = nullptr;
+#endif
+    QmlComponentInfo* pMAVLink                  = nullptr;
+    QmlComponentInfo* pConsole                  = nullptr;
+    QmlComponentInfo* pHelp                     = nullptr;
+#if defined(QT_DEBUG)
+    QmlComponentInfo* pMockLink                 = nullptr;
+    QmlComponentInfo* pDebug                    = nullptr;
+    QmlComponentInfo* pQmlTest                  = nullptr;
+#endif
+    QmlComponentInfo* pRemoteID                  = nullptr;
+
+    QGCOptions*         defaultOptions          = nullptr;
+    QVariantList        settingsList;
+    QVariantList        analyzeList;
+
+    QmlObjectListModel _emptyCustomMapItems;
+};
+
+QGCCorePlugin::~QGCCorePlugin()
+{
+    if(_p) {
+        delete _p;
+    }
+}
+
+QGCCorePlugin::QGCCorePlugin(QGCApplication *app, QGCToolbox* toolbox)
+    : QGCTool(app, toolbox)
+    , _showTouchAreas(false)
+    , _showAdvancedUI(true)
+{
+    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+    _p = new QGCCorePlugin_p;
+}
+
+void QGCCorePlugin::setToolbox(QGCToolbox *toolbox)
+{
+    QGCTool::setToolbox(toolbox);
+
+    qmlRegisterUncreatableType<QGCCorePlugin>       ("QGroundControl", 1, 0, "QGCCorePlugin",       "Reference only");
+    qmlRegisterUncreatableType<QGCOptions>          ("QGroundControl", 1, 0, "QGCOptions",          "Reference only");
+    qmlRegisterUncreatableType<QGCFlyViewOptions>   ("QGroundControl", 1, 0, "QGCFlyViewOptions",   "Reference only");
+}
+
+QVariantList &QGCCorePlugin::settingsPages()
+{
+    if(!_p->pGeneral) {
+        _p->pGeneral = new QmlComponentInfo(tr("General"),
+                                            QUrl::fromUserInput("qrc:/qml/GeneralSettings.qml"),
+                                            QUrl::fromUserInput("qrc:/res/gear-white.svg"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pGeneral)));
+        _p->pCommLinks = new QmlComponentInfo(tr("Comm Links"),
+                                              QUrl::fromUserInput("qrc:/qml/LinkSettings.qml"),
+                                              QUrl::fromUserInput("qrc:/res/waves.svg"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pCommLinks)));
+        _p->pOfflineMaps = new QmlComponentInfo(tr("Offline Maps"),
+                                                QUrl::fromUserInput("qrc:/qml/OfflineMap.qml"),
+                                                QUrl::fromUserInput("qrc:/res/waves.svg"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pOfflineMaps)));
+#if defined(QGC_GST_TAISYNC_ENABLED)
+        _p->pTaisync = new QmlComponentInfo(tr("Taisync"),
+                                            QUrl::fromUserInput("qrc:/qml/TaisyncSettings.qml"),
+                                            QUrl::fromUserInput(""));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pTaisync)));
+#endif
+#if defined(QGC_GST_MICROHARD_ENABLED)
+        _p->pMicrohard = new QmlComponentInfo(tr("Microhard"),
+                                              QUrl::fromUserInput("qrc:/qml/MicrohardSettings.qml"),
+                                              QUrl::fromUserInput(""));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pMicrohard)));
+#endif
+        _p->pMAVLink = new QmlComponentInfo(tr("MAVLink"),
+                                            QUrl::fromUserInput("qrc:/qml/MavlinkSettings.qml"),
+                                            QUrl::fromUserInput("qrc:/res/waves.svg"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pMAVLink)));
+        _p->pRemoteID = new QmlComponentInfo(tr("Remote ID"),
+                                            QUrl::fromUserInput("qrc:/qml/RemoteIDSettings.qml"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pRemoteID)));
+        _p->pConsole = new QmlComponentInfo(tr("Console"),
+                                            QUrl::fromUserInput("qrc:/qml/QGroundControl/Controls/AppMessages.qml"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pConsole)));
+        _p->pHelp = new QmlComponentInfo(tr("Help"),
+                                         QUrl::fromUserInput("qrc:/qml/HelpSettings.qml"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pHelp)));
+#if defined(QT_DEBUG)
+        //-- These are always present on Debug builds
+        _p->pMockLink = new QmlComponentInfo(tr("Mock Link"),
+                                             QUrl::fromUserInput("qrc:/qml/MockLink.qml"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pMockLink)));
+        _p->pDebug = new QmlComponentInfo(tr("Debug"),
+                                          QUrl::fromUserInput("qrc:/qml/DebugWindow.qml"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pDebug)));
+        _p->pQmlTest = new QmlComponentInfo(tr("Palette Test"),
+                                            QUrl::fromUserInput("qrc:/qml/QmlTest.qml"));
+        _p->settingsList.append(QVariant::fromValue(reinterpret_cast<QmlComponentInfo*>(_p->pQmlTest)));
+#endif
+    }
+    return _p->settingsList;
+}
+
+QVariantList& QGCCorePlugin::analyzePages()
+{
+    if (!_p->analyzeList.count()) {
+        _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Log Download"),     QUrl::fromUserInput("qrc:/qml/LogDownloadPage.qml"),        QUrl::fromUserInput("qrc:/qmlimages/LogDownloadIcon"))));
+#if !defined(__mobile__)
+        _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("GeoTag Images"),    QUrl::fromUserInput("qrc:/qml/GeoTagPage.qml"),             QUrl::fromUserInput("qrc:/qmlimages/GeoTagIcon"))));
+#endif
+        _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("MAVLink Console"),  QUrl::fromUserInput("qrc:/qml/MavlinkConsolePage.qml"),     QUrl::fromUserInput("qrc:/qmlimages/MavlinkConsoleIcon"))));
+#if !defined(QGC_DISABLE_MAVLINK_INSPECTOR)
+        _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("MAVLink Inspector"),QUrl::fromUserInput("qrc:/qml/MAVLinkInspectorPage.qml"),   QUrl::fromUserInput("qrc:/qmlimages/MAVLinkInspector"))));
+#endif
+        _p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Vibration"),        QUrl::fromUserInput("qrc:/qml/VibrationPage.qml"),          QUrl::fromUserInput("qrc:/qmlimages/VibrationPageIcon"))));
+    }
+    return _p->analyzeList;
+}
+
+int QGCCorePlugin::defaultSettings()
+{
+    return 0;
+}
+
+QGCOptions* QGCCorePlugin::options()
+{
+    if (!_p->defaultOptions) {
+        _p->defaultOptions = new QGCOptions(this);
+    }
+    return _p->defaultOptions;
+}
+
+bool QGCCorePlugin::overrideSettingsGroupVisibility(QString name)
+{
+    Q_UNUSED(name);
+
+    // Always show all
+    return true;
+}
+
+bool QGCCorePlugin::adjustSettingMetaData(const QString& settingsGroup, FactMetaData& metaData)
+{
+    if (settingsGroup == AppSettings::settingsGroup) {
+#if !defined(QGC_ENABLE_PAIRING)
+        //-- If we don't support pairing, disable it.
+        if (metaData.name() == AppSettings::usePairingName) {
+            metaData.setRawDefaultValue(false);
+            //-- And hide the option
+            return false;
+        }
+#endif
+
+        //-- Default Palette
+        if (metaData.name() == AppSettings::indoorPaletteName) {
+            QVariant outdoorPalette;
+#if defined (__mobile__)
+            outdoorPalette = 0;
+#else
+            outdoorPalette = 1;
+#endif
+            metaData.setRawDefaultValue(outdoorPalette);
+            return true;
+        }
+
+#if defined (__mobile__)
+        if (metaData.name() == AppSettings::telemetrySaveName) {
+            // Mobile devices have limited storage so don't turn on telemtry saving by default
+            metaData.setRawDefaultValue(false);
+            return true;
+        }
+#endif
+
+#ifndef __android__
+        if (metaData.name() == AppSettings::androidSaveToSDCardName) {
+            // This only shows on android builds
+            return false;
+        }
+#endif
+    }
+
+    return true; // Show setting in ui
+}
+
+void QGCCorePlugin::setShowTouchAreas(bool show)
+{
+    if (show != _showTouchAreas) {
+        _showTouchAreas = show;
+        emit showTouchAreasChanged(show);
+    }
+}
+
+void QGCCorePlugin::setShowAdvancedUI(bool show)
+{
+    if (show != _showAdvancedUI) {
+        _showAdvancedUI = show;
+        emit showAdvancedUIChanged(show);
+    }
+}
+
+void QGCCorePlugin::paletteOverride(QString /*colorName*/, QGCPalette::PaletteColorInfo_t& /*colorInfo*/)
+{
+
+}
+
+QString QGCCorePlugin::showAdvancedUIMessage() const
+{
+    return tr("WARNING: You are about to enter Advanced Mode. "
+              "If used incorrectly, this may cause your vehicle to malfunction thus voiding your warranty. "
+              "You should do so only if instructed by customer support. "
+              "Are you sure you want to enable Advanced Mode?");
+}
+
+void QGCCorePlugin::factValueGridCreateDefaultSettings(const QString& defaultSettingsGroup)
+{
+    HorizontalFactValueGrid factValueGrid(defaultSettingsGroup);
+
+    bool        includeFWValues = factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassFixedWing || factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassVTOL || factValueGrid.vehicleClass() == QGCMAVLink::VehicleClassAirship;
+
+    factValueGrid.setFontSize(FactValueGrid::LargeFontSize);
+
+    factValueGrid.appendColumn();
+    factValueGrid.appendColumn();
+    factValueGrid.appendColumn();
+    if (includeFWValues) {
+        factValueGrid.appendColumn();
+    }
+    factValueGrid.appendRow();
+
+    int                 rowIndex    = 0;
+    QmlObjectListModel* column      = factValueGrid.columns()->value<QmlObjectListModel*>(0);
+
+    InstrumentValueData* value = column->value<InstrumentValueData*>(rowIndex++);
+    value->setFact("Vehicle", "AltitudeRelative");
+    value->setIcon("arrow-thick-up.svg");
+    value->setText(value->fact()->shortDescription());
+    value->setShowUnits(true);
+
+    value = column->value<InstrumentValueData*>(rowIndex++);
+    value->setFact("Vehicle", "DistanceToHome");
+    value->setIcon("bookmark copy 3.svg");
+    value->setText(value->fact()->shortDescription());
+    value->setShowUnits(true);
+
+    rowIndex    = 0;
+    column      = factValueGrid.columns()->value<QmlObjectListModel*>(1);
+
+    value = column->value<InstrumentValueData*>(rowIndex++);
+    value->setFact("Vehicle", "ClimbRate");
+    value->setIcon("arrow-simple-up.svg");
+    value->setText(value->fact()->shortDescription());
+    value->setShowUnits(true);
+
+    value = column->value<InstrumentValueData*>(rowIndex++);
+    value->setFact("Vehicle", "GroundSpeed");
+    value->setIcon("arrow-simple-right.svg");
+    value->setText(value->fact()->shortDescription());
+    value->setShowUnits(true);
+
+
+    if (includeFWValues) {
+        rowIndex    = 0;
+        column      = factValueGrid.columns()->value<QmlObjectListModel*>(2);
+
+        value = column->value<InstrumentValueData*>(rowIndex++);
+        value->setFact("Vehicle", "AirSpeed");
+        value->setText("AirSpd");
+        value->setShowUnits(true);
+
+        value = column->value<InstrumentValueData*>(rowIndex++);
+        value->setFact("Vehicle", "ThrottlePct");
+        value->setText("Thr");
+        value->setShowUnits(true);
+    }
+
+    rowIndex    = 0;
+    column      = factValueGrid.columns()->value<QmlObjectListModel*>(includeFWValues ? 3 : 2);
+
+    value = column->value<InstrumentValueData*>(rowIndex++);
+    value->setFact("Vehicle", "FlightTime");
+    value->setIcon("timer.svg");
+    value->setText(value->fact()->shortDescription());
+    value->setShowUnits(false);
+
+    value = column->value<InstrumentValueData*>(rowIndex++);
+    value->setFact("Vehicle", "FlightDistance");
+    value->setIcon("travel-walk.svg");
+    value->setText(value->fact()->shortDescription());
+    value->setShowUnits(true);
+}
+
+QQmlApplicationEngine* QGCCorePlugin::createQmlApplicationEngine(QObject* parent)
+{
+    QQmlApplicationEngine* qmlEngine = new QQmlApplicationEngine(parent);
+    qmlEngine->addImportPath("qrc:/qml");
+    qmlEngine->rootContext()->setContextProperty("joystickManager", qgcApp()->toolbox()->joystickManager());
+    qmlEngine->rootContext()->setContextProperty("debugMessageModel", AppMessages::getModel());
+    return qmlEngine;
+}
+
+void QGCCorePlugin::createRootWindow(QQmlApplicationEngine* qmlEngine)
+{
+    qmlEngine->load(QUrl(QStringLiteral("qrc:/qml/MainRootWindow.qml")));
+}
+
+bool QGCCorePlugin::mavlinkMessage(Vehicle* vehicle, LinkInterface* link, mavlink_message_t message)
+{
+    Q_UNUSED(vehicle);
+    Q_UNUSED(link);
+    Q_UNUSED(message);
+
+    return true;
+}
+
+QmlObjectListModel* QGCCorePlugin::customMapItems()
+{
+    return &_p->_emptyCustomMapItems;
+}
+
+VideoManager* QGCCorePlugin::createVideoManager(QGCApplication *app, QGCToolbox *toolbox)
+{
+    return new VideoManager(app, toolbox);
+}
+
+VideoReceiver* QGCCorePlugin::createVideoReceiver(QObject* parent)
+{
+#if defined(QGC_GST_STREAMING)
+    return GStreamer::createVideoReceiver(parent);
+#else
+    Q_UNUSED(parent)
+    return nullptr;
+#endif
+}
+
+void* QGCCorePlugin::createVideoSink(QObject* parent, QQuickItem* widget)
+{
+#if defined(QGC_GST_STREAMING)
+    return GStreamer::createVideoSink(parent, widget);
+#else
+    Q_UNUSED(parent)
+    Q_UNUSED(widget)
+    return nullptr;
+#endif
+}
+
+void QGCCorePlugin::releaseVideoSink(void* sink)
+{
+#if defined(QGC_GST_STREAMING)
+    GStreamer::releaseVideoSink(sink);
+#else
+    Q_UNUSED(sink)
+#endif
+}
+
+bool QGCCorePlugin::guidedActionsControllerLogging() const
+{
+    return GuidedActionsControllerLog().isDebugEnabled();
+}
+
+QString QGCCorePlugin::stableVersionCheckFileUrl() const
+{
+#ifdef QGC_CUSTOM_BUILD
+    // Custom builds must override to turn on and provide their own location
+    return QString();
+#else
+    return QString("https://s3-us-west-2.amazonaws.com/qgroundcontrol/latest/QGC.version.txt");
+#endif
+}
+
+const QVariantList& QGCCorePlugin::toolBarIndicators(void)
+{
+    //-- Default list of indicators for all vehicles.
+    if(_toolBarIndicatorList.size() == 0) {
+        _toolBarIndicatorList = QVariantList({
+                                                 QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSRTKIndicator.qml")),
+                                             });
+    }
+    return _toolBarIndicatorList;
+}
+
+QList<int> QGCCorePlugin::firstRunPromptStdIds(void)
+{
+    QList<int> rgStdIds = { unitsFirstRunPromptId, offlineVehicleFirstRunPromptId };
+    return rgStdIds;
+}
+
+QList<int> QGCCorePlugin::firstRunPromptCustomIds(void)
+{
+    return QList<int>();
+}
+
+QVariantList QGCCorePlugin::firstRunPromptsToShow(void)
+{
+    QList<int> rgIdsToShow;
+
+    rgIdsToShow.append(firstRunPromptStdIds());
+    rgIdsToShow.append(firstRunPromptCustomIds());
+
+    QList<int> rgAlreadyShownIds = AppSettings::firstRunPromptsIdsVariantToList(_toolbox->settingsManager()->appSettings()->firstRunPromptIdsShown()->rawValue());
+
+    for (int idToRemove: rgAlreadyShownIds) {
+        rgIdsToShow.removeOne(idToRemove);
+    }
+
+    QVariantList rgVarIdsToShow;
+    for (int id: rgIdsToShow) {
+        rgVarIdsToShow.append(id);
+    }
+
+    return rgVarIdsToShow;
+}
+
+QString QGCCorePlugin::firstRunPromptResource(int id)
+{
+    switch (id) {
+    case unitsFirstRunPromptId:
+        return "/FirstRunPromptDialogs/UnitsFirstRunPrompt.qml";
+    case offlineVehicleFirstRunPromptId:
+        return "/FirstRunPromptDialogs/OfflineVehicleFirstRunPrompt.qml";
+        break;
+    }
+
+    return QString();
+}

src/api/QGCCorePlugin.h

@@ -0,0 +1,222 @@
+/****************************************************************************
+ *
+ * (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 "QGCToolbox.h"
+#include "QGCPalette.h"
+#include "QGCMAVLink.h"
+#include "QmlObjectListModel.h"
+
+#include <QObject>
+#include <QVariantList>
+
+/// @file
+/// @brief Core Plugin Interface for QGroundControl
+/// @author Gus Grubba <gus@auterion.com>
+
+class QGCApplication;
+class QGCOptions;
+class QGCSettings;
+class QGCCorePlugin_p;
+class FactMetaData;
+class QGeoPositionInfoSource;
+class QQmlApplicationEngine;
+class Vehicle;
+class LinkInterface;
+class QmlObjectListModel;
+class VideoReceiver;
+class VideoSink;
+class PlanMasterController;
+class QGCCameraManager;
+class QGCCameraControl;
+class QQuickItem;
+class InstrumentValueAreaController;
+
+#ifndef OPAQUE_PTR_QGCCorePlugin
+    #define OPAQUE_PTR_QGCCorePlugin
+    Q_DECLARE_OPAQUE_POINTER(QGCOptions*)
+#endif
+
+class QGCCorePlugin : public QGCTool
+{
+    Q_OBJECT
+public:
+    QGCCorePlugin(QGCApplication* app, QGCToolbox* toolbox);
+    ~QGCCorePlugin();
+
+    Q_PROPERTY(QVariantList         settingsPages                   READ settingsPages                                  NOTIFY settingsPagesChanged)
+    Q_PROPERTY(QVariantList         analyzePages                    READ analyzePages                                   NOTIFY analyzePagesChanged)
+    Q_PROPERTY(int                  defaultSettings                 READ defaultSettings                                CONSTANT)
+    Q_PROPERTY(QGCOptions*          options                         READ options                                        CONSTANT)
+    Q_PROPERTY(bool                 showTouchAreas                  READ showTouchAreas         WRITE setShowTouchAreas NOTIFY showTouchAreasChanged)
+    Q_PROPERTY(bool                 showAdvancedUI                  READ showAdvancedUI         WRITE setShowAdvancedUI NOTIFY showAdvancedUIChanged)
+    Q_PROPERTY(QString              showAdvancedUIMessage           READ showAdvancedUIMessage                          CONSTANT)
+    Q_PROPERTY(QString              brandImageIndoor                READ brandImageIndoor                               CONSTANT)
+    Q_PROPERTY(QString              brandImageOutdoor               READ brandImageOutdoor                              CONSTANT)
+    Q_PROPERTY(QmlObjectListModel*  customMapItems                  READ customMapItems                                 CONSTANT)
+    Q_PROPERTY(QVariantList         toolBarIndicators               READ toolBarIndicators                              NOTIFY toolBarIndicatorsChanged)
+    Q_PROPERTY(int                  unitsFirstRunPromptId           MEMBER unitsFirstRunPromptId                        CONSTANT)
+    Q_PROPERTY(int                  offlineVehicleFirstRunPromptId  MEMBER offlineVehicleFirstRunPromptId               CONSTANT)
+
+    Q_INVOKABLE bool guidedActionsControllerLogging() const;
+
+    /// The list of settings under the Settings Menu
+    /// @return A list of QGCSettings
+    virtual QVariantList& settingsPages();
+
+    /// The list of pages/buttons under the Analyze Menu
+    /// @return A list of QmlPageInfo
+    virtual QVariantList& analyzePages();
+
+    /// The default settings panel to show
+    /// @return The settings index
+    virtual int defaultSettings();
+
+    /// Global options
+    /// @return An instance of QGCOptions
+    virtual QGCOptions* options();
+
+    /// Allows the core plugin to override the visibility for a settings group
+    ///     @param name - SettingsGroup name
+    /// @return true: Show settings ui, false: Hide settings ui
+    virtual bool overrideSettingsGroupVisibility(QString name);
+
+    /// Allows the core plugin to override the setting meta data before the setting fact is created.
+    ///     @param settingsGroup - QSettings group which contains this item
+    ///     @param metaData - MetaData for setting fact
+    /// @return true: Setting should be visible in ui, false: Setting should not be shown in ui
+    virtual bool adjustSettingMetaData(const QString& settingsGroup, FactMetaData& metaData);
+
+    /// Return the resource file which contains the brand image for for Indoor theme.
+    virtual QString brandImageIndoor() const { return QString(); }
+
+    /// Return the resource file which contains the brand image for for Outdoor theme.
+    virtual QString brandImageOutdoor() const { return QString(); }
+
+    /// @return The message to show to the user when they a re prompted to confirm turning on advanced ui.
+    virtual QString showAdvancedUIMessage() const;
+
+    /// @return An instance of an alternate position source (or NULL if not available)
+    virtual QGeoPositionInfoSource* createPositionSource(QObject* /*parent*/) { return nullptr; }
+
+    /// Allows a plugin to override the specified color name from the palette
+    virtual void paletteOverride(QString colorName, QGCPalette::PaletteColorInfo_t& colorInfo);
+
+    virtual void factValueGridCreateDefaultSettings(const QString& defaultSettingsGroup);
+
+    /// Allows the plugin to override or get access to the QmlApplicationEngine to do things like add import
+    /// path or stuff things into the context prior to window creation.
+    virtual QQmlApplicationEngine* createQmlApplicationEngine(QObject* parent);
+
+    /// Allows the plugin to override the creation of the root (native) window.
+    virtual void createRootWindow(QQmlApplicationEngine* qmlEngine);
+
+    /// Allows the plugin to override the creation of VideoManager.
+    virtual VideoManager* createVideoManager(QGCApplication* app, QGCToolbox* toolbox);
+    /// Allows the plugin to override the creation of VideoReceiver.
+    virtual VideoReceiver* createVideoReceiver(QObject* parent);
+    /// Allows the plugin to override the creation of VideoSink.
+    virtual void* createVideoSink(QObject* parent, QQuickItem* widget);
+    /// Allows the plugin to override the release of VideoSink.
+    virtual void releaseVideoSink(void* sink);
+
+    /// Allows the plugin to see all mavlink traffic to a vehicle
+    /// @return true: Allow vehicle to continue processing, false: Vehicle should not process message
+    virtual bool mavlinkMessage(Vehicle* vehicle, LinkInterface* link, mavlink_message_t message);
+
+    /// Allows custom builds to add custom items to the FlightMap. Objects put into QmlObjectListModel should derive from QmlComponentInfo and set the url property.
+    virtual QmlObjectListModel* customMapItems();
+
+    /// Allows custom builds to add custom items to the plan file before the document is created.
+    virtual void    preSaveToJson           (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {}
+    /// Allows custom builds to add custom items to the plan file after the document is created.
+    virtual void    postSaveToJson          (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {}
+
+    /// Allows custom builds to add custom items to the mission section of the plan file before the item is created.
+    virtual void    preSaveToMissionJson    (PlanMasterController* /*pController*/, QJsonObject& /*missionJson*/) {}
+    /// Allows custom builds to add custom items to the mission section of the plan file after the item is created.
+    virtual void    postSaveToMissionJson   (PlanMasterController* /*pController*/, QJsonObject& /*missionJson*/) {}
+
+    /// Allows custom builds to load custom items from the plan file before the document is parsed.
+    virtual void    preLoadFromJson     (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {}
+    /// Allows custom builds to load custom items from the plan file after the document is parsed.
+    virtual void    postLoadFromJson    (PlanMasterController* /*pController*/, QJsonObject& /*json*/) {}
+
+    /// Returns the url to download the stable version check file. Return QString() to indicate no version check should be performed.
+    /// Default QGC mainline implemenentation returns QGC Stable file location. Default QGC custom build code returns QString().
+    /// Custom builds can override to turn on and provide their own location.
+    /// The contents of this file should be a single line in the form:
+    ///     v3.4.4
+    /// This indicates the latest stable version number.
+    virtual QString stableVersionCheckFileUrl() const;
+
+    /// Returns the user visible url to show user where to download new stable builds from.
+    /// Custom builds must override to provide their own location.
+    virtual QString stableDownloadLocation() const { return QString("qgroundcontrol.com"); }
+
+    /// Returns the complex mission items to display in the Plan UI
+    /// @param complexMissionItemNames Default set of complex items
+    /// @return Complex items to be made available to user
+    virtual QStringList complexMissionItemNames(Vehicle* /*vehicle*/, const QStringList& complexMissionItemNames) { return complexMissionItemNames; }
+
+    /// Returns the standard list of first run prompt ids for possible display. Actual display is based on the
+    /// current AppSettings::firstRunPromptIds value. The order of this list also determines the order the prompts
+    /// will be displayed in.
+    virtual QList<int> firstRunPromptStdIds(void);
+
+    /// Returns the custom build list of first run prompt ids for possible display. Actual display is based on the
+    /// current AppSettings::firstRunPromptIds value. The order of this list also determines the order the prompts
+    /// will be displayed in.
+    virtual QList<int> firstRunPromptCustomIds(void);
+
+    /// Returns the resource which contains the specified first run prompt for display
+    Q_INVOKABLE virtual QString firstRunPromptResource(int id);
+
+    /// Returns the list of toolbar indicators which are not related to a vehicle
+    ///     signals toolbarIndicatorsChanged
+    /// @return A list of QUrl with the indicators
+    virtual const QVariantList& toolBarIndicators(void);
+
+    /// Returns the list of first run prompt ids which need to be displayed according to current settings
+    Q_INVOKABLE QVariantList firstRunPromptsToShow(void);
+
+    bool showTouchAreas() const { return _showTouchAreas; }
+    bool showAdvancedUI() const { return _showAdvancedUI; }
+    void setShowTouchAreas(bool show);
+    void setShowAdvancedUI(bool show);
+
+    // Override from QGCTool
+    void                            setToolbox              (QGCToolbox* toolbox);
+
+    // Standard first run prompt ids
+    static const int unitsFirstRunPromptId =            1;
+    static const int offlineVehicleFirstRunPromptId =   2;
+
+    // Custom builds can start there first run prompt ids from here
+    static const int firstRunPromptIdsFirstCustomId = 10000;
+
+signals:
+    void settingsPagesChanged       ();
+    void analyzePagesChanged        ();
+    void showTouchAreasChanged      (bool showTouchAreas);
+    void showAdvancedUIChanged      (bool showAdvancedUI);
+    void toolBarIndicatorsChanged   ();
+
+protected:
+    bool                _showTouchAreas;
+    bool                _showAdvancedUI;
+    Vehicle*            _activeVehicle  = nullptr;
+    QGCCameraManager*   _cameraManager  = nullptr;
+    QGCCameraControl*   _currentCamera  = nullptr;
+    QVariantList        _toolBarIndicatorList;
+
+private:
+    QGCCorePlugin_p*    _p;
+};

src/api/QGCOptions.cc

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ * (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 "QGCOptions.h"
+
+#include <QtQml>
+
+/// @file
+///     @brief Core Plugin Interface for QGroundControl - Application Options
+///     @author Gus Grubba <gus@auterion.com>
+
+QGCOptions::QGCOptions(QObject* parent)
+    : QObject(parent)
+{
+    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+}
+
+QColor QGCOptions::toolbarBackgroundLight() const
+{
+    return QColor(255,255,255);
+}
+
+QColor QGCOptions::toolbarBackgroundDark() const
+{
+    return QColor(0,0,0);
+}
+
+QGCFlyViewOptions* QGCOptions::flyViewOptions(void)
+{
+    if (!_defaultFlyViewOptions) {
+        _defaultFlyViewOptions = new QGCFlyViewOptions(this);
+    }
+    return _defaultFlyViewOptions;
+}
+
+QGCFlyViewOptions::QGCFlyViewOptions(QGCOptions* options, QObject* parent)
+    : QObject   (parent)
+    , _options  (options)
+{
+    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+}

src/api/QGCOptions.h

@@ -0,0 +1,170 @@
+/****************************************************************************
+ *
+ * (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 <QObject>
+#include <QString>
+#include <QUrl>
+#include <QColor>
+
+class QGCOptions;
+
+class QGCFlyViewOptions : public QObject
+{
+    Q_OBJECT
+public:
+    QGCFlyViewOptions(QGCOptions* options, QObject* parent = nullptr);
+
+    Q_PROPERTY(bool                     showMultiVehicleList            READ showMultiVehicleList           CONSTANT)
+    Q_PROPERTY(bool                     showInstrumentPanel             READ showInstrumentPanel            CONSTANT)
+    Q_PROPERTY(bool                     showMapScale                    READ showMapScale                   CONSTANT)
+    Q_PROPERTY(bool                     guidedBarShowEmergencyStop      READ guidedBarShowEmergencyStop     NOTIFY guidedBarShowEmergencyStopChanged)
+    Q_PROPERTY(bool                     guidedBarShowOrbit              READ guidedBarShowOrbit             NOTIFY guidedBarShowOrbitChanged)
+    Q_PROPERTY(bool                     guidedBarShowROI                READ guidedBarShowROI               NOTIFY guidedBarShowROIChanged)
+
+protected:
+    virtual bool    showMultiVehicleList        () const { return true; }
+    virtual bool    showMapScale                () const { return true; }
+    virtual bool    showInstrumentPanel         () const { return true; }
+    virtual bool    guidedBarShowEmergencyStop  () const { return true; }
+    virtual bool    guidedBarShowOrbit          () const { return true; }
+    virtual bool    guidedBarShowROI            () const { return true; }
+
+    QGCOptions* _options;
+
+signals:
+    void guidedBarShowEmergencyStopChanged      (bool show);
+    void guidedBarShowOrbitChanged              (bool show);
+    void guidedBarShowROIChanged                (bool show);
+};
+
+class QGCOptions : public QObject
+{
+    Q_OBJECT
+public:
+    QGCOptions(QObject* parent = nullptr);
+
+    Q_PROPERTY(bool                     combineSettingsAndSetup         READ combineSettingsAndSetup        CONSTANT)
+    Q_PROPERTY(double                   toolbarHeightMultiplier         READ toolbarHeightMultiplier        CONSTANT)
+    Q_PROPERTY(bool                     enablePlanViewSelector          READ enablePlanViewSelector         CONSTANT)
+    Q_PROPERTY(QUrl                     preFlightChecklistUrl           READ preFlightChecklistUrl          CONSTANT)
+    Q_PROPERTY(bool                     showSensorCalibrationCompass    READ showSensorCalibrationCompass   NOTIFY showSensorCalibrationCompassChanged)
+    Q_PROPERTY(bool                     showSensorCalibrationGyro       READ showSensorCalibrationGyro      NOTIFY showSensorCalibrationGyroChanged)
+    Q_PROPERTY(bool                     showSensorCalibrationAccel      READ showSensorCalibrationAccel     NOTIFY showSensorCalibrationAccelChanged)
+    Q_PROPERTY(bool                     showSensorCalibrationLevel      READ showSensorCalibrationLevel     NOTIFY showSensorCalibrationLevelChanged)
+    Q_PROPERTY(bool                     showSensorCalibrationAirspeed   READ showSensorCalibrationAirspeed  NOTIFY showSensorCalibrationAirspeedChanged)
+    Q_PROPERTY(bool                     sensorsHaveFixedOrientation     READ sensorsHaveFixedOrientation    CONSTANT)
+    Q_PROPERTY(bool                     wifiReliableForCalibration      READ wifiReliableForCalibration     CONSTANT)
+    Q_PROPERTY(bool                     showFirmwareUpgrade             READ showFirmwareUpgrade            NOTIFY showFirmwareUpgradeChanged)
+    Q_PROPERTY(QString                  firmwareUpgradeSingleURL        READ firmwareUpgradeSingleURL       CONSTANT)
+    Q_PROPERTY(bool                     missionWaypointsOnly            READ missionWaypointsOnly           NOTIFY missionWaypointsOnlyChanged)
+    Q_PROPERTY(bool                     multiVehicleEnabled             READ multiVehicleEnabled            NOTIFY multiVehicleEnabledChanged)
+    Q_PROPERTY(bool                     showOfflineMapExport            READ showOfflineMapExport           NOTIFY showOfflineMapExportChanged)
+    Q_PROPERTY(bool                     showOfflineMapImport            READ showOfflineMapImport           NOTIFY showOfflineMapImportChanged)
+    Q_PROPERTY(bool                     useMobileFileDialog             READ useMobileFileDialog            CONSTANT)
+    Q_PROPERTY(bool                     showMissionStatus               READ showMissionStatus              CONSTANT)
+    Q_PROPERTY(bool                     guidedActionsRequireRCRSSI      READ guidedActionsRequireRCRSSI     CONSTANT)
+    Q_PROPERTY(bool                     showMissionAbsoluteAltitude     READ showMissionAbsoluteAltitude    NOTIFY showMissionAbsoluteAltitudeChanged)
+    Q_PROPERTY(bool                     showSimpleMissionStart          READ showSimpleMissionStart         NOTIFY showSimpleMissionStartChanged)
+    Q_PROPERTY(bool                     disableVehicleConnection        READ disableVehicleConnection       CONSTANT)
+    Q_PROPERTY(float                    devicePixelRatio                READ devicePixelRatio               NOTIFY devicePixelRatioChanged)
+    Q_PROPERTY(float                    devicePixelDensity              READ devicePixelDensity             NOTIFY devicePixelDensityChanged)
+    Q_PROPERTY(bool                     checkFirmwareVersion            READ checkFirmwareVersion           CONSTANT)
+    Q_PROPERTY(bool                     showMavlinkLogOptions           READ showMavlinkLogOptions          CONSTANT)
+    Q_PROPERTY(bool                     enableSaveMainWindowPosition    READ enableSaveMainWindowPosition   CONSTANT)
+    Q_PROPERTY(QStringList              surveyBuiltInPresetNames        READ surveyBuiltInPresetNames       CONSTANT)
+    Q_PROPERTY(bool                     allowJoystickSelection          READ allowJoystickSelection         NOTIFY allowJoystickSelectionChanged)
+
+    Q_PROPERTY(QGCFlyViewOptions*       flyView                         READ flyViewOptions                 CONSTANT)
+
+    /// Should QGC hide its settings menu and colapse it into one single menu (Settings and Vehicle Setup)?
+    /// @return true if QGC should consolidate both menus into one.
+    virtual bool        combineSettingsAndSetup     () { return false; }
+
+    /// Main ToolBar Multiplier.
+    /// @return Factor to use when computing toolbar height
+    virtual double      toolbarHeightMultiplier     () { return 1.0; }
+
+    /// Enable Plan View Selector (Mission, Fence or Rally)
+    /// @return True or false
+    virtual bool        enablePlanViewSelector      () { return true; }
+
+    /// Should the mission status indicator (Plan View) be shown?
+    /// @return Yes or no
+    virtual bool    showMissionStatus               () { return true; }
+
+    /// Provides an optional, custom preflight checklist
+    virtual QUrl    preFlightChecklistUrl           () const { return QUrl::fromUserInput("qrc:/qml/PreFlightCheckList.qml"); }
+
+    /// Allows replacing the toolbar Light Theme color
+    virtual QColor  toolbarBackgroundLight          () const;
+    /// Allows replacing the toolbar Dark Theme color
+    virtual QColor  toolbarBackgroundDark           () const;
+    /// By returning false you can hide the following sensor calibration pages
+    virtual bool    showSensorCalibrationCompass    () const { return true; }
+    virtual bool    showSensorCalibrationGyro       () const { return true; }
+    virtual bool    showSensorCalibrationAccel      () const { return true; }
+    virtual bool    showSensorCalibrationLevel      () const { return true; }
+    virtual bool    showSensorCalibrationAirspeed   () const { return true; }
+    virtual bool    wifiReliableForCalibration      () const { return false; }
+    virtual bool    sensorsHaveFixedOrientation     () const { return false; }
+    virtual bool    showFirmwareUpgrade             () const { return true; }
+    virtual bool    missionWaypointsOnly            () const { return false; }  ///< true: Only allow waypoints and complex items in Plan
+    virtual bool    multiVehicleEnabled             () const { return true; }   ///< false: multi vehicle support is disabled
+    virtual bool    guidedActionsRequireRCRSSI      () const { return false; }  ///< true: Guided actions will be disabled is there is no RC RSSI
+    virtual bool    showOfflineMapExport            () const { return true; }
+    virtual bool    showOfflineMapImport            () const { return true; }
+    virtual bool    showMissionAbsoluteAltitude     () const { return true; }
+    virtual bool    showSimpleMissionStart          () const { return false; }
+    virtual bool    disableVehicleConnection        () const { return false; }  ///< true: vehicle connection is disabled
+    virtual bool    checkFirmwareVersion            () const { return true; }
+    virtual bool    showMavlinkLogOptions           () const { return true; }
+    virtual bool    allowJoystickSelection          () const { return true; }   ///< false: custom build has automatically enabled a specific joystick
+    /// Desktop builds save the main application size and position on close (and restore it on open)
+    virtual bool    enableSaveMainWindowPosition    () const { return true; }
+    virtual QStringList surveyBuiltInPresetNames    () const { return QStringList(); } // Built in presets cannot be deleted
+
+#if defined(__mobile__)
+    virtual bool    useMobileFileDialog             () const { return true;}
+#else
+    virtual bool    useMobileFileDialog             () const { return false;}
+#endif
+
+    /// If returned QString in non-empty it means that firmware upgrade will run in a mode which only
+    /// supports downloading a single firmware file from the URL. It also supports custom install through
+    /// the Advanced options.
+    virtual QString firmwareUpgradeSingleURL        () const { return QString(); }
+
+    /// Device specific pixel ratio/density (for when Qt doesn't properly read it from the hardware)
+    virtual float   devicePixelRatio                () const { return 0.0f; }
+    virtual float   devicePixelDensity              () const { return 0.0f; }
+
+    virtual QGCFlyViewOptions* flyViewOptions       ();
+
+signals:
+    void showSensorCalibrationCompassChanged    (bool show);
+    void showSensorCalibrationGyroChanged       (bool show);
+    void showSensorCalibrationAccelChanged      (bool show);
+    void showSensorCalibrationLevelChanged      (bool show);
+    void showSensorCalibrationAirspeedChanged   (bool show);
+    void showFirmwareUpgradeChanged             (bool show);
+    void missionWaypointsOnlyChanged            (bool missionWaypointsOnly);
+    void multiVehicleEnabledChanged             (bool multiVehicleEnabled);
+    void allowJoystickSelectionChanged          (bool allow);
+    void showOfflineMapExportChanged            ();
+    void showOfflineMapImportChanged            ();
+    void showMissionAbsoluteAltitudeChanged     ();
+    void showSimpleMissionStartChanged          ();
+    void devicePixelRatioChanged                ();
+    void devicePixelDensityChanged              ();
+
+protected:
+    QGCFlyViewOptions* _defaultFlyViewOptions = nullptr;
+};

src/api/QGCSettings.cc

@@ -0,0 +1,21 @@
+/****************************************************************************
+ *
+ * (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 "QGCSettings.h"
+
+/// @file
+///     @brief Core Plugin Interface for QGroundControl. Settings element.
+///     @author Gus Grubba <gus@auterion.com>
+
+QGCSettings::QGCSettings(QString title, QUrl url, QUrl icon)
+    : _title(title)
+    , _url(url)
+    , _icon(icon)
+{
+}

src/api/QGCSettings.h

@@ -0,0 +1,37 @@
+/****************************************************************************
+ *
+ * (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 <QObject>
+#include <QUrl>
+
+/// @file
+/// @brief Core Plugin Interface for QGroundControl. Settings element.
+/// @author Gus Grubba <gus@auterion.com>
+
+class QGCSettings : public QObject
+{
+    Q_OBJECT
+public:
+    QGCSettings(QString title, QUrl url, QUrl icon = QUrl());
+
+    Q_PROPERTY(QString  title       READ title      CONSTANT)
+    Q_PROPERTY(QUrl     url         READ url        CONSTANT)
+    Q_PROPERTY(QUrl     icon        READ icon       CONSTANT)
+
+    virtual QString     title       () { return _title; }
+    virtual QUrl        url         () { return _url;   }
+    virtual QUrl        icon        () { return _icon;  }
+
+protected:
+    QString _title;
+    QUrl    _url;
+    QUrl    _icon;
+};

src/api/QmlComponentInfo.cc

@@ -0,0 +1,19 @@
+/****************************************************************************
+ *
+ * (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 "QmlComponentInfo.h"
+
+QmlComponentInfo::QmlComponentInfo(QString title, QUrl url, QUrl icon, QObject* parent)
+    : QObject   (parent)
+    , _title    (title)
+    , _url      (url)
+    , _icon     (icon)
+{
+
+}

src/api/QmlComponentInfo.h

@@ -0,0 +1,35 @@
+/****************************************************************************
+ *
+ * (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 <QObject>
+#include <QUrl>
+
+/// Represents a Qml component which can be loaded from a resource.
+class QmlComponentInfo : public QObject
+{
+    Q_OBJECT
+
+public:
+    QmlComponentInfo(QString title, QUrl url, QUrl icon = QUrl(), QObject* parent = nullptr);
+
+    Q_PROPERTY(QString  title   READ title  CONSTANT)   ///< Title for page
+    Q_PROPERTY(QUrl     url     READ url    CONSTANT)   ///< Qml source code
+    Q_PROPERTY(QUrl     icon    READ icon   CONSTANT)   ///< Icon for page
+
+    virtual QString title   () { return _title; }
+    virtual QUrl    url     () { return _url;   }
+    virtual QUrl    icon    () { return _icon;  }
+
+protected:
+    QString _title;
+    QUrl    _url;
+    QUrl    _icon;
+};

src/fhog.cpp

@@ -1,43 +1,3 @@
-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-//  By downloading, copying, installing or using the software you agree to this license.
-//  If you do not agree to this license, do not download, install,
-//  copy or use the software.
-//
-//
-//                           License Agreement
-//                For Open Source Computer Vision Library
-//
-// Copyright (C) 2010-2013, University of Nizhny Novgorod, all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-//   * Redistribution's of source code must retain the above copyright notice,
-//     this list of conditions and the following disclaimer.
-//
-//   * Redistribution's in binary form must reproduce the above copyright notice,
-//     this list of conditions and the following disclaimer in the documentation
-//     and/or other materials provided with the distribution.
-//
-//   * The name of the copyright holders may not be used to endorse or promote products
-//     derived from this software without specific prior written permission.
-//
-// This software is provided by the copyright holders and contributors "as is" and
-// any express or implied warranties, including, but not limited to, the implied
-// warranties of merchantability and fitness for a particular purpose are disclaimed.
-// In no event shall the Intel Corporation or contributors be liable for any direct,
-// indirect, incidental, special, exemplary, or consequential damages
-// (including, but not limited to, procurement of substitute goods or services;
-// loss of use, data, or profits; or business interruption) however caused
-// and on any theory of liability, whether in contract, strict liability,
-// or tort (including negligence or otherwise) arising in any way out of
-// the use of this software, even if advised of the possibility of such damage.
-//
-//M*/
 
 
 //Modified from latentsvm module's "lsvmc_featurepyramid.cpp".
@@ -77,60 +37,92 @@
 // RESULT
 // Error status
 */
+// 从输入的图像中提取特征映射
 int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade **map)
 {
+    // 图像的宽度和高度
     int sizeX, sizeY;
-    int p, px, stringSize;
+    // 每个cell中的特征数量
+    int p, px;
+    // 存储特征的字符串大小
+    int stringSize;
+    // 图像的高度、宽度和通道数
     int height, width, numChannels;
+    // 循环变量
     int i, j, kk, c, ii, jj, d;
+    // 梯度数据指针
     float  * datadx, * datady;
 
+    // 角度相关变量
     int   ch;
     float magnitude, x, y, tx, ty;
 
+    // 梯度图像
     IplImage * dx, * dy;
+    // 最近的邻域点索引
     int *nearest;
-    float *w, a_x, b_x;
+    // 权重向量
+    float *w;
+    // 线性插值系数
+    float a_x, b_x;
 
+    // 梯度算子
     float kernel[3] = {-1.f, 0.f, 1.f};
+    // OpenCV矩阵格式的梯度算子
     CvMat kernel_dx = cvMat(1, 3, CV_32F, kernel);
     CvMat kernel_dy = cvMat(3, 1, CV_32F, kernel);
 
+    // 临时指针
     float * r;
+    // 未知用途的整型数组（可能是用于后续处理的索引或标志）
     int   * alfa;
 
+    // 边界角度值数组
     float boundary_x[NUM_SECTOR + 1];
     float boundary_y[NUM_SECTOR + 1];
+    // 用于查找最大值的变量
     float max, dotProd;
     int   maxi;
 
+    // 获取图像的高度和宽度
     height = image->height;
-    width  = image->width ;
+    width  = image->width;
 
+    // 获取图像的通道数
     numChannels = image->nChannels;
 
+    // 创建用于存储梯度图像的容器
     dx    = cvCreateImage(cvSize(image->width, image->height),
                           IPL_DEPTH_32F, 3);
     dy    = cvCreateImage(cvSize(image->width, image->height),
                           IPL_DEPTH_32F, 3);
 
+    // 计算特征映射的宽和高（基于k的大小）
     sizeX = width  / k;
     sizeY = height / k;
+    // 计算每个cell中的特征数量（基于NUM_SECTOR）
     px    = 3 * NUM_SECTOR;
     p     = px;
+    // 计算整个特征映射所需的总存储大小
     stringSize = sizeX * p;
+    // 分配特征映射对象的内存
     allocFeatureMapObject(map, sizeX, sizeY, p);
 
+    // 使用Sobel算子计算图像的梯度
     cvFilter2D(image, dx, &kernel_dx, cvPoint(-1, 0));
     cvFilter2D(image, dy, &kernel_dy, cvPoint(0, -1));
 
+    // 计算边界角度值
     float arg_vector;
     for(i = 0; i <= NUM_SECTOR; i++)
     {
+        // 计算角度（基于NUM_SECTOR将圆周等分为NUM_SECTOR个部分）
         arg_vector    = ( (float) i ) * ( (float)(PI) / (float)(NUM_SECTOR) );
+        // 计算对应的余弦和正弦值
         boundary_x[i] = cosf(arg_vector);
         boundary_y[i] = sinf(arg_vector);
-    }/*for(i = 0; i <= NUM_SECTOR; i++) */
+    }
+
 
     r    = (float *)malloc( sizeof(float) * (width * height));
     alfa = (int   *)malloc( sizeof(int  ) * (width * height * 2));
@@ -287,43 +279,63 @@ int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade *
 // RESULT
 // Error status
 */
+// 对特征映射进行归一化和截断处理
 int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa)
 {
-    int i,j, ii;
-    int sizeX, sizeY, p, pos, pp, xp, pos1, pos2;
-    float * partOfNorm; // norm of C(i, j)
-    float * newData;
-    float   valOfNorm;
+    int i, j, ii; // 定义循环变量
+    int sizeX, sizeY, p, pos, pp, xp; // 定义变量
+    float * partOfNorm; // 存储C(i, j)的范数的指针
+    float * newData; // 存储归一化后数据的指针
+    float valOfNorm; // 存储范数的值
 
-    sizeX     = map->sizeX;
-    sizeY     = map->sizeY;
-    partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY));
+    sizeX = map->sizeX; // 获取特征映射的宽度
+    sizeY = map->sizeY; // 获取特征映射的高度
+    partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY)); // 分配存储范数的内存
 
-    p  = NUM_SECTOR;
-    xp = NUM_SECTOR * 3;
-    pp = NUM_SECTOR * 12;
+    p  = NUM_SECTOR; // 定义一个常量，可能表示特征的分区数
+    xp = NUM_SECTOR * 3; // 定义一个常量，可能表示特征的某种属性
+    pp = NUM_SECTOR * 12; // 定义一个常量，可能表示特征的某种属性
 
+    // 计算每个部分区域的范数
     for(i = 0; i < sizeX * sizeY; i++)
     {
-        valOfNorm = 0.0f;
-        pos = i * map->numFeatures;
-        for(j = 0; j < p; j++)
+        valOfNorm = 0.0f; // 初始化范数为0
+        pos = i * map->numFeatures; // 计算每个部分的起始位置
+        for(j = 0; j < p; j++) // 遍历每个部分的特征
         {
-            valOfNorm += map->map[pos + j] * map->map[pos + j];
+            valOfNorm += map->map[pos + j] * map->map[pos + j]; // 计算每个部分的范数
         }/*for(j = 0; j < p; j++)*/
-        partOfNorm[i] = valOfNorm;
+        partOfNorm[i] = valOfNorm; // 将每个部分的范数存储起来
     }/*for(i = 0; i < sizeX * sizeY; i++)*/
 
+    // 调整特征映射的大小
     sizeX -= 2;
     sizeY -= 2;
 
+    // 分配存储归一化后数据的内存
     newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp));
-//normalization
+
+    // 归一化处理
     for(i = 1; i <= sizeY; i++)
     {
         for(j = 1; j <= sizeX; j++)
         {
-            valOfNorm = sqrtf(
+                valOfNorm = sqrtf( // 计算当前像素的范数
+                partOfNorm[(i    )*(sizeX + 2) + (j    )] +
+                partOfNorm[(i    )*(sizeX + 2) + (j + 1)] +
+                partOfNorm[(i + 1)*(sizeX + 2) + (j    )] +
+                partOfNorm[(i + 1)*(sizeX + 2) + (j + 1)]) + FLT_EPSILON;
+            pos1 = (i  ) * (sizeX + 2) * xp + (j  ) * xp;
+            pos2 = (i-1) * (sizeX    ) * pp + (j-1) * pp;
+            for(ii = 0; ii < p; ii++)
+            {
+                newData[pos2 + ii        ] = map->map[pos1 + ii    ] / valOfNorm; // 归一化
+            }/*for(ii = 0; ii < p; ii++)*/
+            for(ii = 0; ii < 2 * p; ii++)
+            {
+                newData[pos2 + ii + p * 4] = map->map[pos1 + ii + p] / valOfNorm; // 归一化
+            }/*for(ii = 0; ii < 2 * p; ii++)*/
+            valOfNorm = sqrtf( // 计算当前像素的范数
                 partOfNorm[(i    )*(sizeX + 2) + (j    )] +
                 partOfNorm[(i    )*(sizeX + 2) + (j + 1)] +
                 partOfNorm[(i + 1)*(sizeX + 2) + (j    )] +
@@ -411,102 +423,159 @@ int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa)
 // RESULT
 // Error status
 */
+// 对输入的CvLSVMFeatureMapCaskade对象进行PCA特征映射处理
 int PCAFeatureMaps(CvLSVMFeatureMapCaskade *map)
 {
-    int i,j, ii, jj, k;
+    int i, j, ii, jj, k;
     int sizeX, sizeY, p, pp, xp, yp, pos1, pos2;
     float * newData;
     float val;
     float nx, ny;
 
+    // 获取特征映射的宽、高和特征数量
     sizeX = map->sizeX;
     sizeY = map->sizeY;
     p     = map->numFeatures;
+
+    // 定义新的特征数量pp和xp、yp等参数
     pp    = NUM_SECTOR * 3 + 4;
     yp    = 4;
     xp    = NUM_SECTOR;
 
+    // 计算nx和ny两个归一化因子
     nx    = 1.0f / sqrtf((float)(xp * 2));
-    ny    = 1.0f / sqrtf((float)(yp    ));
+    ny    = 1.0f / sqrtf((float)(yp));
 
+    // 为新的特征映射数据分配内存
     newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp));
+    if (newData == NULL) {
+        // 如果内存分配失败，应返回错误代码
+        // 这里为了简洁省略了错误处理
+        return LATENT_SVM_MEM_ERROR; // 假设的错误码
+    }
 
+    // 遍历特征映射的每一个位置
     for(i = 0; i < sizeY; i++)
     {
         for(j = 0; j < sizeX; j++)
         {
+            // 计算原始数据在当前位置的索引pos1
             pos1 = ((i)*sizeX + j)*p;
+            // 计算新数据在当前位置的索引pos2
             pos2 = ((i)*sizeX + j)*pp;
-            k = 0;
+
+            k = 0; // 初始化k用于索引新数据的位置
+
+            // 计算第一个部分的新特征
             for(jj = 0; jj < xp * 2; jj++)
             {
-                val = 0;
+                val = 0; // 初始化累加值
+                // 遍历yp个原始特征值进行累加
                 for(ii = 0; ii < yp; ii++)
                 {
                     val += map->map[pos1 + yp * xp + ii * xp * 2 + jj];
-                }/*for(ii = 0; ii < yp; ii++)*/
+                }
+                // 将累加值乘以ny并存储到新数据中
                 newData[pos2 + k] = val * ny;
                 k++;
-            }/*for(jj = 0; jj < xp * 2; jj++)*/
+            }
+
+            // 计算第二个部分的新特征
             for(jj = 0; jj < xp; jj++)
             {
-                val = 0;
+                val = 0; // 初始化累加值
+                // 遍历yp个原始特征值进行累加
                 for(ii = 0; ii < yp; ii++)
                 {
                     val += map->map[pos1 + ii * xp + jj];
-                }/*for(ii = 0; ii < yp; ii++)*/
+                }
+                // 将累加值乘以ny并存储到新数据中
                 newData[pos2 + k] = val * ny;
                 k++;
-            }/*for(jj = 0; jj < xp; jj++)*/
+            }
+
+            // 计算第三个部分的新特征
             for(ii = 0; ii < yp; ii++)
             {
-                val = 0;
+                val = 0; // 初始化累加值
+                // 遍历2*xp个原始特征值进行累加
                 for(jj = 0; jj < 2 * xp; jj++)
                 {
                     val += map->map[pos1 + yp * xp + ii * xp * 2 + jj];
-                }/*for(jj = 0; jj < xp; jj++)*/
+                }
+                // 将累加值乘以nx并存储到新数据中
                 newData[pos2 + k] = val * nx;
                 k++;
-            } /*for(ii = 0; ii < yp; ii++)*/           
-        }/*for(j = 0; j < sizeX; j++)*/
-    }/*for(i = 0; i < sizeY; i++)*/
-//swop data
+            }
+        }
+    }
 
+    // 交换数据，这里看起来没有实际的交换操作，可能是对数据的后续处理或注释有误
+
+    // 更新特征数量
     map->numFeatures = pp;
 
+    // 释放原始特征映射数据占用的内存
     free (map->map);
 
+    // 将新的特征映射数据赋值给map的map成员
     map->map = newData;
 
+    // 返回成功码
     return LATENT_SVM_OK;
 }
 
-
-//modified from "lsvmc_routine.cpp"
-//两个函数分别用于分配和释放CvLSVMFeatureMapCaskade结构体的内存。
-
+// 分配一个CvLSVMFeatureMapCaskade对象并初始化其成员
 int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX,
                           const int sizeY, const int numFeatures)
 {
     int i;
+
+    // 使用malloc为CvLSVMFeatureMapCaskade对象分配内存
     (*obj) = (CvLSVMFeatureMapCaskade *)malloc(sizeof(CvLSVMFeatureMapCaskade));
+    if (!(*obj)) { // 如果分配失败，应该返回一个错误代码，但这里假设它总是成功的
+        // 实际应用中应该添加错误处理
+    }
+
+    // 初始化对象的成员变量
     (*obj)->sizeX       = sizeX;
     (*obj)->sizeY       = sizeY;
     (*obj)->numFeatures = numFeatures;
+
+    // 使用malloc为map成员分配内存，它用于存储特征映射数据
     (*obj)->map = (float *) malloc(sizeof (float) *
                                   (sizeX * sizeY  * numFeatures));
+    if (!(*obj)->map) { // 如果分配失败，应该返回一个错误代码，但这里假设它总是成功的
+        // 实际应用中应该添加错误处理，并释放已经为对象分配的内存
+        free(*obj);
+        *obj = NULL;
+        return LATENT_SVM_MEM_ERROR; // 假设这是一个内存分配错误的返回码
+    }
+
+    // 将map中的所有元素初始化为0.0f
     for(i = 0; i < sizeX * sizeY * numFeatures; i++)
     {
         (*obj)->map[i] = 0.0f;
     }
+
+    // 返回成功码
     return LATENT_SVM_OK;
 }
 
+// 释放一个CvLSVMFeatureMapCaskade对象占用的内存
 int freeFeatureMapObject (CvLSVMFeatureMapCaskade **obj)
 {
+    // 如果obj为空，返回内存为空的错误码
     if(*obj == NULL) return LATENT_SVM_MEM_NULL;
+
+    // 释放map成员占用的内存
     free((*obj)->map);
+    // 释放对象本身占用的内存
     free(*obj);
+
+    // 将指针设置为NULL，避免野指针
     (*obj) = NULL;
+
+    // 返回成功码
     return LATENT_SVM_OK;
 }

src/gimbal_base.hpp

@@ -0,0 +1,454 @@
+#ifndef gimbal_base
+#define gimbal_base
+#include <stdint.h>
+#include <iostream>
+#include <opencv2/videoio.hpp>
+#include <opencv2/videoio/videoio_c.h>
+// #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+class GimbalBase
+{
+    friend std::ostream &operator<<(std::ostream &output, GimbalBase *gimbal);
+
+public:
+    static std::string time_string_now();
+
+
+    // vel rpy
+    GimbalBase() = delete;
+    GimbalBase(std::string save_path = "gimbal_save_data", std::string device_name = "", int max_zoom = 0x4000, int min_zoom = 0x0000)
+        : past_clock(std::chrono::system_clock::now()), past_clock_zoom(std::chrono::system_clock::now()), max_zoom(max_zoom), min_zoom(min_zoom), save_path(save_path), device_name(device_name)
+    {
+
+        this->save_path = save_path[save_path.size() - 1] == '/' ? save_path : save_path + '/';
+        this->save_path = this->save_path[0] == '/' ? this->save_path : std::string("/") + this->save_path;
+        this->save_path = std::string(getenv("HOME")) + this->save_path;
+        std::cout << "Data Save Path: " << this->save_path << std::endl;
+        if (access(this->save_path.c_str(), 0x00) != 0)
+        {
+            mkdir(this->save_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR | S_IRWXG | S_IRWXO);
+            if (access(this->save_path.c_str(), 0x00) != 0)
+            {
+                throw std::runtime_error("Create dir for save DIR failed!");
+            }
+        }
+    };
+    GimbalBase(const GimbalBase &) = delete;
+    virtual ~GimbalBase() = default;
+    // 派生类必须要实现到方法
+    virtual void open_tty(std::string tty_port = "/dev/ttyUSB0") = 0;
+    virtual void open_camera(std::string camera_id = "0", int width = 640, int height = 480, int fps=30) = 0;
+    virtual bool get_info(float ret[12]) = 0; // 入口
+    virtual void ctl_gimbal(uint8_t rpy_ctl[3], double rpy_vel[3], bool stop_search = true) = 0;
+    virtual void home() = 0;
+    virtual void hold() = 0;
+    virtual void fellow() = 0;
+
+    // 基类已有基础功能
+    virtual void tracking(float xy_info[6], int ctl_roll = 0, float kp = 0.1, float ki = 0.0, float kd = 0.0, float ignore_error = 35);
+    virtual bool search(bool is_roll, float search_vision_h = 50, float search_vision_v = 30, float search_center_h = 0, float search_center_v = 45);
+    virtual bool start_record();
+    virtual void finish_record();
+    virtual bool get_picture(std::string name = "");
+    virtual float get_dt();
+
+    // 派生类可选
+    virtual void focus_out(){};
+    virtual void focus_in(){};
+    virtual void focus_stop(){};
+    virtual void zoom_in(){};
+    virtual void zoom_out(){};
+    virtual void zoom_stop(){};
+    virtual uint16_t get_zoom_val(){};
+    virtual bool auto_zoom(float curr_area, uint16_t now_zoom_val, float thresh_area_up = 0.1, float thresh_area_down = 0.5){};
+
+    // 读取视频流
+    cv::VideoCapture &operator>>(cv::Mat &mat);
+    cv::VideoCapture cap;
+    // 保存视频
+    cv::VideoWriter outvideo;
+    // 当前正在处理的帧
+    cv::Mat frame;
+
+    // 当期状态
+    enum
+    {
+        MANUAL_CONTROL = 0,
+        HOME_BACK,
+        TRACKING,
+        YAW_FOLLOW,
+        HOLDING,
+        SEARCHING
+    } move_state;
+
+    enum
+    {
+        ZOOM_KEEP = 0,
+        ZOOM_OUT,
+        ZOOM_IN,
+        ZOOM_AUTO
+    } zoom_state;
+    enum
+    {
+        INTERACTIVE_FEEDBACK = 0,
+        AUTO_FEEDBACK
+    } feedback_state;
+
+    uint16_t zoom_val = 0;
+    float fzoom_val = 0.;
+    bool is_recording = false;
+
+    std::string save_path;
+    const uint16_t max_zoom;
+    const uint16_t min_zoom;
+
+    // 停止
+    bool is_stop_auto = false;
+
+    // 吊舱角度信息
+    float past_stator_rel_camera[3];
+    float gimbal_state[12];
+    float gimbal_state_paste[12];
+    /*
+    std::cout << "----------------------gimbal-now-status-------------------------" << std::endl;
+    std::cout << "imu_roll:       " << std::setprecision(6) << ret[0] << std::endl;
+    std::cout << "imu_pitch:      " << std::setprecision(6) << ret[1] << std::endl;
+    std::cout << "imu_yaw:        " << std::setprecision(6) << ret[2] << std::endl;
+    std::cout << "target_roll:    " << std::setprecision(6) << ret[3] << std::endl;
+    std::cout << "target_pitch:   " << std::setprecision(6) << ret[4] << std::endl;
+    std::cout << "target_yaw:     " << std::setprecision(6) << ret[5] << std::endl;
+    std::cout << "rel_roll:       " << std::setprecision(6) << ret[6] << std::endl;
+    std::cout << "rel_pitch:      " << std::setprecision(6) << ret[7] << std::endl;
+    std::cout << "rel_yaw:        " << std::setprecision(6) << ret[8] << std::endl;
+    std::cout << "imu_roll_vel:   " << std::setprecision(6) << ret[9] << std::endl;
+    std::cout << "imu_pitch_vel:  " << std::setprecision(6) << ret[10] << std::endl;
+    std::cout << "imu_yaw_vel:    " << std::setprecision(6) << ret[11] << std::endl;
+    */
+
+    const std::string device_name;
+
+protected:
+    std::chrono::system_clock::time_point past_clock;
+    // 保存上次查询放大倍数时间
+    std::chrono::system_clock::time_point past_clock_zoom;
+
+    int camera_width;
+    int camera_hight;
+
+    // 用于中断吊舱搜索到线程
+    bool stop_search_;
+    // 吊仓搜索路线
+    float search_line[4][2];
+    std::thread *t_search = nullptr;
+
+    void _search(bool is_roll = false);
+    // 用于再吊舱进行转动搜索时判定，是否到达期望值。
+    // q10f控制的是电机角度， g1控制的是yaw电机角度，roll,pitch到imu角度
+    double control_feedback_truth[3];
+};
+
+bool GimbalBase::search(bool is_roll, float search_vision_h, float search_vision_v, float search_center_h, float search_center_v)
+{
+    float ignore_error = 5;
+    this->search_line[0][0] = search_center_v - search_vision_v / 2 - ignore_error;
+    this->search_line[0][1] = search_center_h - search_vision_h / 2 - ignore_error;
+
+    this->search_line[1][0] = search_center_v - search_vision_v / 2 - ignore_error;
+    this->search_line[1][1] = search_center_h + search_vision_h / 2 + ignore_error;
+
+    this->search_line[2][0] = search_center_v + search_vision_v / 2 + ignore_error;
+    this->search_line[2][1] = search_center_h + search_vision_h / 2 + ignore_error;
+
+    this->search_line[3][0] = search_center_v + search_vision_v / 2 + ignore_error;
+    this->search_line[3][1] = search_center_h - search_vision_h / 2 - ignore_error;
+    // 判定是否存在
+    if (t_search && t_search->joinable())
+    {
+        this->stop_search_ = true;
+        t_search->join();
+    }
+    this->stop_search_ = false;
+    t_search = new std::thread(&GimbalBase::_search, this, is_roll);
+    this->move_state = SEARCHING;
+    return t_search->joinable();
+}
+
+void GimbalBase::_search(bool is_roll)
+{
+    float t = 0;
+    uint8_t rpy_ctl[3]{2, 2, 2};
+    double rpy_val[3]{0, 0, 0};
+    while (true)
+    {
+        if (this->stop_search_)
+        {
+            return;
+        }
+        for (int i = 0; i < 4 && !this->stop_search_; ++i)
+        {
+            if (is_roll)
+            {
+                rpy_val[0] = this->search_line[i][1];
+                rpy_val[1] = this->search_line[i][0];
+                rpy_val[2] = 0;
+            }
+            else
+            {
+                rpy_val[0] = 0;
+                rpy_val[1] = this->search_line[i][0];
+                rpy_val[2] = this->search_line[i][1];
+            }
+
+            // 等待到达指定角度
+            while (true)
+            {
+                if (this->stop_search_)
+                {
+                    return;
+                }
+                this->ctl_gimbal(rpy_ctl, rpy_val, false);
+                std::cout << "\033[33m"
+                          << "Wait for go to the point"
+                          << "\033[30m"
+                          << "\n"
+                          << "target_roll: " << rpy_val[0] << " "
+                          << "current_roll: " << this->control_feedback_truth[0] << "\n"
+                          << "target_pitch: " << rpy_val[1] << " "
+                          << "current_pitch: " << this->control_feedback_truth[1] << "\n"
+                          << "target_yaw: " << rpy_val[2] << " "
+                          << ":current_yaw: " << this->control_feedback_truth[2] << "\n"
+                          << std::endl;
+                using namespace std::chrono_literals;
+                std::this_thread::sleep_for(10ms);
+                if (is_roll)
+                {
+                    if (std::fabs(rpy_val[0] - this->control_feedback_truth[0]) < 3 && std::fabs(rpy_val[1] - this->control_feedback_truth[1]) < 3)
+                        break;
+                }
+                else
+                {
+                    if (std::fabs(rpy_val[2] - this->control_feedback_truth[2]) < 3 && std::fabs(rpy_val[1] - this->control_feedback_truth[1]) < 3)
+                        break;
+                }
+            }
+        }
+    }
+}
+
+void GimbalBase::tracking(float xy_info[2], int ctl_roll, float kp, float ki, float kd, float ignore_error)
+{
+    // std::cout << "PID: " << kp << " " << ki << " " << kd << std::endl;
+    static std::chrono::system_clock::time_point paste_track_ts = std::chrono::system_clock::now();
+    static float i_x = 0, i_y = 0;
+
+    std::chrono::system_clock::time_point now_ts = std::chrono::system_clock::now();
+    // TODO: 就很离谱，有时候会dt值会变成0.0001几，先使用指数平均顶着
+    std::chrono::duration<double> dt = now_ts - paste_track_ts;
+    static std::chrono::duration<double> filter_dt = dt;
+    if (dt.count() > 0.01)
+        filter_dt = 0.5 * filter_dt + 0.5 * dt;
+
+    float x = xy_info[0];
+    float y = xy_info[1];
+    static float paste_xy[2]{x, y};
+
+    float dt_error_x = x - paste_xy[0];
+    float dt_error_y = y - paste_xy[1];
+    static float dt_error[2]{dt_error_x, dt_error_y};
+
+    // 重置
+    if (std::abs(std::sqrt(x * x + y * y) - std::sqrt(paste_xy[0] * paste_xy[0] + paste_xy[1] * paste_xy[1])) > 50)
+    {
+        paste_xy[0] = x;
+        paste_xy[1] = y;
+        dt_error_x = 0;
+        dt_error_y = 0;
+        dt_error[0] = 0;
+        dt_error[1] = 0;
+        i_x = 0;
+        i_y = 0;
+        // std::cout << ignore_error <<" :重置历史数据: " << std::abs(std::sqrt(x * x + y * y) - std::sqrt(paste_xy[0] * paste_xy[0] + paste_xy[1] * paste_xy[1])) << std::endl;
+    }
+
+    float vel_x = (dt_error_x - dt_error[0]) / filter_dt.count();
+    float vel_y = (dt_error_y - dt_error[1]) / filter_dt.count();
+    i_x += dt_error_x * filter_dt.count();
+    i_y += dt_error_y * filter_dt.count();
+
+    paste_track_ts = now_ts;
+    paste_xy[0] = x;
+    paste_xy[1] = y;
+    dt_error[0] = dt_error_x;
+    dt_error[1] = dt_error_y;
+    // std::cout << "vel_x: " << vel_x << " vel_y: " << vel_y << " dt: " << dt.count() << std::endl;
+    // kd = 0;
+
+    // TODO 没有提供vel_x, i_x时自己计算
+    // std::cout << "像素误差: " << x << " " << y << std::endl;
+    // ignore_error = ignore_error * (get_zoom_val() / max_zoom);
+    x = std::abs(x) > std::fmax(ignore_error, 10) ? (x > 0 ? 1 : -1) * fmax(std::abs(x) * kp, 1) : 0;
+    y = std::abs(y) > std::fmax(ignore_error, 10) ? (y > 0 ? 1 : -1) * fmax(std::abs(y) * kp, 1) : 0;
+    // std::cout << "像素误差: " << x << " " << y << std::endl;
+
+    double mix_control = (x == 0 || std::abs(x) == 1) ? x : x + ki * i_x + kd * vel_x;
+    double pitch_control = (y == 0 || std::abs(y) == 1) ? y : y + ki * i_y + kd * vel_y;
+
+    // std::cout << "最终速度: " << pitch_control << " " << mix_control << std::endl;
+    uint8_t ctl[3];
+    double vel[3];
+    switch (ctl_roll)
+    {
+    case 0: // yaw + pitch
+        ctl[0] = 2;
+        ctl[1] = 1;
+        ctl[2] = 1;
+        vel[0] = 0;
+        vel[1] = pitch_control;
+        vel[2] = mix_control;
+        break;
+    case 1: // roll + pitch
+        ctl[0] = 1;
+        ctl[1] = 1;
+        ctl[2] = 2;
+        vel[0] = -mix_control;
+        vel[1] = pitch_control;
+        vel[2] = 0;
+        break;
+    case 2: // 混合控制
+        /*
+        "imu_roll:        ret[0]
+        "imu_pitch:       ret[1]
+        "imu_yaw:         ret[2]
+        "target_roll:     ret[3]
+        "target_pitch:    ret[4]
+        "target_yaw:      ret[5]
+        "rel_roll:        ret[6]
+        "rel_pitch:       ret[7]
+        "rel_yaw:         ret[8]
+        "imu_roll_vel:    ret[9]
+        "imu_pitch_vel:   ret[10]
+        "imu_yaw_vel:     ret[11]
+        "rel_roll_vel:    ret[12]
+        "rel_pitch_vel:   ret[13]
+        "rel_yaw_vel:     ret[14]
+        */
+        // static bool is_switch = true;
+        // if (this->gimbal_state[7] < 70) // pitch + roll
+        // {
+        //     if (is_switch == true)
+        //     {
+        //         is_switch = false;
+        //     }
+        //     else
+        //     {
+        //     }
+        //     /*
+        //     1. roll = (if yaw > 0 ? -1 : 1 ) * (90 - abs(yaw))
+        //     2. yaw = 0
+        //     */
+        // }
+        // else // pitch + yaw
+        // {
+        //     if (is_switch == false)
+        //     {
+        //         is_switch = true;
+        //     }
+        //     else
+        //     {
+        //     }
+        //     /*
+        //     1. yaw = (if roll > 0 ? -1 : 1 ) * (90 - abs(roll))
+        //     2. roll = 0
+        //     */
+        // }
+        break;
+    }
+    this->ctl_gimbal(ctl, vel);
+    this->move_state = TRACKING;
+}
+
+float GimbalBase::get_dt()
+{
+    std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
+    std::chrono::duration<double> elapsed_seconds = past_clock - now;
+    past_clock = now;
+    return elapsed_seconds.count();
+};
+
+std::ostream &operator<<(std::ostream &output, GimbalBase *gimbal)
+{
+    while (true)
+    {
+        float ret[12];
+        // std::memset(ret, 0, 15);
+        if (!gimbal->get_info(ret))
+            continue;
+        std::cout << "----------------------gimbal-now-status-------------------------" << std::endl;
+        std::cout << "imu_roll:       " << std::setprecision(6) << ret[0] << std::endl;
+        std::cout << "imu_pitch:      " << std::setprecision(6) << ret[1] << std::endl;
+        std::cout << "imu_yaw:        " << std::setprecision(6) << ret[2] << std::endl;
+        std::cout << "target_roll:    " << std::setprecision(6) << ret[3] << std::endl;
+        std::cout << "target_pitch:   " << std::setprecision(6) << ret[4] << std::endl;
+        std::cout << "target_yaw:     " << std::setprecision(6) << ret[5] << std::endl;
+        std::cout << "rel_roll:       " << std::setprecision(6) << ret[6] << std::endl;
+        std::cout << "rel_pitch:      " << std::setprecision(6) << ret[7] << std::endl;
+        std::cout << "rel_yaw:        " << std::setprecision(6) << ret[8] << std::endl;
+        std::cout << "imu_roll_vel:   " << std::setprecision(6) << ret[9] << std::endl;
+        std::cout << "imu_pitch_vel:  " << std::setprecision(6) << ret[10] << std::endl;
+        std::cout << "imu_yaw_vel:    " << std::setprecision(6) << ret[11] << std::endl;
+        break;
+    }
+    return output;
+};
+
+cv::VideoCapture &GimbalBase::operator>>(cv::Mat &mat)
+{
+    cap >> frame;
+    mat = frame;
+    if (outvideo.isOpened())
+    {
+        outvideo << frame.clone();
+    }
+    return cap;
+};
+
+bool GimbalBase::start_record()
+{
+    if (!outvideo.isOpened())
+    {
+        std::string video_path = this->save_path + time_string_now() + ".mp4";
+        std::string pipeline = "appsrc ! autovideoconvert ! omxh265enc ! matroskamux ! filesink location=" + video_path;
+        outvideo.open(pipeline, CV_FOURCC('M', 'P', '4', 'V'), 25.0, cv::Size(this->camera_width, this->camera_hight));
+        this->is_recording = true;
+    }
+    return outvideo.isOpened();
+}
+void GimbalBase::finish_record()
+{
+    this->is_recording = false;
+    outvideo.release();
+}
+
+bool GimbalBase::get_picture(std::string name)
+{
+    std::string img_path;
+    if (name.empty())
+    {
+        img_path = this->save_path + time_string_now() + ".png";
+    }
+    else
+    {
+        img_path = this->save_path + name + ".png";
+    }
+    return cv::imwrite(img_path.c_str(), frame);
+}
+inline std::string GimbalBase::time_string_now()
+{
+    time_t t = std::time(NULL);
+    tm *local = localtime(&t);
+    char buf[64];
+    std::strftime(buf, 64, "%Y-%m-%d_%H:%M:%S", local);
+    return buf;
+}
+#endif

src/gimbal_server.cpp

@@ -0,0 +1,458 @@
+#include <ros/ros.h>
+#include <string>
+#include <thread>
+#include <std_msgs/String.h>
+#include <std_srvs/SetBool.h>
+#include "gimbal.h"
+#include "ViewLink.h"
+#include "amov_gimbal.h"
+#include "prometheus_gimbal_control/GimbalControl.h"
+#include "prometheus_gimbal_control/GimbalState.h"
+#include "prometheus_gimbal_control/VisionDiff.h"
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+// amovGimbal::gimbal *gimbal = nullptr;
+// 定义一个全局的GimbalBase指针，用于后续对云台的控制
+GimbalBase *g_gimbal = nullptr;
+
+// 定义一个全局的GimbalState对象，可能用于发布云台的状态信息，但在这段代码中并未使用
+prometheus_gimbal_control::GimbalState g_pub_info;
+
+// 定义一个全局布尔变量，用于控制是否停止追踪（可能是某种目标追踪功能）
+bool g_stop_track = false;
+
+// 回调函数，当接收到GimbalControl消息时触发
+void ctlCallback(const prometheus_gimbal_control::GimbalControl::ConstPtr &ctl)
+{
+    // 根据GimbalControl消息中的rpyMode字段判断并执行相应的云台控制操作
+    switch (ctl->rpyMode)
+    {
+    case prometheus_gimbal_control::GimbalControl::manual:
+        // 定义两个数组，一个用于存储角度控制（roll, yaw, pitch），一个用于存储速度值
+        uint8_t rpy_ctl[3];
+        double rpy_vel[3];
+
+        // 从GimbalControl消息中提取roll, yaw, pitch的角度值
+        rpy_ctl[0] = ctl->roll;
+        rpy_ctl[1] = ctl->yaw;
+        rpy_ctl[2] = ctl->pitch;
+
+        // 从GimbalControl消息中提取rValue, pValue, yValue的速度值
+        rpy_vel[0] = ctl->rValue;
+        rpy_vel[1] = ctl->pValue;
+        rpy_vel[2] = ctl->yValue;
+
+        // 调用g_gimbal对象的ctl_gimbal方法，传入角度和速度值来控制云台
+        g_gimbal->ctl_gimbal(rpy_ctl, rpy_vel);
+
+        // 打印日志，显示当前处于手动模式，并显示角度和速度值
+        ROS_INFO_STREAM("Manual "
+                        << "rpy control mode: " << (int)rpy_ctl[0] << " " << (int)rpy_ctl[1] << " " << (int)rpy_ctl[2]
+                        << " value: " << rpy_vel[0] << " " << rpy_vel[1] << " " << rpy_vel[2]);
+
+        break;
+    case prometheus_gimbal_control::GimbalControl::home:
+        g_gimbal->home();
+        ROS_INFO_STREAM("Back Home");
+        break;
+    case prometheus_gimbal_control::GimbalControl::hold:
+        g_gimbal->hold();
+        ROS_INFO_STREAM("Hold");
+        break;
+    case prometheus_gimbal_control::GimbalControl::fellow:
+        g_gimbal->fellow();
+        ROS_INFO_STREAM("Fellow");
+        break;
+    }
+    switch (ctl->focusMode)
+    {
+    case prometheus_gimbal_control::GimbalControl::focusIn:
+        g_gimbal->focus_in();
+        ROS_INFO_STREAM("Focus In");
+        break;
+    case prometheus_gimbal_control::GimbalControl::focusOut:
+        g_gimbal->focus_out();
+        ROS_INFO_STREAM("Focus Out");
+        break;
+    case prometheus_gimbal_control::GimbalControl::focusStop:
+        g_gimbal->focus_stop();
+        ROS_INFO_STREAM("Focus Stop");
+        break;
+    }
+    switch (ctl->zoomMode)
+    {
+    case prometheus_gimbal_control::GimbalControl::zoomIn:
+        g_gimbal->zoom_in();
+        ROS_INFO_STREAM("Zoom In");
+        break;
+
+    case prometheus_gimbal_control::GimbalControl::zoomOut:
+        g_gimbal->zoom_out();
+        ROS_INFO_STREAM("Zoom Out");
+        break;
+    case prometheus_gimbal_control::GimbalControl::zoomStop:
+        g_gimbal->zoom_stop();
+        ROS_INFO_STREAM("Zoom Stop");
+        break;
+    }
+};
+
+// 这个回调函数用于处理视觉差分信息，并根据这些信息控制云台追踪目标。
+void trackCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &cent)
+{
+    // 如果已经设置了停止追踪的标志，则直接返回，不执行追踪。
+    if (g_stop_track)
+        return;
+
+    // 定义一个临时数组，用于存储目标中心点的坐标偏移量。
+    float tmp_cent[2]{0, 0};
+    // 计算目标中心点在图像中的x坐标偏移量。
+    tmp_cent[0] = cent->objectX + cent->objectWidth / 2 - cent->frameWidth / 2;
+    // 计算目标中心点在图像中的y坐标偏移量。
+    tmp_cent[1] = cent->objectY + cent->objectHeight / 2 - cent->frameHeight / 2;
+    // 获取云台的滚动模式。
+    int roll_mode = cent->ctlMode;
+    // 获取PID控制参数：比例系数kp。
+    float kp = cent->kp;
+    // 获取PID控制参数：积分系数ki。
+    float ki = cent->ki;
+    // 获取PID控制参数：微分系数kd。
+    float kd = cent->kd;
+
+    // 定义静态变量，用于记录是否进行了变焦操作。
+    static bool flag_zoom = false;
+    // 定义静态变量，用于记录是否进行了保持操作。
+    static bool flag_hold = false;
+
+    // 如果检测到目标。
+    if (cent->detect != 0)
+    {
+        // 根据当前大小和最大变焦倍数计算变焦值。
+        int zoom_val = cent->currSize ? std::fmax(g_gimbal->zoom_val, 1) : 1;
+        // 根据变焦值计算缩放比例。
+        float scale = std::fmax(g_gimbal->max_zoom - zoom_val, 1.) / g_gimbal->max_zoom;
+        // 使用计算出的参数调用云台追踪函数。
+        g_gimbal->tracking(tmp_cent, roll_mode, kp * scale, ki, kd, cent->trackIgnoreError * zoom_val / g_gimbal->max_zoom);
+        // 如果设置了自动变焦，并且目标在可忽略误差范围内，则进行自动变焦。
+        if (cent->autoZoom && cent->currSize && tmp_cent[0] < cent->trackIgnoreError && tmp_cent[1] < cent->trackIgnoreError)
+        {
+            g_gimbal->auto_zoom(cent->currSize, zoom_val, cent->maxSize, cent->minSize);
+            flag_zoom = true;
+        }
+        // 设置保持标志为真。
+        flag_hold = true;
+    }
+    else
+    {
+        // 如果检测不到目标，并且之前进行了变焦操作，则停止变焦。
+        if (cent->currSize && flag_zoom)
+        {
+            g_gimbal->zoom_stop();
+            flag_zoom = false;
+        }
+        // 如果之前进行了保持操作，则调用云台保持函数。
+        if (flag_hold)
+        {
+            g_gimbal->hold();
+            flag_hold = false;
+        }
+    }
+}
+
+void currSizeCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &info)
+{
+    static bool flag = false;
+    int now_flag = 0;
+    int zoom_val = std::fmax(g_gimbal->zoom_val, 1);
+    if (info->detect != 0 && (std::abs(g_pub_info.imuAngleVel[0]) + std::abs(g_pub_info.imuAngleVel[1]) + std::abs(g_pub_info.imuAngleVel[2])) < 3)
+    {
+        g_gimbal->auto_zoom(info->currSize, zoom_val, info->maxSize, info->minSize);
+        flag = true;
+    }
+    else
+    {
+        if (flag)
+        {
+            g_gimbal->zoom_stop();
+            flag = false;
+        }
+    }
+}
+
+// 这个函数是一个服务回调函数，用于处理视频录制的开始和停止请求。
+bool videoRecord(std_srvs::SetBool::Request &ref, std_srvs::SetBool::Response &req)
+{
+    // 如果请求的内容是开始录制视频。
+    if (ref.data)
+    {
+        // 调用云台对象的开始录制视频函数。
+        g_gimbal->start_record();
+        // 设置响应的成功标志为视频流是否已成功打开。
+        req.success = g_gimbal->outvideo.isOpened();
+        // 打印信息到ROS日志。
+        ROS_INFO_STREAM("Start Video Record ! ");
+    }
+    else
+    {
+        // 如果请求的内容是停止录制视频。
+        // 如果视频流没有打开，则直接返回失败响应。
+        if (!g_gimbal->outvideo.isOpened())
+        {
+            req.success = false;
+            req.message = "video record not is started ! ";
+            ROS_WARN_STREAM("video record not is started ! ");
+        }
+        // 调用云台对象的完成录制视频函数。
+        g_gimbal->finish_record();
+        // 如果视频流仍然打开，说明停止录制失败。
+        if (g_gimbal->outvideo.isOpened())
+        {
+            req.success = false;
+            req.message = "finish video fail ! ";
+            ROS_WARN_STREAM("finish video fail ! ");
+        }
+        else
+        {
+            // 如果视频流已关闭，说明停止录制成功。
+            req.success = true;
+            ROS_INFO_STREAM("Finish Video Record ! ");
+        }
+    }
+    // 返回true表示处理成功。
+    return true;
+}
+
+
+void picCallback(const std_msgs::String::ConstPtr &info)
+{
+    ROS_INFO("Get Picture ");
+    g_gimbal->get_picture();
+}
+
+// 这个函数用于连续从云台获取图像帧，并通过图像发布器发布。
+void callAll(image_transport::Publisher *image_pub)
+{
+    cv::Mat frame;
+    while (true)
+    {
+        // 从云台获取一帧图像。
+        (*g_gimbal) >> frame;
+        // 如果帧不为空，则将其发布。
+        if (!frame.empty())
+        {
+            image_pub->publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg());
+        }
+        // 等待10毫秒。
+        std::this_thread::sleep_for(10ms);
+    }
+}
+
+// 这个服务回调函数用于启动或停止搜索模式。
+bool search(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
+{
+    // 设置响应的成功标志为云台搜索操作的结果。
+    resp.success = g_gimbal->search(req.data);
+    return true;
+}
+
+// 这个服务回调函数用于切换跟踪模式，以获取真实的角速度或计算出的角速度。
+bool switchTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
+{
+    // 将云台的获取真实角速度标志设置为请求中的数据。
+    static_cast<GimbalQ10f *>(g_gimbal)->isGetRealVelocity = req.data;
+    resp.success = true;
+    // 设置响应消息，表明是返回真实角速度还是计算角速度。
+    resp.message = static_cast<GimbalQ10f *>(g_gimbal)->isGetRealVelocity ? "return real angle speed" : "return compute angle speed";
+    return true;
+}
+
+// 这个服务回调函数用于停止跟踪控制。
+bool stopTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
+{
+    // 设置全局停止跟踪标志为请求中的数据。
+    g_stop_track = req.data;
+    resp.success = true;
+    return true;
+}
+
+// 这个函数是设备状态回调函数，用于处理设备的不同状态信息。
+int VLK_DevStatusCallback(int iType, const char* szBuffer, int iBufLen, void* pUserParam)
+{
+    // 根据状态类型处理不同的信息。
+    if (VLK_DEV_STATUS_TYPE_MODEL == iType)
+    {
+        // 如果是设备模型信息，则解析并打印模型代码和名称。
+        VLK_DEV_MODEL* pModel = (VLK_DEV_MODEL*)szBuffer;
+        std::cout << "model code: " << pModel->cModelCode << ", model name: " << pModel->szModelName << std::endl;
+    }
+    else if (VLK_DEV_STATUS_TYPE_CONFIG == iType)
+    {
+        // 如果是设备配置信息，则解析并打印版本号、设备ID和序列号。
+        VLK_DEV_CONFIG* pDevConfig = (VLK_DEV_CONFIG*)szBuffer;
+        std::cout << "VersionNO: " << pDevConfig->cVersionNO << ", DeviceID: " << pDevConfig->cDeviceID << ", SerialNO: " << pDevConfig->cSerialNO << std::endl;
+    }
+    else if (VLK_DEV_STATUS_TYPE_TELEMETRY == iType)
+    {
+        // 如果是设备遥测信息，则解析并打印偏航角、俯仰角、传感器类型和变焦倍数。
+        // 为了避免干扰用户输入，注释掉了打印遥测信息的代码。
+        // VLK_DEV_TELEMETRY* pTelemetry = (VLK_DEV_TELEMETRY*)szBuffer;
+        // cout << "Yaw: " << pTelemetry->dYaw << ", Pitch: " << pTelemetry->dPitch << ", sensor type: " << pTelemetry->emSensorType << ", Zoom mag times: " << pTelemetry->sZoomMagTimes << endl;
+    }
+    else
+    {
+        // 如果是未知的状态类型，则打印错误信息。
+        std::cout << "error: unknown status type: " << iType << std::endl;
+    }
+
+    return 0;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "gimbal");
+    ros::NodeHandle n("~");
+    image_transport::ImageTransport it(n);
+    image_transport::Publisher image_pub;
+    cv::Mat frame;
+    sensor_msgs::ImagePtr img;
+    // rpy控制, 变焦, 放大
+    image_pub = it.advertise("image_raw", 1);
+    ros::Subscriber sub_ctl = n.subscribe<prometheus_gimbal_control::GimbalControl>("control", 10, ctlCallback);
+    ros::Subscriber sub_track = n.subscribe<prometheus_gimbal_control::VisionDiff>("track", 10, trackCallback);
+    ros::Subscriber save_pic = n.subscribe<std_msgs::String>("get_pic", 100, picCallback);
+    ros::Publisher pub_rpy = n.advertise<prometheus_gimbal_control::GimbalState>("state", 10);
+    ros::ServiceServer srv_search = n.advertiseService("search", search);
+    ros::ServiceServer srv_video = n.advertiseService("record_video", videoRecord);
+    ros::ServiceServer srv_track = n.advertiseService("stop_track", stopTrack);
+
+    ros::Rate rate(20);
+
+    int camera_width, camera_height, uav_id;
+    std::string camera_id;
+    std::string tty_url, save_path, gimbal_type;
+    n.param<std::string>("camera_id", camera_id, "0");
+    n.param<int>("uav_id", uav_id, 0); // 无人机的ID
+    n.param<std::string>("tty_url", tty_url, "/dev/ttyUSB0");
+    n.param<int>("camera_width", camera_width, 1920);
+    n.param<int>("camera_height", camera_height, 1080);
+    n.param<std::string>("save_path", save_path, "gimbal_video_data");
+    n.param<std::string>("gimbal_type", gimbal_type, "at10");
+
+    if (gimbal_type == "q10f")
+    {
+        g_gimbal = new GimbalQ10f(save_path);
+    }
+    else if (gimbal_type == "g1")
+    {
+        g_gimbal = new AmovG1(save_path);
+    }
+    else if (gimbal_type == "at10")
+    {
+        g_gimbal = new GimbalAT10(save_path);
+    }
+    else
+    {
+        ROS_ERROR_STREAM("NOT SUPPORT " << gimbal_type);
+        return -1;
+    }
+
+    try
+    {
+        g_gimbal->open_tty(tty_url);
+    }
+    catch (...)
+    {
+        ROS_ERROR_STREAM("Unable to open Serial Port ! ");
+        getchar();
+        return -1;
+    }
+
+    try
+    {
+        // 以60帧率打开，仅对AMOV G1有效
+        g_gimbal->open_camera(camera_id, camera_width, camera_height, 60);
+    }
+    catch (...)
+    {
+        ROS_ERROR_STREAM("Unable to open Camera ! ");
+        getchar();
+        return -1;
+    }
+
+    if (g_gimbal->device_name == "q10f" || g_gimbal->device_name == "at10")
+    {
+        static ros::ServiceServer srv_track_mode = n.advertiseService("track_mode", switchTrack);
+        static ros::Subscriber sub_currSize = n.subscribe<prometheus_gimbal_control::VisionDiff>("currSize", 10, currSizeCallback);
+        static_cast<GimbalQ10f *>(g_gimbal)->stopAuto();
+        static_cast<GimbalAT10 *>(g_gimbal)->stopAuto();
+        // initialize sdk
+        int iRet = VLK_Init();
+        if (VLK_ERROR_NO_ERROR != iRet)
+        {
+        std::cout << "VLK_Init failed, error: " << iRet << std::endl;
+        return -1;
+        }
+        // register device status callback
+        VLK_RegisterDevStatusCB(VLK_DevStatusCallback, NULL);
+        // ROS_INFO_STREAM("AT10 !");
+        // 5秒后停止 zoom_out()
+        static ros::Timer stop_zoom = n.createTimer(
+            ros::Duration(5.0), [&](const ros::TimerEvent &)
+            { g_gimbal->zoom_out(); },
+            true);
+    }
+
+    g_gimbal->home();
+    g_gimbal->zoom_out();
+    ROS_INFO_STREAM("Program Started ! ");
+    std::thread spin(callAll, &image_pub);
+    if(g_gimbal->device_name == "AT10")
+    {
+        ROS_INFO_STREAM("at10 !");
+    }else if(g_gimbal->device_name == "q10f")
+    {
+        ROS_INFO_STREAM("q10f !");
+    }else if(g_gimbal->device_name == "g1"){
+        ROS_INFO_STREAM("g1 !");
+        // print sdk version
+        std::cout << "ViewLink SDK version: " << GetSDKVersion() << std::endl;
+    }else {
+        ROS_INFO_STREAM("unknow device!!!");
+        std::cout << "device name: " << g_gimbal->device_name << std::endl;
+        std::cout << "device type: " << gimbal_type << std::endl;
+    }
+
+
+    g_pub_info.Id = uav_id;
+    while (ros::ok())
+    {
+        ros::spinOnce();
+        float info[12];
+        g_gimbal->get_info(info);
+        g_pub_info.mode = g_gimbal->move_state;
+        g_pub_info.feedbackMode = g_gimbal->feedback_state;
+        g_pub_info.isRecording = g_gimbal->is_recording;
+        g_pub_info.zoomState = g_gimbal->zoom_state;
+        g_pub_info.zoomVal = g_gimbal->fzoom_val;
+        g_pub_info.imuAngle[0] = info[0];
+        g_pub_info.imuAngle[1] = info[1];
+        g_pub_info.imuAngle[2] = info[2];
+        g_pub_info.rotorAngleTarget[0] = info[3];
+        g_pub_info.rotorAngleTarget[1] = info[4];
+        g_pub_info.rotorAngleTarget[2] = info[5];
+        g_pub_info.rotorAngle[0] = info[6];
+        g_pub_info.rotorAngle[1] = info[7];
+        g_pub_info.rotorAngle[2] = info[8];
+        g_pub_info.imuAngleVel[0] = info[9];
+        g_pub_info.imuAngleVel[1] = info[10];
+        g_pub_info.imuAngleVel[2] = info[11];
+        pub_rpy.publish(g_pub_info);
+        rate.sleep();
+        // std::cout << "run ........" << std::endl;
+    }
+    delete g_gimbal;
+}

src/kcf.cpp

@@ -0,0 +1,363 @@
+// Track Object---advanced by Wangmingqiang -----------------------------------------
+// 2024.6.01 at Hunan Changsha.
+
+#include <math.h>
+#include <string>
+#include <vector>
+#include <iostream>
+#include <pthread.h>
+#include <thread>
+#include <chrono>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/shared_mutex.hpp>
+
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Pose2D.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/ml.hpp>
+#include <std_srvs/SetBool.h>
+
+#include "kcftracker.hpp"
+
+#include "prometheus_gimbal_control/VisionDiff.h"
+#include "gimbal_track/WindowPosition.h"
+
+using namespace std;
+using namespace cv;
+
+#define MARKER_SIZE 0.18
+#define F1 300
+#define F2 300
+#define C1 320
+#define C2 240
+
+static const std::string RGB_WINDOW = "RGB Image window";
+
+//! Camera related parameters.
+int frameWidth_;
+int frameHeight_;
+
+float get_ros_time(ros::Time begin); //获取ros当前时间
+
+std_msgs::Header imageHeader_;
+cv::Mat camImageCopy_;
+boost::shared_mutex mutexImageCallback_;
+bool imageStatus_ = false;
+boost::shared_mutex mutexImageStatus_;
+
+/*在接收到ROS图像话题消息时被调用。这个函数用于处理从摄像头或其他图像源接收到的图像数据，并将其转换为OpenCV格式。*/
+
+// 图像话题回调函数，用于处理接收到的图像消息
+void cameraCallback(const sensor_msgs::ImageConstPtr &msg)
+{
+    ROS_DEBUG("[EllipseDetector] USB image received."); // 打印调试信息，表示收到了图像消息
+
+    cv_bridge::CvImagePtr cam_image; // 定义一个OpenCV图像指针
+
+    // 尝试将ROS图像消息转换为OpenCV图像
+    try
+    {
+        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); // 转换为BGR8格式的OpenCV图像
+        imageHeader_ = msg->header; // 保存图像消息的头部信息
+    }
+    catch (cv_bridge::Exception &e) // 捕获cv_bridge的异常
+    {
+        ROS_ERROR("cv_bridge exception: %s", e.what()); // 打印错误信息
+        return; // 结束函数调用
+    }
+
+    // 如果图像转换成功
+    if (cam_image)
+    {
+        // 使用互斥锁来保护图像数据的复制过程
+        {
+            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutexImageCallback_);
+            camImageCopy_ = cam_image->image.clone(); // 克隆图像到全局变量
+        }
+        // 使用互斥锁来更新图像状态
+        {
+            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutexImageStatus_);
+            imageStatus_ = true; // 设置图像状态为已接收
+        }
+        frameWidth_ = cam_image->image.size().width; // 保存图像宽度
+        frameHeight_ = cam_image->image.size().height; // 保存图像高度
+    }
+    return; // 结束函数调用
+}
+
+
+// 用此函数查看是否收到图像话题
+bool getImageStatus(void)
+{
+    boost::shared_lock<boost::shared_mutex> lock(mutexImageStatus_);
+    return imageStatus_;
+}
+
+//! ROS subscriber and publisher.
+image_transport::Subscriber imageSubscriber_;
+image_transport::Publisher image_vision_pub;
+ros::Publisher pose_pub;
+
+cv::Rect selectRect;
+cv::Point origin;
+cv::Rect result;
+
+bool select_flag = false;
+bool bRenewROI = false; // the flag to enable the implementation of KCF algorithm for the new chosen ROI
+bool bBeginKCF = false;
+int g_control_gimbal = 1;
+
+float get_ros_time(ros::Time begin)
+{
+    ros::Time time_now = ros::Time::now();
+    float currTimeSec = time_now.sec - begin.sec;
+    float currTimenSec = time_now.nsec / 1e9 - begin.nsec / 1e9;
+    return (currTimeSec + currTimenSec);
+}
+
+void bboxDrawCb(const gimbal_track::WindowPosition::ConstPtr &msg)
+{
+    if (msg->mode != 0)
+    {
+        selectRect.x = msg->origin_x;
+        selectRect.y = msg->origin_y;
+        selectRect.width = msg->width;
+        selectRect.height = msg->height;
+        selectRect &= cv::Rect(0, 0, frameWidth_, frameHeight_);
+        if (selectRect.width * selectRect.height > 64)
+        {
+            bRenewROI = true;
+        }
+        g_control_gimbal = 1;
+    }
+    else
+    {
+        g_control_gimbal = 0;
+    }
+}
+
+/*在OpenCV中用于处理鼠标事件。这个函数在用户与图像窗口交互时被调用，
+允许用户通过鼠标选择一个区域（ROI）来进行跟踪*/
+// 鼠标事件回调函数
+void onMouse(int event, int x, int y, int, void *)
+{
+    // 如果正在选择区域
+    if (select_flag)
+    {
+        // 更新选择区域的坐标和大小
+        selectRect.x = MIN(origin.x, x); // 选择区域的左上角x坐标是最小值
+        selectRect.y = MIN(origin.y, y); // 选择区域的左上角y坐标是最小值
+        selectRect.width = abs(x - origin.x); // 选择区域的宽度是鼠标移动的水平距离
+        selectRect.height = abs(y - origin.y); // 选择区域的高度是鼠标移动的垂直距离
+        // 限制选择区域在图像范围内
+        selectRect &= cv::Rect(0, 0, frameWidth_, frameHeight_);
+    }
+    // 如果是鼠标左键按下事件
+    if (event == CV_EVENT_LBUTTONDOWN)
+    {
+        bBeginKCF = false; // 标志位，表示不开始KCF跟踪
+        select_flag = true; // 标志位，表示开始选择区域
+        origin = cv::Point(x, y); // 记录鼠标按下的起始点
+        selectRect = cv::Rect(x, y, 0, 0); // 初始化选择区域
+    }
+    // 如果是鼠标左键释放事件
+    else if (event == CV_EVENT_LBUTTONUP)
+    {
+        // 如果选择区域的面积小于64（可能是一个无效的选择）
+        if (selectRect.width * selectRect.height < 64)
+        {
+            // 不执行任何操作
+        }
+        else
+        {
+            select_flag = false; // 标志位，表示结束选择区域
+            bRenewROI = true; // 标志位，表示需要更新ROI
+        }
+    }
+}
+
+/*定义了一个ROS服务回调函数gimbalSer，它用于处理一个名为/detection/gimbal_control的服务请求。
+服务的目的是控制云台（gimbal）的行为，根据请求的内容来决定是否开启或关闭云台的控制。*/
+
+// 服务回调函数，用于处理云台控制服务请求
+bool gimbalSer(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
+{
+    // 如果请求的数据为真（通常表示关闭云台控制）
+    if (req.data)
+    {
+        g_control_gimbal = 0; //// 设置全局变量，表示关闭云台控制
+    }
+    // 如果请求的数据为假（通常表示开启云台控制）
+    else if (selectRect.width * selectRect.height > 0)
+    {
+        bRenewROI = true; // 设置标志，表示需要更新跟踪的区域（ROI）
+        g_control_gimbal = 1; // 设置全局变量，表示开启云台控制
+    }
+    else
+    {
+        bRenewROI = false; // 设置标志，表示不需要更新跟踪的区域
+        bBeginKCF = false; // 设置标志，表示不需要开始KCF跟踪
+    }
+    // 设置响应的成功标志为真
+    resp.success = true;
+    // 根据请求的数据设置响应的消息
+    resp.message = req.data ? "Gimbal Control Close" : "Gimbal Control Open";
+    // 返回真，表示服务处理成功
+    return true;
+}
+
+bool HOG = true;
+bool FIXEDWINDOW = false;
+bool MULTISCALE = true;
+bool SILENT = true;
+bool LAB = false;
+
+// Create KCFTracker object
+KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB);
+
+int main(int argc, char **argv)
+{
+
+    ros::init(argc, argv, "tracker_ros");  // // 初始化ROS节点
+    ros::NodeHandle nh("~");   // 创建节点句柄
+    image_transport::ImageTransport it(nh); // 图像传输对象
+    ros::Rate loop_rate(30);  // 定义循环频率
+    bool auto_zoom, show_ui;
+    float max_size, min_size;
+    nh.param<bool>("auto_zoom", auto_zoom, false);  // 从参数服务器获取参数
+    nh.param<bool>("show_ui", show_ui, true);
+    nh.param<float>("max_size", max_size, 0.0);
+    nh.param<float>("min_size", min_size, 0.0);
+    std::cout << "auto_zoom: " << auto_zoom << " "
+              << "max_size: " << max_size << " "
+              << "min_size: " << min_size << std::endl;
+
+    // 接收图像的话题
+    imageSubscriber_ = it.subscribe("/gimbal/image_raw", 1, cameraCallback);
+    // 发送绘制图像
+    image_vision_pub = it.advertise("/detection/image", 1);
+
+    // diff
+    ros::Publisher position_diff_pub = nh.advertise<prometheus_gimbal_control::VisionDiff>("/gimbal/track", 10);
+    // ros::Publisher auto_zoom_pub = nh.advertise<prometheus_gimbal_control::Diff>("/gimbal_server/auto_zoom", 10);
+    ros::Subscriber sub_bbox_draw = nh.subscribe("/detection/bbox_draw", 10, bboxDrawCb);
+    ros::ServiceServer server = nh.advertiseService("/detection/gimbal_control", gimbalSer);
+
+    sensor_msgs::ImagePtr msg_ellipse;
+
+    const auto wait_duration = std::chrono::milliseconds(2000);
+    if (show_ui)
+    {
+        cv::namedWindow(RGB_WINDOW);
+        cv::setMouseCallback(RGB_WINDOW, onMouse, 0);
+    }
+
+    float cur_time;
+    float last_time;
+    float last_error_x, last_error_y;
+    float dt;
+
+    prometheus_gimbal_control::VisionDiff error_pixels;
+    ros::Time begin_time = ros::Time::now();
+
+while (ros::ok()) // 循环，只要ROS节点正常运行就继续执行
+{
+    // 获取当前ROS时间与开始时间的差值
+    cur_time = get_ros_time(begin_time);
+    dt = (cur_time - last_time); // 计算时间差
+    if (dt > 1.0 || dt < 0.0) // 如果时间差大于1秒或小于0秒，则设置为0.05秒
+    {
+        dt = 0.05;
+    }
+    // 等待直到接收到图像
+    while (!getImageStatus())
+    {
+        printf("Waiting for image.\n"); // 打印等待图像的消息
+        std::this_thread::sleep_for(wait_duration); // 休眠一段时间
+        ros::spinOnce(); // 更新ROS事件循环
+    }
+
+    Mat frame; // 定义一个OpenCV图像矩阵
+    {
+        boost::unique_lock<boost::shared_mutex> lockImageCallback(mutexImageCallback_);
+        frame = camImageCopy_.clone(); // 克隆图像到局部变量
+    }
+    if (bRenewROI) // 如果需要更新ROI
+    {
+        tracker.init(selectRect, frame); // 初始化跟踪器
+        cv::rectangle(frame, selectRect, cv::Scalar(255, 0, 0), 2, 8, 0); // 在图像上绘制ROI
+        bRenewROI = false; // 重置更新ROI的标志
+        bBeginKCF = true; // 开始KCF跟踪
+    }
+    else if (bBeginKCF) // 如果已经开始KCF跟踪
+    {
+        result = tracker.update(frame); // 更新跟踪结果
+        error_pixels.detect = 1; // 设置检测到的标志
+
+        error_pixels.objectX = result.x; // 对象在图像中的位置
+        error_pixels.objectY = result.y;
+        error_pixels.objectWidth = result.width;
+        error_pixels.objectHeight = result.height;
+
+        error_pixels.frameWidth = frameWidth_; // 图像的宽度
+        error_pixels.frameHeight = frameHeight_; // 图像的高度
+
+        error_pixels.currSize = (float)result.width * (float)result.height / (frameHeight_ * frameWidth_); // 当前对象的大小
+        error_pixels.maxSize = (float)selectRect.width * (float)selectRect.height / (frameHeight_ * frameWidth_); // 最大对象大小
+
+        cv::rectangle(frame, result, cv::Scalar(255, 0, 0), 2, 8, 0); // 在图像上绘制跟踪结果
+    }
+    else // 如果未开始KCF跟踪
+    {
+        error_pixels.detect = 0; // 设置未检测到的标志
+    }
+    error_pixels.kp = 0.2; // 比例常数
+    error_pixels.ki = 0.0001; // 积分常数
+    error_pixels.kd = 0.003; // 微分常数
+    if (max_size != 0 && min_size != 0 && auto_zoom) // 如果自动缩放被启用
+    {
+        error_pixels.maxSize = max_size; // 设置最大对象大小
+        error_pixels.minSize = min_size; // 设置最小对象大小
+    }
+    error_pixels.autoZoom = auto_zoom; // 设置自动缩放标志
+    error_pixels.trackIgnoreError = 35; // 设置跟踪忽略误差
+    if (g_control_gimbal == 0) // 如果云台控制关闭
+    {
+        error_pixels.detect = 0; // 设置检测到的标志为0
+    }
+    position_diff_pub.publish(error_pixels); // 发布位置差异信息
+        // auto_zoom_pub.publish(error_pixels);
+
+        float left_point = frame.cols / 2 - 20;
+        float right_point = frame.cols / 2 + 20;
+        float up_point = frame.rows / 2 + 20;
+        float down_point = frame.rows / 2 - 20;
+        // draw
+        line(frame, Point(left_point, frame.rows / 2), Point(right_point, frame.rows / 2), Scalar(0, 255, 0), 1, 8);
+        line(frame, Point(frame.cols / 2, down_point), Point(frame.cols / 2, up_point), Scalar(0, 255, 0), 1, 8);
+        putText(frame, "x:", Point(50, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 3, 8);
+        putText(frame, "y:", Point(50, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 3, 8);
+
+        // draw
+        char s[20] = "";
+        sprintf(s, "%.2f", float(result.x + result.width / 2 - frame.cols / 2));
+        putText(frame, s, Point(100, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 2, 8);
+        sprintf(s, "%.2f", float(result.y + result.height / 2 - frame.rows / 2));
+        putText(frame, s, Point(100, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 2, 8);
+
+        if (show_ui)
+        {
+            imshow(RGB_WINDOW, frame);
+            waitKey(20);
+        }
+
+        image_vision_pub.publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg());
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+}

src/kcftracker.cpp

@@ -1,84 +1,4 @@
-/*
 
-Tracker based on Kernelized Correlation Filter (KCF) [1] and Circulant Structure with Kernels (CSK) [2].
-CSK is implemented by using raw gray level features, since it is a single-channel filter.
-KCF is implemented by using HOG features (the default), since it extends CSK to multiple channels.
-
-[1] J. F. Henriques, R. Caseiro, P. Martins, J. Batista,
-"High-Speed Tracking with Kernelized Correlation Filters", TPAMI 2015.
-
-[2] J. F. Henriques, R. Caseiro, P. Martins, J. Batista,
-"Exploiting the Circulant Structure of Tracking-by-detection with Kernels", ECCV 2012.
-
-Authors: Joao Faro, Christian Bailer, Joao F. Henriques
-Contacts: joaopfaro@gmail.com, Christian.Bailer@dfki.de, henriques@isr.uc.pt
-Institute of Systems and Robotics - University of Coimbra / Department Augmented Vision DFKI
-
-
-Constructor parameters, all boolean:
-    hog: use HOG features (default), otherwise use raw pixels
-    fixed_window: fix window size (default), otherwise use ROI size (slower but more accurate)
-    multiscale: use multi-scale tracking (default; cannot be used with fixed_window = true)
-
-Default values are set for all properties of the tracker depending on the above choices.
-Their values can be customized further before calling init():
-    interp_factor: linear interpolation factor for adaptation
-    sigma: gaussian kernel bandwidth
-    lambda: regularization
-    cell_size: HOG cell size
-    padding: area surrounding the target, relative to its size
-    output_sigma_factor: bandwidth of gaussian target
-    template_size: template size in pixels, 0 to use ROI size
-    scale_step: scale step for multi-scale estimation, 1 to disable it
-    scale_weight: to downweight detection scores of other scales for added stability
-
-For speed, the value (template_size/cell_size) should be a power of 2 or a product of small prime numbers.
-
-Inputs to init():
-   image is the initial frame.
-   roi is a cv::Rect with the target positions in the initial frame
-
-Inputs to update():
-   image is the current frame.
-
-Outputs of update():
-   cv::Rect with target positions for the current frame
-
-
-By downloading, copying, installing or using the software you agree to this license.
-If you do not agree to this license, do not download, install,
-copy or use the software.
-
-
-                          License Agreement
-               For Open Source Computer Vision Library
-                       (3-clause BSD License)
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-  * Redistributions of source code must retain the above copyright notice,
-    this list of conditions and the following disclaimer.
-
-  * Redistributions in binary form must reproduce the above copyright notice,
-    this list of conditions and the following disclaimer in the documentation
-    and/or other materials provided with the distribution.
-
-  * Neither the names of the copyright holders nor the names of the contributors
-    may be used to endorse or promote products derived from this software
-    without specific prior written permission.
-
-This software is provided by the copyright holders and contributors "as is" and
-any express or implied warranties, including, but not limited to, the implied
-warranties of merchantability and fitness for a particular purpose are disclaimed.
-In no event shall copyright holders or contributors be liable for any direct,
-indirect, incidental, special, exemplary, or consequential damages
-(including, but not limited to, procurement of substitute goods or services;
-loss of use, data, or profits; or business interruption) however caused
-and on any theory of liability, whether in contract, strict liability,
-or tort (including negligence or otherwise) arising in any way out of
-the use of this software, even if advised of the possibility of such damage.
- */
 #include <iostream>
 #ifndef _KCFTRACKER_HEADERS
 #include "kcftracker.hpp"
@@ -88,75 +8,74 @@ the use of this software, even if advised of the possibility of such damage.
 #include "labdata.hpp"
 #endif
 
-// Constructor
+// KCFTracker类的构造函数，用于初始化KCF跟踪器的参数
 KCFTracker::KCFTracker(bool hog, bool fixed_window, bool multiscale, bool lab)
 {
+    // 所有情况下参数都相同
+    lambda = 0.0001; // 权重参数
+    padding = 2.5; // 边界填充参数
+    output_sigma_factor = 0.125; // 输出高斯函数的标准差缩放因子
 
-    // Parameters equal in all cases
-    lambda = 0.0001;
-    padding = 2.5; 
-    //output_sigma_factor = 0.1;
-    output_sigma_factor = 0.125;
-
-
-    if (hog) {    // HOG
+    // 根据是否使用HOG特征来设置参数
+    if (hog) { // 如果使用HOG特征
         // VOT
-        interp_factor = 0.012;
-        sigma = 0.6; 
-        // TPAMI
-        //interp_factor = 0.02;
-        //sigma = 0.5; 
-        cell_size = 4;
-        _hogfeatures = true;
+        interp_factor = 0.012; // 插值因子
+        sigma = 0.6; // 高斯核的标准差
+
+        cell_size = 4; // 大小
+        _hogfeatures = true; // 设置HOG特征标志为真
 
+        // 如果使用LAB特征
         if (lab) {
-            interp_factor = 0.005;
-            sigma = 0.4; 
+            interp_factor = 0.005; // 插值因子
+            sigma = 0.4; // 高斯核的标准差
             //output_sigma_factor = 0.025;
-            output_sigma_factor = 0.1;
+            output_sigma_factor = 0.1; // 输出高斯函数的标准差缩放因子
 
-            _labfeatures = true;
-            _labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data);
-            cell_sizeQ = cell_size*cell_size;
+            _labfeatures = true; // 设置LAB特征标志为真
+            _labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data); // 初始化LAB特征的聚类中心
+            cell_sizeQ = cell_size * cell_size; // 计算细胞大小的平方
         }
-        else{
-            _labfeatures = false;
+        else {
+            _labfeatures = false; // 设置LAB特征标志为假
         }
     }
-    else {   // RAW
-        interp_factor = 0.075;
-        sigma = 0.2; 
-        cell_size = 1;
-        _hogfeatures = false;
+    else { // 如果使用原始特征
+        interp_factor = 0.075; // 插值因子
+        sigma = 0.2; // 高斯核的标准差
+        cell_size = 1; // 细胞大小
+        _hogfeatures = false; // 设置HOG特征标志为假
 
+        // 如果尝试使用LAB特征，则打印错误信息
         if (lab) {
             printf("Lab features are only used with HOG features.\n");
-            _labfeatures = false;
+            _labfeatures = false; // 设置LAB特征标志为假
         }
     }
 
-
-    if (multiscale) { // multiscale
-        template_size = 96;
+    // 根据是否使用多尺度来设置参数
+    if (multiscale) { // 如果使用多尺度
+        template_size = 96; // 模板大小
         //template_size = 100;
-        scale_step = 1.05;
-        scale_weight = 0.95;
-        if (!fixed_window) {
+        scale_step = 1.05; // 缩放步长
+        scale_weight = 0.95; // 缩放权重
+        if (!fixed_window) { // 如果不是固定窗口
             //printf("Multiscale does not support non-fixed window.\n");
-            fixed_window = true;
+            fixed_window = true; // 设置固定窗口标志为真
         }
     }
-    else if (fixed_window) {  // fit correction without multiscale
-        template_size = 96;
+    else if (fixed_window) { // 如果使用固定窗口
+        template_size = 96; // 模板大小
         //template_size = 100;
-        scale_step = 1;
+        scale_step = 1; // 缩放步长
     }
-    else {
-        template_size = 1;
-        scale_step = 1;
+    else { // 如果都不使用
+        template_size = 1; // 模板大小
+        scale_step = 1; // 缩放步长
     }
 }
 
+
 // Initialize tracker 
 void KCFTracker::init(const cv::Rect &roi, cv::Mat image)
 {
@@ -172,51 +91,51 @@ void KCFTracker::init(const cv::Rect &roi, cv::Mat image)
 // Update position based on the new frame
 cv::Rect KCFTracker::update(cv::Mat image)
 {
-    if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 1;
-    if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 1;
-    if (_roi.x >= image.cols - 1) _roi.x = image.cols - 2;
-    if (_roi.y >= image.rows - 1) _roi.y = image.rows - 2;
-
-    float cx = _roi.x + _roi.width / 2.0f;
-    float cy = _roi.y + _roi.height / 2.0f;
+    if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 1; // 如果ROI的x坐标加上宽度小于0，则调整x坐标
+    if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 1; // 如果ROI的y坐标加上高度小于0，则调整y坐标
+    if (_roi.x >= image.cols - 1) _roi.x = image.cols - 2; // 如果ROI的x坐标加上宽度大于图像宽度，则调整x坐标
+    if (_roi.y >= image.rows - 1) _roi.y = image.rows - 2; // 如果ROI的y坐标加上高度大于图像高度，则调整y坐标
 
+    float cx = _roi.x + _roi.width / 2.0f; // 计算ROI的中心x坐标
+    float cy = _roi.y + _roi.height / 2.0f; // 计算ROI的中心y坐标
 
-    float peak_value;
-    cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value);
+    float peak_value; // 峰值值
+    cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value); // 使用检测函数和模板进行检测
 
-    if (scale_step != 1) {
-        // Test at a smaller _scale
+    if (scale_step != 1) { // 如果缩放步长不是1，则尝试在更小的缩放级别上进行检测
+        // 在更小的缩放级别上进行测试
         float new_peak_value;
         cv::Point2f new_res = detect(_tmpl, getFeatures(image, 0, 1.0f / scale_step), new_peak_value);
 
-        if (scale_weight * new_peak_value > peak_value) {
+        if (scale_weight * new_peak_value > peak_value) { // 如果新峰值值更大，则更新结果
             res = new_res;
             peak_value = new_peak_value;
-            _scale /= scale_step;
-            _roi.width /= scale_step;
-            _roi.height /= scale_step;
+            _scale /= scale_step; // 更新缩放级别
+            _roi.width /= scale_step; // 更新ROI的宽度
+            _roi.height /= scale_step; // 更新ROI的高度
         }
 
-        // Test at a bigger _scale
+        // 在更大的缩放级别上进行测试
         new_res = detect(_tmpl, getFeatures(image, 0, scale_step), new_peak_value);
 
-        if (scale_weight * new_peak_value > peak_value) {
+        if (scale_weight * new_peak_value > peak_value) { // 如果新峰值值更大，则更新结果
             res = new_res;
             peak_value = new_peak_value;
-            _scale *= scale_step;
-            _roi.width *= scale_step;
-            _roi.height *= scale_step;
+            _scale *= scale_step; // 更新缩放级别
+            _roi.width *= scale_step; // 更新ROI的宽度
+            _roi.height *= scale_step; // 更新ROI的高度
         }
     }
 
-    // Adjust by cell size and _scale
-    _roi.x = cx - _roi.width / 2.0f + ((float) res.x * cell_size * _scale);
-    _roi.y = cy - _roi.height / 2.0f + ((float) res.y * cell_size * _scale);
+    // 根据细胞大小和缩放级别调整ROI
+    _roi.x = cx - _roi.width / 2.0f + ((float) res.x * cell_size * _scale); // 调整ROI的x坐标
+    _roi.y = cy - _roi.height / 2.0f + ((float) res.y * cell_size * _scale); // 调整ROI的y坐标
+
+    if (_roi.x >= image.cols - 1) _roi.x = image.cols - 1; // 确保ROI的x坐标不超过图像宽度
+    if (_roi.y >= image.rows - 1) _roi.y = image.rows - 1; // 确保ROI的y坐标不超过图像高度
+    if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 2; // 确保ROI的x坐标加上宽度大于0
+    if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 2; // 确保ROI的y坐标加上高度大于0
 
-    if (_roi.x >= image.cols - 1) _roi.x = image.cols - 1;
-    if (_roi.y >= image.rows - 1) _roi.y = image.rows - 1;
-    if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 2;
-    if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 2;
 
     assert(_roi.width >= 0 && _roi.height >= 0);
     cv::Mat x = getFeatures(image, 0);
@@ -261,30 +180,20 @@ cv::Point2f KCFTracker::detect(cv::Mat z, cv::Mat x, float &peak_value)
     return p;
 }
 
-// train tracker with a single image
+// 使用单个图像训练跟踪器
 void KCFTracker::train(cv::Mat x, float train_interp_factor)
 {
-    using namespace FFTTools;
+    using namespace FFTTools; // 引入FFTTools命名空间，可能包含相关函数和变量
 
-    cv::Mat k = gaussianCorrelation(x, x);
-    cv::Mat alphaf = complexDivision(_prob, (fftd(k) + lambda));
+    cv::Mat k = gaussianCorrelation(x, x); // 计算x和x之间的高斯相关性
+    cv::Mat alphaf = complexDivision(_prob, (fftd(k) + lambda)); // 计算高斯核的傅里叶变换，并除以(核变换 + lambda)
 
-    _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x;
-    _alphaf = (1 - train_interp_factor) * _alphaf + (train_interp_factor) * alphaf;
-
-
-    /*cv::Mat kf = fftd(gaussianCorrelation(x, x));
-    cv::Mat num = complexMultiplication(kf, _prob);
-    cv::Mat den = complexMultiplication(kf, kf + lambda);
-    
-    _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x;
-    _num = (1 - train_interp_factor) * _num + (train_interp_factor) * num;
-    _den = (1 - train_interp_factor) * _den + (train_interp_factor) * den;
-
-    _alphaf = complexDivision(_num, _den);*/
+    _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x; // 更新模板
+    _alphaf = (1 - train_interp_factor) * _alphaf + (train_interp_factor) * alphaf; // 更新高斯核的傅里叶变换
 
 }
 
+
 // Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts between input images X and Y, which must both be MxN. They must    also be periodic (ie., pre-processed with a cosine window).
 cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2)
 {
@@ -321,27 +230,28 @@ cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2)
     return k;
 }
 
-// Create Gaussian Peak. Function called only in the first frame.
+// 在第一帧中创建高斯峰值
 cv::Mat KCFTracker::createGaussianPeak(int sizey, int sizex)
 {
-    cv::Mat_<float> res(sizey, sizex);
+    cv::Mat_<float> res(sizey, sizex); // 创建一个浮点数矩阵，大小为sizey x sizex
 
-    int syh = (sizey) / 2;
-    int sxh = (sizex) / 2;
+    int syh = (sizey) / 2; // 计算y方向的高斯峰值中心
+    int sxh = (sizex) / 2; // 计算x方向的高斯峰值中心
 
-    float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor;
-    float mult = -0.5 / (output_sigma * output_sigma);
+    float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor; // 计算输出高斯核的标准差
+    float mult = -0.5 / (output_sigma * output_sigma); // 计算高斯核的缩放因子
 
-    for (int i = 0; i < sizey; i++)
-        for (int j = 0; j < sizex; j++)
+    for (int i = 0; i < sizey; i++) // 遍历矩阵的每一行
+        for (int j = 0; j < sizex; j++) // 遍历矩阵的每一列
         {
-            int ih = i - syh;
-            int jh = j - sxh;
-            res(i, j) = std::exp(mult * (float) (ih * ih + jh * jh));
+            int ih = i - syh; // 计算当前点与y方向高斯峰值中心的距离
+            int jh = j - sxh; // 计算当前点与x方向高斯峰值中心的距离
+            res(i, j) = std::exp(mult * (float) (ih * ih + jh * jh)); // 计算高斯函数的值
         }
-    return FFTTools::fftd(res);
+    return FFTTools::fftd(res); // 返回高斯峰值的傅里叶变换
 }
 
+
 // Obtain sub-window from image, with replication-padding and extract features
 cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scale_adjust)
 {
@@ -486,21 +396,24 @@ cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scal
     return FeaturesMap;
 }
     
-// Initialize Hanning window. Function called only in the first frame.
+// 初始化汉明窗口。该函数仅在第一帧调用。
 void KCFTracker::createHanningMats()
 {
     cv::Mat hann1t = cv::Mat(cv::Size(size_patch[1],1), CV_32F, cv::Scalar(0));
     cv::Mat hann2t = cv::Mat(cv::Size(1,size_patch[0]), CV_32F, cv::Scalar(0));
 
+    // 创建汉明窗口的一维版本
     for (int i = 0; i < hann1t.cols; i++)
-        hann1t.at<float > (0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1)));
+        hann1t.at<float>(0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1)));
     for (int i = 0; i < hann2t.rows; i++)
-        hann2t.at<float > (i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1)));
+        hann2t.at<float>(i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1)));
 
+    // 将一维汉明窗口组合成二维汉明窗口
     cv::Mat hann2d = hann2t * hann1t;
-    // HOG features
+
+    // HOG特征
     if (_hogfeatures) {
-        cv::Mat hann1d = hann2d.reshape(1,1); // Procedure do deal with cv::Mat multichannel bug
+        cv::Mat hann1d = hann2d.reshape(1,1); // 解决cv::Mat多通道bug的步骤
 
         hann = cv::Mat(cv::Size(size_patch[0]*size_patch[1], size_patch[2]), CV_32F, cv::Scalar(0));
         for (int i = 0; i < size_patch[2]; i++) {
@@ -509,13 +422,13 @@ void KCFTracker::createHanningMats()
             }
         }
     }
-    // Gray features
+    // 灰度特征
     else {
         hann = hann2d;
     }
 }
 
-// Calculate sub-pixel peak for one dimension
+// 为单维度计算亚像素峰值
 float KCFTracker::subPixelPeak(float left, float center, float right)
 {
     float divisor = 2 * center - right - left;

src/serial.cpp

@@ -0,0 +1,378 @@
+#include <algorithm>
+
+#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__)
+#include <alloca.h>
+#endif
+
+#include "serial/serial.h"
+
+#include "serial/impl/unix.h"
+
+using std::invalid_argument;
+using std::min;
+using std::numeric_limits;
+using std::size_t;
+using std::string;
+using std::vector;
+
+using serial::bytesize_t;
+using serial::flowcontrol_t;
+using serial::IOException;
+using serial::parity_t;
+using serial::Serial;
+using serial::SerialException;
+using serial::stopbits_t;
+
+class Serial::ScopedReadLock {
+public:
+  // 构造函数，传入串行实现类指针，并获取读锁
+  ScopedReadLock(SerialImpl *pimpl) : pimpl_(pimpl) {
+    this->pimpl_->readLock();
+  }
+  // 析构函数，释放读锁
+  ~ScopedReadLock() { this->pimpl_->readUnlock(); }
+
+private:
+  // 禁用复制构造函数
+  ScopedReadLock(const ScopedReadLock &);
+  // 禁用赋值操作符
+  const ScopedReadLock &operator=(ScopedReadLock);
+
+  SerialImpl *pimpl_;
+};
+
+// 这是一个私有的类定义部分，可能是一个互斥锁封装类
+class ScopedWriteLock {
+private:
+  // 禁用拷贝构造函数，避免对象被误拷贝
+  // Disable copy constructors there
+  ScopedWriteLock(const ScopedWriteLock &); // 删除此函数或声明为=delete
+
+  // 禁用赋值运算符，避免对象被误赋值
+  const ScopedWriteLock &operator=(ScopedWriteLock); // 缺少引用符号，应改为const ScopedWriteLock& operator=(const ScopedWriteLock&) = delete;
+
+  // 指向实现类的指针，使用了Pimpl（Pointer to Implementation）技术
+  SerialImpl *pimpl_;
+};
+
+// Serial类定义，可能是一个串口通信类
+class Serial {
+public:
+  // 构造函数，初始化串口设置
+  Serial(const string &port, uint32_t baudrate, serial::Timeout timeout,
+         bytesize_t bytesize, parity_t parity, stopbits_t stopbits,
+         flowcontrol_t flowcontrol)
+      : pimpl_(new SerialImpl(port, baudrate, bytesize, parity, stopbits,
+                              flowcontrol)) {
+    pimpl_->setTimeout(timeout); // 设置超时时间
+  }
+
+  // 析构函数，释放资源
+  ~Serial() { delete pimpl_; }
+
+  // 打开串口
+  void open() { pimpl_->open(); }
+
+  // 关闭串口
+  void close() { pimpl_->close(); }
+
+  // 检查串口是否打开
+  bool isOpen() const { return pimpl_->isOpen(); }
+
+  // 获取串口上可用的字节数
+  size_t available() { return pimpl_->available(); }
+
+  // 等待串口可读
+  bool waitReadable() {
+    serial::Timeout timeout(pimpl_->getTimeout());
+    return pimpl_->waitReadable(timeout.read_timeout_constant);
+  }
+
+  // 等待特定数量的字节时间（可能是为了同步）
+  void waitByteTimes(size_t count) { pimpl_->waitByteTimes(count); }
+
+  // 从串口读取数据（无锁）
+  size_t read_(uint8_t *buffer, size_t size) {
+    return this->pimpl_->read(buffer, size);
+  }
+
+  // 从串口读取数据（使用ScopedReadLock加锁）
+  size_t read(uint8_t *buffer, size_t size) {
+    ScopedReadLock lock(this->pimpl_); // 假设ScopedReadLock是一个读取锁类
+    return this->pimpl_->read(buffer, size);
+  }
+
+private:
+  // 指向实现类的指针，使用了Pimpl（Pointer to Implementation）技术
+  SerialImpl *pimpl_;
+};
+
+size_t Serial::read(std::vector<uint8_t> &buffer, size_t size) {
+  // 创建一个读锁，保证读操作的安全性
+  ScopedReadLock lock(this->pimpl_);
+  // 分配一个新的缓冲区来存储读取的数据
+  uint8_t *buffer_ = new uint8_t[size];
+  // 初始化已读取的字节数为0
+  size_t bytes_read = 0;
+
+  try {
+    // 尝试从串行端口读取数据到缓冲区
+    bytes_read = this->pimpl_->read(buffer_, size);
+  } catch (const std::exception &e) {
+    // 如果读取过程中发生异常，删除缓冲区并重新抛出异常
+    delete[] buffer_;
+    throw;
+  }
+
+  // 将读取的数据插入到传入的vector中
+  buffer.insert(buffer.end(), buffer_, buffer_ + bytes_read);
+  // 删除临时缓冲区
+  delete[] buffer_;
+  // 返回实际读取的字节数
+  return bytes_read;
+}
+
+size_t Serial::read(std::string &buffer, size_t size) {
+  // 创建一个读锁，保证读操作的安全性
+  ScopedReadLock lock(this->pimpl_);
+  // 分配一个新的缓冲区来存储读取的数据
+  uint8_t *buffer_ = new uint8_t[size];
+  // 初始化已读取的字节数为0
+  size_t bytes_read = 0;
+  try {
+    // 尝试从串行端口读取数据到缓冲区
+    bytes_read = this->pimpl_->read(buffer_, size);
+  } catch (const std::exception &e) {
+    // 如果读取过程中发生异常，删除缓冲区并重新抛出异常
+    delete[] buffer_;
+    throw;
+  }
+  // 将读取的数据追加到传入的string中
+  buffer.append(reinterpret_cast<const char *>(buffer_), bytes_read);
+  // 删除临时缓冲区
+  delete[] buffer_;
+  // 返回实际读取的字节数
+  return bytes_read;
+}
+
+
+// Serial类中的成员函数，用于读取指定字节数并返回字符串
+string Serial::read(size_t size) {
+  std::string buffer; // 创建一个字符串用于存储读取的数据
+  buffer.resize(size); // 预分配足够的存储空间
+  this->read(&buffer[0], size); // 调用重载的read函数，将读取的数据填充到buffer中
+  return buffer; // 返回读取到的字符串
+}
+
+// Serial类中的成员函数，用于读取一行数据并返回字符串
+size_t Serial::readline(string &buffer, size_t size, string eol) {
+  ScopedReadLock lock(this->pimpl_); // 使用ScopedReadLock对读取操作进行加锁
+  size_t eol_len = eol.length(); // 获取行结束符的长度
+  // 使用alloca函数在栈上动态分配足够大小的字节数组来存储读取的数据
+  // 注意：alloca函数不是C++标准库的一部分，并且在某些平台上可能不被支持或存在安全问题
+  // 在实践中，更推荐使用std::vector<uint8_t>或std::array<uint8_t, N>来替代
+  uint8_t *buffer_ = static_cast<uint8_t *>(alloca(size * sizeof(uint8_t)));
+  size_t read_so_far = 0; // 记录已经读取的字节数
+  while (true) {
+    // 尝试读取一个字节
+    size_t bytes_read = this->read_(buffer_ + read_so_far, 1);
+    read_so_far += bytes_read;
+    // 如果没有读取到任何数据（可能是超时），则退出循环
+    if (bytes_read == 0) {
+      break; // 读取1字节时发生超时
+    }
+    // 如果已经读取的字节数还不足以构成一个完整的行结束符，则继续读取
+    if (read_so_far < eol_len)
+      continue;
+    // 检查当前读取到的数据是否包含行结束符
+    if (string(reinterpret_cast<const char *>(buffer_ + read_so_far - eol_len),
+               eol_len) == eol) {
+      break; // 找到行结束符
+    }
+    // 如果已经读取了最大字节数，则退出循环
+    if (read_so_far == size) {
+      break; // 达到最大读取长度
+    }
+  }
+  // 将读取到的数据转换为字符串并追加到传入的buffer中
+  buffer.append(reinterpret_cast<const char *>(buffer_), read_so_far);
+  // 返回实际读取的字节数
+  return read_so_far;
+}
+
+// Serial类中的成员函数，读取一行数据并返回字符串（使用临时字符串对象）
+string Serial::readline(size_t size, string eol) {
+  std::string buffer; // 创建一个临时字符串对象
+  this->readline(buffer, size, eol); // 调用重载的readline函数
+  return buffer; // 返回读取到的字符串
+}
+
+// 中文注释：
+// Serial::read函数：
+//   读取指定字节数的数据，并将数据转换为字符串返回。
+//
+// Serial::readline函数（带引用参数）：
+//   读取一行数据，直到遇到指定的行结束符或者读取了指定数量的字节或者发生超时。
+//   读取到的数据会被追加到传入的字符串buffer中，并返回实际读取的字节数。
+//   该函数使用ScopedReadLock进行加锁操作，以确保在多线程环境中线程安全。
+//
+// Serial::readline函数（返回字符串）：
+//   与带引用参数的readline函数功能相同，但返回的是一个新创建的字符串对象。
+//   内部使用了一个临时字符串对象来存储读取到的数据。
+
+// Serial类中的成员函数，用于从串口读取多行数据并返回一个string向量
+vector<string> Serial::readlines(size_t size, string eol) {
+  ScopedReadLock lock(this->pimpl_); // 使用ScopedReadLock对读取操作进行加锁
+  std::vector<std::string> lines; // 创建一个字符串向量来存储读取到的多行数据
+  size_t eol_len = eol.length(); // 获取行结束符的长度
+
+  // 注意：虽然这里使用了alloca来分配内存，但在C++中更推荐使用std::vector或std::array
+  // 这里为了保持与原始代码的一致性，仍然使用alloca
+  uint8_t *buffer_ = static_cast<uint8_t *>(alloca(size * sizeof(uint8_t)));
+  size_t read_so_far = 0; // 记录已经读取的字节数
+  size_t start_of_line = 0; // 记录当前行的起始位置
+
+  // 循环读取数据，直到达到最大读取长度或发生超时
+  while (read_so_far < size) {
+    // 尝试读取一个字节
+    size_t bytes_read = this->read_(buffer_ + read_so_far, 1);
+    read_so_far += bytes_read;
+
+    // 如果读取失败（可能是超时）
+    if (bytes_read == 0) {
+      // 如果当前行有数据（即start_of_line不等于当前位置），则添加到lines中
+      if (start_of_line != read_so_far) {
+        lines.push_back(
+            string(reinterpret_cast<const char *>(buffer_ + start_of_line),
+                   read_so_far - start_of_line));
+      }
+      break; // 读取1字节时发生超时，退出循环
+    }
+
+    // 如果当前读取的字节数还不足以构成一个完整的行结束符，则继续读取
+    if (read_so_far < eol_len)
+      continue;
+
+    // 检查当前读取到的数据是否包含行结束符
+    if (string(reinterpret_cast<const char *>(buffer_ + read_so_far - eol_len),
+               eol_len) == eol) {
+      // 发现行结束符，将当前行添加到lines中
+      lines.push_back(
+          string(reinterpret_cast<const char *>(buffer_ + start_of_line),
+                 read_so_far - eol_len));
+      // 更新当前行的起始位置为行结束符之后
+      start_of_line = read_so_far;
+    }
+
+    // 如果已经读取了最大字节数，则退出循环
+    if (read_so_far == size) {
+      // 如果当前行有数据（即start_of_line不等于当前位置），则添加到lines中
+      if (start_of_line != read_so_far) {
+        lines.push_back(
+            string(reinterpret_cast<const char *>(buffer_ + start_of_line),
+                   read_so_far - start_of_line));
+      }
+      break; // 达到最大读取长度，退出循环
+    }
+  }
+
+  // 返回读取到的多行数据
+  return lines;
+}
+
+size_t Serial::write(const string &data) {
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_(reinterpret_cast<const uint8_t *>(data.c_str()),
+                      data.length());
+}
+
+size_t Serial::write(const std::vector<uint8_t> &data) {
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_(&data[0], data.size());
+}
+
+size_t Serial::write(const uint8_t *data, size_t size) {
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_(data, size);
+}
+
+size_t Serial::write_(const uint8_t *data, size_t length) {
+  return pimpl_->write(data, length);
+}
+
+void Serial::setPort(const string &port) {
+  ScopedReadLock rlock(this->pimpl_);
+  ScopedWriteLock wlock(this->pimpl_);
+  bool was_open = pimpl_->isOpen();
+  if (was_open)
+    close();
+  pimpl_->setPort(port);
+  if (was_open)
+    open();
+}
+
+string Serial::getPort() const { return pimpl_->getPort(); }
+
+void Serial::setTimeout(serial::Timeout &timeout) {
+  pimpl_->setTimeout(timeout);
+}
+
+serial::Timeout Serial::getTimeout() const { return pimpl_->getTimeout(); }
+
+void Serial::setBaudrate(uint32_t baudrate) { pimpl_->setBaudrate(baudrate); }
+
+uint32_t Serial::getBaudrate() const { return uint32_t(pimpl_->getBaudrate()); }
+
+void Serial::setBytesize(bytesize_t bytesize) { pimpl_->setBytesize(bytesize); }
+
+bytesize_t Serial::getBytesize() const { return pimpl_->getBytesize(); }
+
+void Serial::setParity(parity_t parity) { pimpl_->setParity(parity); }
+
+parity_t Serial::getParity() const { return pimpl_->getParity(); }
+
+void Serial::setStopbits(stopbits_t stopbits) { pimpl_->setStopbits(stopbits); }
+
+stopbits_t Serial::getStopbits() const { return pimpl_->getStopbits(); }
+
+void Serial::setFlowcontrol(flowcontrol_t flowcontrol) {
+  pimpl_->setFlowcontrol(flowcontrol);
+}
+
+flowcontrol_t Serial::getFlowcontrol() const {
+  return pimpl_->getFlowcontrol();
+}
+
+void Serial::flush() {
+  ScopedReadLock rlock(this->pimpl_);
+  ScopedWriteLock wlock(this->pimpl_);
+  pimpl_->flush();
+}
+
+void Serial::flushInput() {
+  ScopedReadLock lock(this->pimpl_);
+  pimpl_->flushInput();
+}
+
+void Serial::flushOutput() {
+  ScopedWriteLock lock(this->pimpl_);
+  pimpl_->flushOutput();
+}
+
+void Serial::sendBreak(int duration) { pimpl_->sendBreak(duration); }
+
+void Serial::setBreak(bool level) { pimpl_->setBreak(level); }
+
+void Serial::setRTS(bool level) { pimpl_->setRTS(level); }
+
+void Serial::setDTR(bool level) { pimpl_->setDTR(level); }
+
+bool Serial::waitForChange() { return pimpl_->waitForChange(); }
+
+bool Serial::getCTS() { return pimpl_->getCTS(); }
+
+bool Serial::getDSR() { return pimpl_->getDSR(); }
+
+bool Serial::getRI() { return pimpl_->getRI(); }
+
+bool Serial::getCD() { return pimpl_->getCD(); }