Add zoom and focus control to camera controller.

Add "known" parameters to camera controller.

src/Camera/QGCCameraControl.cc

@@ -58,6 +58,15 @@ static const char* kPhotoMode       = "PhotoMode";
 static const char* kPhotoLapse      = "PhotoLapse";
 static const char* kPhotoLapseCount = "PhotoLapseCount";
 
+//-----------------------------------------------------------------------------
+// Known Parameters
+static const char *kCAM_EV          = "CAM_EV";
+static const char *kCAM_EXPMODE     = "CAM_EXPMODE";
+static const char *kCAM_ISO         = "CAM_ISO";
+static const char* kCAM_SHUTTER     = "CAM_SHUTTER";
+static const char* kCAM_APERTURE    = "CAM_APERTURE";
+static const char* kCAM_WBMODE      = "CAM_WBMODE";
+
 //-----------------------------------------------------------------------------
 QGCCameraOptionExclusion::QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_)
     : QObject(parent)
@@ -146,6 +155,7 @@ QGCCameraControl::QGCCameraControl(const mavlink_camera_information_t *info, Veh
     , _compID(compID)
     , _version(0)
     , _cached(false)
+    , _paramComplete(false)
     , _storageFree(0)
     , _storageTotal(0)
     , _netManager(nullptr)
@@ -465,6 +475,44 @@ QGCCameraControl::setPhotoMode()
     }
 }
 
+//-----------------------------------------------------------------------------
+void
+QGCCameraControl::setZoomLevel(qreal level)
+{
+    qCDebug(CameraControlLog) << "setZoomLevel()" << level;
+    if(hasZoom()) {
+        //-- Limit
+        level = std::min(std::max(level, 0.0), 100.0);
+        if(_vehicle) {
+            _vehicle->sendMavCommand(
+                _compID,                                // Target component
+                MAV_CMD_SET_CAMERA_ZOOM,                // Command id
+                false,                                  // ShowError
+                ZOOM_TYPE_RANGE,                        // Zoom type
+                static_cast<float>(level));             // Level
+        }
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraControl::setFocusLevel(qreal level)
+{
+    qCDebug(CameraControlLog) << "setFocusLevel()" << level;
+    if(hasFocus()) {
+        //-- Limit
+        level = std::min(std::max(level, 0.0), 100.0);
+        if(_vehicle) {
+            _vehicle->sendMavCommand(
+                _compID,                                // Target component
+                MAV_CMD_SET_CAMERA_FOCUS,               // Command id
+                false,                                  // ShowError
+                FOCUS_TYPE_RANGE,                       // Focus type
+                static_cast<float>(level));             // Level
+        }
+    }
+}
+
 //-----------------------------------------------------------------------------
 void
 QGCCameraControl::resetSettings()
@@ -497,6 +545,51 @@ QGCCameraControl::formatCard(int id)
     }
 }
 
+//-----------------------------------------------------------------------------
+void
+QGCCameraControl::stepZoom(int direction)
+{
+    qCDebug(CameraControlLog) << "stepZoom()" << direction;
+    if(_vehicle && hasZoom()) {
+        _vehicle->sendMavCommand(
+            _compID,                                // Target component
+            MAV_CMD_SET_CAMERA_ZOOM,                // Command id
+            false,                                  // ShowError
+            ZOOM_TYPE_STEP,                         // Zoom type
+            direction);                             // Direction (-1 wide, 1 tele)
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraControl::startZoom(int direction)
+{
+    qCDebug(CameraControlLog) << "startZoom()" << direction;
+    if(_vehicle && hasZoom()) {
+        _vehicle->sendMavCommand(
+            _compID,                                // Target component
+            MAV_CMD_SET_CAMERA_ZOOM,                // Command id
+            true,                                   // ShowError
+            ZOOM_TYPE_CONTINUOUS,                   // Zoom type
+            direction);                             // Direction (-1 wide, 1 tele)
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+QGCCameraControl::stopZoom()
+{
+    qCDebug(CameraControlLog) << "stopZoom()";
+    if(_vehicle && hasZoom()) {
+        _vehicle->sendMavCommand(
+            _compID,                                // Target component
+            MAV_CMD_SET_CAMERA_ZOOM,                // Command id
+            true,                                   // ShowError
+            ZOOM_TYPE_CONTINUOUS,                   // Zoom type
+            0);                                     // Direction (-1 wide, 1 tele)
+    }
+}
+
 //-----------------------------------------------------------------------------
 void
 QGCCameraControl::_requestCaptureStatus()
@@ -1069,6 +1162,10 @@ QGCCameraControl::_updateActiveList()
         qCDebug(CameraControlLogVerbose) << "Excluding" << exclusionList;
         _activeSettings = active;
         emit activeSettingsChanged();
+        //-- Force validity of "Facts" based on active set
+        if(_paramComplete) {
+            emit parametersReady();
+        }
     }
 }
 
@@ -1273,6 +1370,16 @@ QGCCameraControl::handleSettings(const mavlink_camera_settings_t& settings)
 {
     qCDebug(CameraControlLog) << "handleSettings() Mode:" << settings.mode_id;
     _setCameraMode(static_cast<CameraMode>(settings.mode_id));
+    qreal z = static_cast<qreal>(settings.zoomLevel);
+    qreal f = static_cast<qreal>(settings.focusLevel);
+    if(std::isfinite(z) && z != _zoomLevel) {
+        _zoomLevel = z;
+        emit zoomLevelChanged();
+    }
+    if(std::isfinite(f) && f != _focusLevel) {
+        _focusLevel = f;
+        emit focusLevelChanged();
+    }
 }
 
 //-----------------------------------------------------------------------------
@@ -1563,6 +1670,7 @@ QGCCameraControl::_paramDone()
         }
     }
     //-- All parameters loaded (or timed out)
