camera: fix format storage

src/integration_tests/CMakeLists.txt

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

src/mavsdk/plugins/camera/camera.cpp

@@ -703,7 +703,8 @@ std::ostream& operator<<(std::ostream& str, Camera::EulerAngle const& euler_angl
 
 bool operator==(const Camera::CaptureInfo& lhs, const Camera::CaptureInfo& rhs)
 {
-    return (rhs.position == lhs.position) && (rhs.attitude_quaternion == lhs.attitude_quaternion) &&
+    return (rhs.camera_id == lhs.camera_id) && (rhs.position == lhs.position) &&
+           (rhs.attitude_quaternion == lhs.attitude_quaternion) &&
            (rhs.attitude_euler_angle == lhs.attitude_euler_angle) &&
            (rhs.time_utc_us == lhs.time_utc_us) && (rhs.is_success == lhs.is_success) &&
            (rhs.index == lhs.index) && (rhs.file_url == lhs.file_url);
@@ -713,6 +714,7 @@ std::ostream& operator<<(std::ostream& str, Camera::CaptureInfo const& capture_i
 {
     str << std::setprecision(15);
     str << "capture_info:" << '\n' << "{\n";
+    str << "    camera_id: " << capture_info.camera_id << '\n';
     str << "    position: " << capture_info.position << '\n';
     str << "    attitude_quaternion: " << capture_info.attitude_quaternion << '\n';
     str << "    attitude_euler_angle: " << capture_info.attitude_euler_angle << '\n';

src/mavsdk/plugins/camera/camera_impl.cpp

@@ -719,7 +719,7 @@ void CameraImpl::request_slower()
     for (auto& camera : _potential_cameras) {
         _system_impl->mavlink_request_message().request(
             MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION,
-            component_id_for_camera_id(camera.component_id),
+            component_id_for_camera_id_with_lock(camera.component_id),
             nullptr);
     }
 }
@@ -995,7 +995,7 @@ void CameraImpl::process_camera_capture_status(const mavlink_message_t& message)
     mavlink_msg_camera_capture_status_decode(&message, &camera_capture_status);
 
     std::lock_guard lock(_mutex);
-    auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(message.compid);
+    auto maybe_potential_camera = maybe_potential_camera_for_component_id_with_lock(message.compid);
 
     if (maybe_potential_camera == nullptr) {
         return;
@@ -1104,67 +1104,77 @@ void CameraImpl::process_camera_image_captured(const mavlink_message_t& message)
     mavlink_camera_image_captured_t image_captured;
     mavlink_msg_camera_image_captured_decode(&message, &image_captured);
 
-    // TODO: enable again
+    std::lock_guard lock(_mutex);
+    auto maybe_potential_camera = maybe_potential_camera_for_component_id_with_lock(message.compid);
+
+    if (maybe_potential_camera == nullptr) {
+        return;
+    }
+
+    auto& camera = *maybe_potential_camera;
 
+    mavsdk::Quaternion quaternion{};
+    quaternion.w = image_captured.q[0];
+    quaternion.x = image_captured.q[1];
+    quaternion.y = image_captured.q[2];
+    quaternion.z = image_captured.q[3];
+    auto euler = to_euler_angle_from_quaternion(quaternion);
+
+    Camera::CaptureInfo capture_info = {};
+    capture_info.position.latitude_deg = image_captured.lat / 1e7;
+    capture_info.position.longitude_deg = image_captured.lon / 1e7;
+    capture_info.position.absolute_altitude_m = static_cast<float>(image_captured.alt) / 1e3f;
+    capture_info.position.relative_altitude_m =
+        static_cast<float>(image_captured.relative_alt) / 1e3f;
+    capture_info.time_utc_us = image_captured.time_utc;
+    capture_info.attitude_quaternion.w = image_captured.q[0];
+    capture_info.attitude_quaternion.x = image_captured.q[1];
+    capture_info.attitude_quaternion.y = image_captured.q[2];
+    capture_info.attitude_quaternion.z = image_captured.q[3];
+    capture_info.attitude_euler_angle.roll_deg = euler.roll_deg;
+    capture_info.attitude_euler_angle.pitch_deg = euler.pitch_deg;
+    capture_info.attitude_euler_angle.yaw_deg = euler.yaw_deg;
+    capture_info.file_url = std::string(image_captured.file_url);
+    capture_info.is_success = (image_captured.capture_result == 1);
+    capture_info.index = image_captured.image_index;
+    capture_info.camera_id = camera_id_for_potential_camera_with_lock(camera);
+
+    _capture_info_callbacks.queue(
+        capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); });
+
+    // TODO: enable again
 #if 0
-    {
-        mavsdk::Quaternion quaternion{};
-        quaternion.w = image_captured.q[0];
-        quaternion.x = image_captured.q[1];
-        quaternion.y = image_captured.q[2];
-        quaternion.z = image_captured.q[3];
-        auto euler = to_euler_angle_from_quaternion(quaternion);
-
-        Camera::CaptureInfo capture_info = {};
-        capture_info.position.latitude_deg = image_captured.lat / 1e7;
-        capture_info.position.longitude_deg = image_captured.lon / 1e7;
-        capture_info.position.absolute_altitude_m = static_cast<float>(image_captured.alt) / 1e3f;
-        capture_info.position.relative_altitude_m =
-            static_cast<float>(image_captured.relative_alt) / 1e3f;
-        capture_info.time_utc_us = image_captured.time_utc;
-        capture_info.attitude_quaternion.w = image_captured.q[0];
-        capture_info.attitude_quaternion.x = image_captured.q[1];
-        capture_info.attitude_quaternion.y = image_captured.q[2];
-        capture_info.attitude_quaternion.z = image_captured.q[3];
-        capture_info.attitude_euler_angle.roll_deg = euler.roll_deg;
-        capture_info.attitude_euler_angle.pitch_deg = euler.pitch_deg;
-        capture_info.attitude_euler_angle.yaw_deg = euler.yaw_deg;
-        capture_info.file_url = std::string(image_captured.file_url);
-        capture_info.is_success = (image_captured.capture_result == 1);
-        capture_info.index = image_captured.image_index;
-
-        _status.photo_list.insert(std::make_pair(image_captured.image_index, capture_info));
-
-        _captured_request_cv.notify_all();
-
-        std::lock_guard lock(_capture_info.mutex);
-        // Notify user if a new image has been captured.
-        if (_capture_info.last_advertised_image_index < capture_info.index) {
-            _capture_info.callbacks.queue(
-                capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); });
-
-            if (_capture_info.last_advertised_image_index != -1) {
-                // Save captured indices that have been dropped to request later, however, don't
-                // do it from the very beginning as there might be many photos from a previous
-                // time that we don't want to request.
-                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;
-                    }
+    _status.photo_list.insert(std::make_pair(image_captured.image_index, capture_info));
+
+    _captured_request_cv.notify_all();
+
+    std::lock_guard lock(_capture_info.mutex);
+    // Notify user if a new image has been captured.
+    if (_capture_info.last_advertised_image_index < capture_info.index) {
+        _capture_info.callbacks.queue(
+            capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); });
+
+        if (_capture_info.last_advertised_image_index != -1) {
+            // Save captured indices that have been dropped to request later, however, don't
+            // do it from the very beginning as there might be many photos from a previous
+            // time that we don't want to request.
+            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()) {
-            _capture_info.callbacks.queue(
-                capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); });
-            _capture_info.missing_image_retries.erase(it);
-        }
+        _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()) {
+        _capture_info.callbacks.queue(
+            capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); });
+        _capture_info.missing_image_retries.erase(it);
     }
 #endif
 }
