camera : add format storage and reset settings function

examples/camera_server/camera_client.cpp

@@ -90,4 +90,11 @@ void do_camera_operation(mavsdk::Camera& camera)
 
     operation_result = camera.stop_video_streaming(2);
     std::cout << "stop video streaming result : " << operation_result << std::endl;
+
+    // format the storage with special storage id test
+    operation_result = camera.format_storage(11);
+    std::cout << "format storage result : " << operation_result << std::endl;
+
+    operation_result = camera.reset_settings();
+    std::cout << "Reset camera settings result : " << operation_result << std::endl;
 }

examples/camera_server/camera_server.cpp

@@ -159,4 +159,11 @@ static void subscribe_camera_operation(mavsdk::CameraServer& camera_server)
         }
         camera_server.respond_capture_status(capture_status);
     });
+
+    camera_server.subscribe_format_storage([](int storage_id) {
+        std::cout << "format storage with id : " << storage_id << std::endl;
+    });
+
+    camera_server.subscribe_reset_settings(
+        [](int camera_id) { std::cout << "reset camera settings" << std::endl; });
 }

src/integration_tests/CMakeLists.txt

@@ -18,6 +18,7 @@ add_executable(integration_tests_runner
     camera_take_photo.cpp
     camera_take_photo_interval.cpp
     camera_format.cpp
+    camera_reset_settings.cpp
     camera_test_helpers.cpp
     connection.cpp
     follow_me.cpp

src/integration_tests/camera_format.cpp

@@ -22,5 +22,5 @@ TEST(CameraTest, Format)
 
     auto camera = std::make_shared<Camera>(system);
 
-    EXPECT_EQ(Camera::Result::Success, camera->format_storage());
+    EXPECT_EQ(Camera::Result::Success, camera->format_storage(1));
 }

src/integration_tests/camera_reset_settings.cpp

@@ -0,0 +1,26 @@
+#include "integration_test_helper.h"
+#include "mavsdk.h"
+#include <iostream>
+#include <functional>
+#include <atomic>
+#include "plugins/camera/camera.h"
+
+using namespace mavsdk;
+
+TEST(CameraTest, ResetSettings)
+{
+    Mavsdk mavsdk;
+
+    ConnectionResult ret = mavsdk.add_udp_connection();
+    ASSERT_EQ(ret, ConnectionResult::Success);
+
+    // Wait for system to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    auto system = mavsdk.systems().at(0);
+    ASSERT_TRUE(system->has_camera());
+
+    auto camera = std::make_shared<Camera>(system);
+
+    EXPECT_EQ(Camera::Result::Success, camera->reset_settings());
+}

src/mavsdk/plugins/camera/camera.cpp

@@ -239,21 +239,31 @@ std::pair<Camera::Result, Camera::Setting> Camera::get_setting(Setting setting)
     return _impl->get_setting(setting);
 }
 
-void Camera::format_storage_async(const ResultCallback callback)
+void Camera::format_storage_async(int32_t storage_id, const ResultCallback callback)
 {
-    _impl->format_storage_async(callback);
+    _impl->format_storage_async(storage_id, callback);
 }
 
-Camera::Result Camera::format_storage() const
+Camera::Result Camera::format_storage(int32_t storage_id) const
 {
-    return _impl->format_storage();
+    return _impl->format_storage(storage_id);
 }
 
 Camera::Result Camera::select_camera(int32_t camera_id) const
 {
     return _impl->select_camera(camera_id);
 }
 
+void Camera::reset_settings_async(const ResultCallback callback)
+{
+    _impl->reset_settings_async(callback);
+}
+
+Camera::Result Camera::reset_settings() const
+{
+    return _impl->reset_settings();
+}
+
 std::ostream& operator<<(std::ostream& str, Camera::Result const& result)
 {
     switch (result) {

src/mavsdk/plugins/camera/camera_impl.cpp

@@ -1955,22 +1955,22 @@ void CameraImpl::request_camera_information()
     _system_impl->send_command_async(command_camera_info, nullptr);
 }
 
-Camera::Result CameraImpl::format_storage()
+Camera::Result CameraImpl::format_storage(int32_t storage_id)
 {
     auto prom = std::make_shared<std::promise<Camera::Result>>();
     auto ret = prom->get_future();
 
-    format_storage_async([prom](Camera::Result result) { prom->set_value(result); });
+    format_storage_async(storage_id, [prom](Camera::Result result) { prom->set_value(result); });
 
     return ret.get();
 }
 
-void CameraImpl::format_storage_async(Camera::ResultCallback callback)
+void CameraImpl::format_storage_async(int32_t storage_id, Camera::ResultCallback callback)
 {
     MavlinkCommandSender::CommandLong cmd_format{};
 
     cmd_format.command = MAV_CMD_STORAGE_FORMAT;
-    cmd_format.params.maybe_param1 = 1.0f; // storage ID
+    cmd_format.params.maybe_param1 = storage_id; // storage ID
     cmd_format.params.maybe_param2 = 1.0f; // format
     cmd_format.params.maybe_param3 = 1.0f; // clear
     cmd_format.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
@@ -1989,6 +1989,33 @@ void CameraImpl::format_storage_async(Camera::ResultCallback callback)
         });
 }
 
