camera: added setter/getter for camera mode

mavlink · Mar 23, 2018 · ce6cb7a · ce6cb7a
1 parent c151c0f
commit ce6cb7a
Show file tree

Hide file tree

Showing 4 changed files with 92 additions and 1 deletion.
diff --git a/plugins/camera/camera.cpp b/plugins/camera/camera.cpp
@@ -63,6 +63,16 @@ void Camera::stop_video_async(const result_callback_t &callback)
     _impl->stop_video_async(callback);
 }
 
+void Camera::set_mode_async(Mode mode, const mode_callback_t &callback)
+{
+    _impl->set_mode_async(mode, callback);
+}
+
+void Camera::get_mode_async(const mode_callback_t &callback)
+{
+    _impl->get_mode_async(callback);
+}
+
 const char *Camera::result_str(Result result)
 {
     switch (result) {

diff --git a/plugins/camera/camera.h b/plugins/camera/camera.h
@@ -154,6 +154,37 @@ class Camera : public PluginBase
      */
     void stop_video_async(const result_callback_t &callback);
 
+
+    /**
+     * @brief Camera mode type.
+     */
+    enum class Mode {
+        PHOTO,
+        PHOTO_SURVEY,
+        VIDEO,
+        UNKNOWN
+    };
+
+    /**
+     * @brief Callback type for asynchronous camera mode calls.
+     */
+    typedef std::function<void(Result, const Mode &)> mode_callback_t;
+
+    /**
+     * @brief Setter for camera mode (asynchronous).
+     *
+     * @param mode Camera mode to set.
+     * @param callback Function to call with result of request.
+     */
+    void set_mode_async(Mode mode, const mode_callback_t &callback);
+
+    /**
+     * @brief Getter for camera mode (asynchronous).
+     *
+     * @param callback Function to call with result of request.
+     */
+    void get_mode_async(const mode_callback_t &callback);
+
     /**
      * @brief Copy constructor (object is not copyable).
      */

diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp
@@ -98,6 +98,37 @@ Camera::Result CameraImpl::camera_result_from_command_result(MavlinkCommands::Re
     }
 }
 
+void CameraImpl::set_mode_async(Camera::Mode mode, const Camera::mode_callback_t &callback)
+{
+    float mavlink_mode;
+    switch (mode) {
+        case Camera::Mode::PHOTO:
+            mavlink_mode = CAMERA_MODE_IMAGE;
+            break;
+        case Camera::Mode::VIDEO:
+            mavlink_mode = CAMERA_MODE_VIDEO;
+            break;
+        case Camera::Mode::PHOTO_SURVEY:
+            mavlink_mode = CAMERA_MODE_IMAGE_SURVEY;
+            break;
+        default:
+        // FALLTHROUGH
+        case Camera::Mode::UNKNOWN:
+            mavlink_mode = NAN;
+            break;
+    }
+
+    _parent.send_command_with_ack_async(
+        MAV_CMD_SET_CAMERA_MODE,
+        MavlinkCommands::Params {0.0f, // Reserved, set to 0
+                                 mavlink_mode,
+                                 NAN, NAN, NAN, NAN, NAN // Reserved
+                                },
+        std::bind(&CameraImpl::receive_set_mode_command_result, this, std::placeholders::_1, callback,
+                  mode),
+        MAV_COMP_ID_CAMERA);
+}
+
 bool CameraImpl::interval_valid(float interval_s)
 {
     // Reject everything faster than 1000 Hz, as well as negative inputs.
@@ -109,4 +140,15 @@ bool CameraImpl::interval_valid(float interval_s)
     }
 }
 
+void CameraImpl::receive_set_mode_command_result(MavlinkCommands::Result command_result,
+                                                 const Camera::mode_callback_t &callback,
+                                                 Camera::Mode mode)
+{
+    Camera::Result camera_result = camera_result_from_command_result(command_result);
+
+    if (callback) {
+        callback(camera_result, mode);
+    }
+}
+
 } // namespace dronecore
diff --git a/plugins/camera/camera_impl.h b/plugins/camera/camera_impl.h
@@ -34,17 +34,25 @@ class CameraImpl : public PluginImplBase
     void start_video_async(const Camera::result_callback_t &callback);
     void stop_video_async(const Camera::result_callback_t &callback);
 
+    void set_mode_async(Camera::Mode mode, const Camera::mode_callback_t &callback);
+    void get_mode_async(const Camera::mode_callback_t &callback);
+
     // Non-copyable
     CameraImpl(const CameraImpl &) = delete;
     const CameraImpl &operator=(const CameraImpl &) = delete;
 
 private:
+    void receive_set_mode_command_result(MavlinkCommands::Result command_result,
+                                         const Camera::mode_callback_t &callback,
+                                         Camera::Mode mode);
+
     static Camera::Result camera_result_from_command_result(
         MavlinkCommands::Result command_result);
 
+    static bool interval_valid(float interval_s);
+
     int _capture_sequence = 0;
 
-    static bool interval_valid(float interval_s);
 };