@@ -2471,20 +2481,30 @@ uint8_t CameraImpl::component_id_for_camera_id(int32_t camera_id)
     return _potential_cameras[camera_id - 1].component_id;
 }
 
-uint16_t CameraImpl::capture_sequence_for_camera_id(int32_t camera_id)
+uint8_t CameraImpl::component_id_for_camera_id_with_lock(int32_t camera_id)
 {
     if (camera_id == 0) {
         // All cameras
         return 0;
     }
 
-    std::lock_guard lock(_mutex);
-    auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(camera_id);
-    if (maybe_potential_camera == nullptr) {
+    if (camera_id < 0 || static_cast<size_t>(camera_id - 1) >= _potential_cameras.size()) {
+        LogErr() << "Invalid camera ID";
         return 0;
     }
 
-    return maybe_potential_camera->capture_sequence++;
+    return _potential_cameras[camera_id - 1].component_id;
+}
+
+uint16_t CameraImpl::capture_sequence_for_camera_id(int32_t camera_id)
+{
+    if (camera_id == 0) {
+        // All cameras
+        return 0;
+    }
+
+    std::lock_guard lock(_mutex);
+    return component_id_for_camera_id_with_lock(camera_id);
 }
 
 int32_t
@@ -2511,4 +2531,21 @@ CameraImpl::maybe_potential_camera_for_camera_id_with_lock(int32_t camera_id)
     return &_potential_cameras[camera_id - 1];
 }
 
