Merge pull request #1599 from mavlink/pr-request-dropped-captures

camera: request dropped IMAGE_CAPTURED messages

src/integration_tests/CMakeLists.txt

@@ -30,6 +30,7 @@ add_executable(integration_tests_runner
     action_goto.cpp
     action_hold.cpp
     calibration.cpp
+    camera_capture_info.cpp
     camera_mode.cpp
     camera_settings.cpp
     camera_status.cpp

src/integration_tests/camera_capture_info.cpp

@@ -0,0 +1,37 @@
+#include "integration_test_helper.h"
+#include "mavsdk.h"
+#include <iostream>
+#include <functional>
+#include <atomic>
+#include "plugins/camera/camera.h"
+
+using namespace mavsdk;
+
+TEST(CameraTest, CaptureInfo)
+{
+    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));
+
+    ASSERT_EQ(mavsdk.systems().size(), 1);
+
+    auto system = mavsdk.systems().at(0);
+    auto camera = Camera{system};
+
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    EXPECT_EQ(camera.take_photo(), Camera::Result::Success);
+
+    bool received_capture_info = false;
+    camera.subscribe_capture_info([&received_capture_info](Camera::CaptureInfo capture_info) {
+        received_capture_info = true;
+        LogInfo() << capture_info;
+    });
+
+    std::this_thread::sleep_for(std::chrono::seconds(5));
+    EXPECT_TRUE(received_capture_info);
+}

src/plugins/camera/camera_impl.cpp

@@ -69,10 +69,14 @@ void CameraImpl::init()
 
     _parent->add_call_every(
         [this]() { check_connection_status(); }, 0.5, &_check_connection_status_call_every_cookie);
+
+    _parent->add_call_every(
+        [this]() { request_missing_capture_info(); }, 0.5, &_request_missing_capture_info_cookie);
 }
 
 void CameraImpl::deinit()
 {
+    _parent->remove_call_every(_request_missing_capture_info_cookie);
     _parent->remove_call_every(_check_connection_status_call_every_cookie);
     _parent->remove_call_every(_status.call_every_cookie);
     _parent->unregister_all_mavlink_message_handlers(this);
@@ -932,15 +936,62 @@ void CameraImpl::process_camera_image_captured(const mavlink_message_t& message)
         _captured_request_cv.notify_all();
 
         std::lock_guard<std::mutex> lock(_capture_info.mutex);
-        if (_capture_info.last_advertised_image_index != -1 &&
-            _capture_info.last_advertised_image_index < capture_info.index &&
-            _capture_info.callback) {
-            _capture_info.last_advertised_image_index = capture_info.index;
-            const auto temp_callback = _capture_info.callback;
-            _parent->call_user_callback(
-                [temp_callback, capture_info]() { temp_callback(capture_info); });
+        if (_capture_info.last_advertised_image_index != -1) {
+            // Notify user if a new image has been captured.
+            if (_capture_info.last_advertised_image_index < capture_info.index) {
+                if (_capture_info.callback) {
+                    const auto temp_callback = _capture_info.callback;
+                    _parent->call_user_callback(
+                        [temp_callback, capture_info]() { temp_callback(capture_info); });
+                }
+
+                // Save captured indices that have been dropped
+                for (int i = _capture_info.last_advertised_image_index + 1; i < capture_info.index;
+                     ++i) {
+                    if (_capture_info.missing_image_retries.find(i) ==
+                        _capture_info.missing_image_retries.end()) {
+                        _capture_info.missing_image_retries[i] = 0;
+                    }
+                }
+
+                _capture_info.last_advertised_image_index = capture_info.index;
+            }
+
+            else if (auto it = _capture_info.missing_image_retries.find(capture_info.index);
+                     it != _capture_info.missing_image_retries.end()) {
+                if (_capture_info.callback) {
+                    const auto temp_callback = _capture_info.callback;
+                    _parent->call_user_callback(
+                        [temp_callback, capture_info]() { temp_callback(capture_info); });
+                }
+                _capture_info.missing_image_retries.erase(it);
+            }
+        }
+    }
+}
+
+void CameraImpl::request_missing_capture_info()
+{
+    std::lock_guard<std::mutex> lock(_capture_info.mutex);
+
+    for (auto it = _capture_info.missing_image_retries.begin();
+         it != _capture_info.missing_image_retries.end();
+         /* ++it */) {
+        if (it->second > 3) {
+            it = _capture_info.missing_image_retries.erase(it);
+        } else {
+            ++it;
         }
     }
+
+    if (!_capture_info.missing_image_retries.empty()) {
+        auto it_lowest_retries = std::min_element(
+            _capture_info.missing_image_retries.begin(), _capture_info.missing_image_retries.end());
+        _parent->send_command_async(
+            CameraImpl::make_command_request_camera_image_captured(it_lowest_retries->first),
+            nullptr);
+        it_lowest_retries->second += 1;
+    }
 }
 
 Camera::EulerAngle CameraImpl::to_euler_angle_from_quaternion(Camera::Quaternion quaternion)
@@ -1831,6 +1882,7 @@ void CameraImpl::format_storage_async(Camera::ResultCallback callback)
                     {
                         std::lock_guard<std::mutex> lock(_capture_info.mutex);
                         _capture_info.last_advertised_image_index = -1;
+                        _capture_info.missing_image_retries.clear();
                     }
                 }
 

src/plugins/camera/camera_impl.h

@@ -158,6 +158,7 @@ class CameraImpl : public PluginImplBase {
     void* _camera_information_call_every_cookie{nullptr};
     void* _flight_information_call_every_cookie{nullptr};
     void* _check_connection_status_call_every_cookie{nullptr};
+    void* _request_missing_capture_info_cookie{nullptr};
 
     void request_camera_settings();
     void request_camera_information();
@@ -186,6 +187,8 @@ class CameraImpl : public PluginImplBase {
     MavlinkCommandSender::CommandLong make_command_request_video_stream_info();
     MavlinkCommandSender::CommandLong make_command_request_video_stream_status();
 
+    void request_missing_capture_info();
+
     std::unique_ptr<CameraDefinition> _camera_definition{};
     bool _is_fetching_camera_definition{false};
     bool _has_camera_definition_timed_out{false};
@@ -228,6 +231,7 @@ class CameraImpl : public PluginImplBase {
         std::mutex mutex{};
         Camera::CaptureInfoCallback callback{nullptr};
         int last_advertised_image_index{-1};
+        std::map<int, int> missing_image_retries{};
     } _capture_info{};
 
     struct {