+Camera::Result CameraImpl::reset_settings()
+{
+    auto prom = std::make_shared<std::promise<Camera::Result>>();
+    auto ret = prom->get_future();
+
+    reset_settings_async([prom](Camera::Result result) { prom->set_value(result); });
+
+    return ret.get();
+}
+void CameraImpl::reset_settings_async(const Camera::ResultCallback callback)
+{
+    MavlinkCommandSender::CommandLong cmd_format{};
+
+    cmd_format.command = MAV_CMD_RESET_CAMERA_SETTINGS;
+    cmd_format.params.maybe_param1 = 1.0; // reset
+    cmd_format.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
+
+    _system_impl->send_command_async(
+        cmd_format, [this, callback](MavlinkCommandSender::Result result, float progress) {
+            UNUSED(progress);
+
+            receive_command_result(result, [this, callback](Camera::Result camera_result) {
+                callback(camera_result);
+            });
+        });
+}
+
 void CameraImpl::reset_following_format_storage()
 {
     {

src/mavsdk/plugins/camera/camera_impl.h

@@ -86,8 +86,11 @@ class CameraImpl : public PluginImplBase {
     subscribe_possible_setting_options(const Camera::PossibleSettingOptionsCallback& callback);
     void unsubscribe_possible_setting_options(Camera::PossibleSettingOptionsHandle handle);
 
-    Camera::Result format_storage();
-    void format_storage_async(Camera::ResultCallback callback);
+    Camera::Result format_storage(int32_t storage_id);
+    void format_storage_async(int32_t storage_id, const Camera::ResultCallback callback);
+
+    Camera::Result reset_settings();
+    void reset_settings_async(const Camera::ResultCallback callback);
 
     std::pair<Camera::Result, std::vector<Camera::CaptureInfo>>
     list_photos(Camera::PhotosRange photos_range);

src/mavsdk/plugins/camera/include/plugins/camera/camera.h

@@ -875,7 +875,7 @@ class Camera : public PluginBase {
      *
      * This function is non-blocking. See 'format_storage' for the blocking counterpart.
      */
-    void format_storage_async(const ResultCallback callback);
+    void format_storage_async(int32_t storage_id, const ResultCallback callback);
 
     /**
      * @brief Format storage (e.g. SD card) in camera.
@@ -886,7 +886,7 @@ class Camera : public PluginBase {
      *
      * @return Result of request.
      */
-    Result format_storage() const;
+    Result format_storage(int32_t storage_id) const;
 
     /**
      * @brief Select current camera .
@@ -899,6 +899,26 @@ class Camera : public PluginBase {
      */
     Result select_camera(int32_t camera_id) const;
 
+    /**
+     * @brief Reset all settings in camera.
+     *
+     * This will reset all camera settings to default value
+     *
+     * This function is non-blocking. See 'reset_settings' for the blocking counterpart.
+     */
+    void reset_settings_async(const ResultCallback callback);
+
+    /**
+     * @brief Reset all settings in camera.
+     *
+     * This will reset all camera settings to default value
+     *
+     * This function is blocking. See 'reset_settings_async' for the non-blocking counterpart.
+     *
+     * @return Result of request.
+     */
+    Result reset_settings() const;
+
     /**
      * @brief Copy constructor.
      */

src/mavsdk/plugins/camera/mocks/camera_mock.h

@@ -44,11 +44,12 @@ class MockCamera {
         unsubscribe_possible_setting_options, void(Camera::PossibleSettingOptionsHandle)){};
     MOCK_CONST_METHOD3(
         set_option_async, void(Camera::ResultCallback, const std::string&, const Camera::Option)){};
-    MOCK_CONST_METHOD0(format_storage, Camera::Result()){};
+    MOCK_CONST_METHOD1(format_storage, Camera::Result(int32_t)){};
     MOCK_CONST_METHOD1(
         list_photos,
         std::pair<Camera::Result, std::vector<Camera::CaptureInfo>>(Camera::PhotosRange)){};
     MOCK_CONST_METHOD1(select_camera, Camera::Result(int32_t)){};
+    MOCK_CONST_METHOD0(reset_settings, Camera::Result()){};
 };
 
 } // namespace testing

src/mavsdk/plugins/camera_server/camera_server.cpp

@@ -137,6 +137,28 @@ CameraServer::Result CameraServer::respond_capture_status(CaptureStatus capture_
     return _impl->respond_capture_status(capture_status);
 }
 
+CameraServer::FormatStorageHandle
+CameraServer::subscribe_format_storage(const FormatStorageCallback& callback)
+{
+    return _impl->subscribe_format_storage(callback);
+}
+
+void CameraServer::unsubscribe_format_storage(FormatStorageHandle handle)
+{
+    _impl->unsubscribe_format_storage(handle);
+}
+
+CameraServer::ResetSettingsHandle
+CameraServer::subscribe_reset_settings(const ResetSettingsCallback& callback)
+{
+    return _impl->subscribe_reset_settings(callback);
+}
+
+void CameraServer::unsubscribe_reset_settings(ResetSettingsHandle handle)
+{
+    _impl->unsubscribe_reset_settings(handle);
+}
+
 bool operator==(const CameraServer::Information& lhs, const CameraServer::Information& rhs)
 {
     return (rhs.vendor_name == lhs.vendor_name) && (rhs.model_name == lhs.model_name) &&

src/mavsdk/plugins/camera_server/camera_server_impl.cpp

@@ -492,6 +492,27 @@ CameraServerImpl::respond_capture_status(CameraServer::CaptureStatus capture_sta
     return CameraServer::Result::Success;
 }
 
+CameraServer::FormatStorageHandle
+CameraServerImpl::subscribe_format_storage(const CameraServer::FormatStorageCallback& callback)
+{
+    return _format_storage_callbacks.subscribe(callback);
+}
+void CameraServerImpl::unsubscribe_format_storage(CameraServer::FormatStorageHandle handle)
+{
+    _format_storage_callbacks.unsubscribe(handle);
+}
+
+CameraServer::ResetSettingsHandle
+CameraServerImpl::subscribe_reset_settings(const CameraServer::ResetSettingsCallback& callback)
+{
+    return _reset_settings_callbacks.subscribe(callback);
+}
+
+void CameraServerImpl::unsubscribe_reset_settings(CameraServer::ResetSettingsHandle handle)
+{
+    _reset_settings_callbacks.unsubscribe(handle);
+}
+
 /**
  * Starts capturing images with the given interval.
  * @param [in]  interval_s      The interval between captures in seconds.
@@ -682,14 +703,18 @@ CameraServerImpl::process_storage_format(const MavlinkCommandReceiver::CommandLo
     auto format = static_cast<bool>(command.params.param2);
     auto reset_image_log = static_cast<bool>(command.params.param3);
 
-    UNUSED(storage_id);
     UNUSED(format);
     UNUSED(reset_image_log);
+    if (_format_storage_callbacks.empty()) {
+        LogDebug() << "process storage format requested with no storage format subscriber";
+        return _server_component_impl->make_command_ack_message(
+            command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
+    }
 
-    LogDebug() << "unsupported storage format request";
+    _format_storage_callbacks(storage_id);
 
     return _server_component_impl->make_command_ack_message(
-        command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
+        command, MAV_RESULT::MAV_RESULT_ACCEPTED);
 }
 
 std::optional<mavlink_message_t> CameraServerImpl::process_camera_capture_status_request(
@@ -727,10 +752,16 @@ CameraServerImpl::process_reset_camera_settings(const MavlinkCommandReceiver::Co
 
     UNUSED(reset);
 
-    LogDebug() << "unsupported reset camera settings request";
+    if (_reset_settings_callbacks.empty()) {
+        LogDebug() << "reset camera settings requested with no camera settings subscriber";
+        return _server_component_impl->make_command_ack_message(
+            command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
+    }
+
+    _reset_settings_callbacks(0);
 
     return _server_component_impl->make_command_ack_message(
-        command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
+        command, MAV_RESULT::MAV_RESULT_ACCEPTED);
 }
 
 std::optional<mavlink_message_t>

src/mavsdk/plugins/camera_server/camera_server_impl.h

@@ -54,6 +54,14 @@ class CameraServerImpl : public ServerPluginImplBase {
     void unsubscribe_capture_status(CameraServer::CaptureStatusHandle handle);
     CameraServer::Result respond_capture_status(CameraServer::CaptureStatus capture_status) const;
 
+    CameraServer::FormatStorageHandle
+    subscribe_format_storage(const CameraServer::FormatStorageCallback& callback);
+    void unsubscribe_format_storage(CameraServer::FormatStorageHandle handle);
+
+    CameraServer::ResetSettingsHandle
+    subscribe_reset_settings(const CameraServer::ResetSettingsCallback& callback);
+    void unsubscribe_reset_settings(CameraServer::ResetSettingsHandle handle);
+
 private:
     enum StatusFlags {
         IN_PROGRESS = 1 << 0,
@@ -85,6 +93,8 @@ class CameraServerImpl : public ServerPluginImplBase {
     CallbackList<CameraServer::Mode> _set_mode_callbacks{};
     CallbackList<int32_t> _storage_information_callbacks{};
     CallbackList<int32_t> _capture_status_callbacks{};
+    CallbackList<int32_t> _format_storage_callbacks{};
+    CallbackList<int32_t> _reset_settings_callbacks{};
 
     MavlinkCommandReceiver::CommandLong _last_take_photo_command;
     uint8_t _last_storage_id;