+CameraImpl::PotentialCamera*
+CameraImpl::maybe_potential_camera_for_component_id_with_lock(uint8_t component_id)
+{
+    const auto it =
+        std::find_if(_potential_cameras.begin(), _potential_cameras.end(),
+            [&](auto& potential_camera) {
+                return potential_camera.component_id = component_id;
+            });
+
+    if (it == _potential_cameras.end()) {
+        return nullptr;
+    }
+
+    // How to get pointer from iterator?
+    return &(*it);
+}
+
 } // namespace mavsdk

src/mavsdk/plugins/camera/camera_impl.h

@@ -348,12 +348,16 @@ class CameraImpl : public PluginImplBase {
     void request_missing_capture_info();
 
     uint8_t component_id_for_camera_id(int32_t camera_id);
+    uint8_t component_id_for_camera_id_with_lock(int32_t camera_id);
+
     uint16_t capture_sequence_for_camera_id(int32_t camera_id);
 
     int32_t camera_id_for_potential_camera_with_lock(const PotentialCamera& potential_camera);
     PotentialCamera* maybe_potential_camera_for_camera_id_with_lock(int32_t camera_id);
     std::vector<PotentialCamera*> potential_cameras_for_camera_id_with_lock(int32_t camera_id);
 
+    PotentialCamera* maybe_potential_camera_for_component_id_with_lock(uint8_t component_id);
+
     static std::string get_filename_from_path(const std::string& path);
 
     std::mutex _mutex;

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

@@ -472,6 +472,7 @@ class Camera : public PluginBase {
      * @brief Information about a picture just captured.
      */
     struct CaptureInfo {
+        int32_t camera_id{}; /**< @brief Camera ID */
         Position position{}; /**< @brief Location where the picture was taken */
         Quaternion attitude_quaternion{}; /**< @brief Attitude of the camera when the picture was
                                              taken (quaternion) */

src/mavsdk/plugins/camera_server/camera_server_impl.cpp

@@ -991,6 +991,11 @@ CameraServerImpl::process_request_message(const MavlinkCommandReceiver::CommandL
         case MAVLINK_MSG_ID_CAMERA_INFORMATION:
             return send_camera_information(command);
 
+        case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
+            send_capture_status();
+            return _server_component_impl->make_command_ack_message(
+                command, MAV_RESULT::MAV_RESULT_ACCEPTED);
+
         default:
             LogWarn() << "Got unknown request message!";
             return _server_component_impl->make_command_ack_message(