+    _paramComplete = true;
     emit parametersReady();
 }
 
@@ -1591,3 +1699,45 @@ QGCCameraControl::activeSettings()
     qCDebug(CameraControlLog) << "Active:" << _activeSettings;
     return _activeSettings;
 }
+
+//-----------------------------------------------------------------------------
+Fact*
+QGCCameraControl::exposureMode()
+{
+    return (_paramComplete && _activeSettings.contains(kCAM_EXPMODE)) ? getFact(kCAM_EXPMODE) : nullptr;
+}
+
+//-----------------------------------------------------------------------------
+Fact*
+QGCCameraControl::ev()
+{
+    return (_paramComplete && _activeSettings.contains(kCAM_EV)) ? getFact(kCAM_EV) : nullptr;
+}
+
+//-----------------------------------------------------------------------------
+Fact*
+QGCCameraControl::iso()
+{
+    return (_paramComplete && _activeSettings.contains(kCAM_ISO)) ? getFact(kCAM_ISO) : nullptr;
+}
+
+//-----------------------------------------------------------------------------
+Fact*
+QGCCameraControl::shutter()
+{
+    return (_paramComplete && _activeSettings.contains(kCAM_SHUTTER)) ? getFact(kCAM_SHUTTER) : nullptr;
+}
+
+//-----------------------------------------------------------------------------
+Fact*
+QGCCameraControl::aperture()
+{
+    return (_paramComplete && _activeSettings.contains(kCAM_APERTURE)) ? getFact(kCAM_APERTURE) : nullptr;
+}
+
+//-----------------------------------------------------------------------------
+Fact*
+QGCCameraControl::wb()
+{
+    return (_paramComplete && _activeSettings.contains(kCAM_WBMODE)) ? getFact(kCAM_WBMODE) : nullptr;
+}

src/Camera/QGCCameraControl.h

@@ -97,12 +97,25 @@ public:
     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         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(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*        shutter             READ shutter            NOTIFY parametersReady)
+    Q_PROPERTY(Fact*        aperture            READ aperture           NOTIFY parametersReady)
+    Q_PROPERTY(Fact*        wb                  READ wb                 NOTIFY parametersReady)
 
     Q_PROPERTY(QStringList  activeSettings      READ activeSettings                                 NOTIFY activeSettingsChanged)
     Q_PROPERTY(VideoStatus  videoStatus         READ videoStatus                                    NOTIFY videoStatusChanged)
@@ -122,6 +135,9 @@ public:
     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       ();
 
     virtual int         version             () { return _version; }
     virtual QString     modelName           () { return _modelName; }
@@ -133,6 +149,8 @@ public:
     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        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; }
 
@@ -148,7 +166,19 @@ public:
     virtual quint32     storageFree         () { return _storageFree;  }
     virtual QString     storageFreeStr      ();
     virtual quint32     storageTotal        () { return _storageTotal; }
+    virtual bool        paramComplete       () { return _paramComplete; }
+    virtual qreal       zoomLevel           () { return _zoomLevel; }
+    virtual qreal       focusLevel          () { return _focusLevel; }
 
+    virtual Fact*       exposureMode        ();
+    virtual Fact*       ev                  ();
+    virtual Fact*       iso                 ();
+    virtual Fact*       shutter             ();
+    virtual Fact*       aperture            ();
+    virtual Fact*       wb                  ();
+
+    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);
@@ -180,6 +210,8 @@ signals:
     void    storageTotalChanged             ();
     void    dataReady                       (QByteArray data);
     void    parametersReady                 ();
+    void    zoomLevelChanged                ();
+    void    focusLevelChanged               ();
 
 protected:
     virtual void    _setVideoStatus         (VideoStatus status);
@@ -198,6 +230,7 @@ protected slots:
     virtual void    _dataReady              (QByteArray data);
     virtual void    _paramDone              ();
 
+
 private:
     bool    _handleLocalization             (QByteArray& bytes);
     bool    _replaceLocaleStrings           (const QDomNode node, QByteArray& bytes);
@@ -224,6 +257,9 @@ protected:
     mavlink_camera_information_t        _info;
     int                                 _version;
     bool                                _cached;
+    bool                                _paramComplete;
+    qreal                               _zoomLevel;
+    qreal                               _focusLevel;
     uint32_t                            _storageFree;
     uint32_t                            _storageTotal;
     QNetworkAccessManager*              _netManager;