camera_server: implement additional information fields

Author: dlech

examples/camera_server/camera_server.cpp

@@ -83,16 +83,22 @@ int main(int argc, char** argv)
 
             // Finally call set_information() to "activate" the camera plugin.
 
-            camera_server->set_information({
+            auto ret = camera_server->set_information({
                 .vendor_name = "MAVSDK",
                 .model_name = "Example Camera Server",
+                .firmware_version = "1.0.0",
                 .focal_length_mm = 3.0,
                 .horizontal_sensor_size_mm = 3.68,
                 .vertical_sensor_size_mm = 2.76,
                 .horizontal_resolution_px = 3280,
                 .vertical_resolution_px = 2464,
             });
 
+            if (ret != CameraServer::Result::Success) {
+                std::cerr << "Failed to set camera info, exiting" << std::endl;
+                exit(1);
+            }
+
             std::cout << "Connected to " << (system->is_standalone() ? "GCS" : "autopilot")
                       << " system ID " << +system->get_system_id() << std::endl;
         }

proto

@@ -50,6 +50,7 @@ CameraServer::Result CameraServer::respond_take_photo(CaptureInfo capture_info)
 bool operator==(const CameraServer::Information& lhs, const CameraServer::Information& rhs)
 {
     return (rhs.vendor_name == lhs.vendor_name) && (rhs.model_name == lhs.model_name) &&
+           (rhs.firmware_version == lhs.firmware_version) &&
            ((std::isnan(rhs.focal_length_mm) && std::isnan(lhs.focal_length_mm)) ||
             rhs.focal_length_mm == lhs.focal_length_mm) &&
            ((std::isnan(rhs.horizontal_sensor_size_mm) &&
@@ -58,7 +59,10 @@ bool operator==(const CameraServer::Information& lhs, const CameraServer::Inform
            ((std::isnan(rhs.vertical_sensor_size_mm) && std::isnan(lhs.vertical_sensor_size_mm)) ||
             rhs.vertical_sensor_size_mm == lhs.vertical_sensor_size_mm) &&
            (rhs.horizontal_resolution_px == lhs.horizontal_resolution_px) &&
-           (rhs.vertical_resolution_px == lhs.vertical_resolution_px);
+           (rhs.vertical_resolution_px == lhs.vertical_resolution_px) &&
+           (rhs.lens_id == lhs.lens_id) &&
+           (rhs.definition_file_version == lhs.definition_file_version) &&
+           (rhs.definition_file_uri == lhs.definition_file_uri);
 }
 
 std::ostream& operator<<(std::ostream& str, CameraServer::Information const& information)
@@ -67,11 +71,15 @@ std::ostream& operator<<(std::ostream& str, CameraServer::Information const& inf
     str << "information:" << '\n' << "{\n";
     str << "    vendor_name: " << information.vendor_name << '\n';
     str << "    model_name: " << information.model_name << '\n';
+    str << "    firmware_version: " << information.firmware_version << '\n';
     str << "    focal_length_mm: " << information.focal_length_mm << '\n';
     str << "    horizontal_sensor_size_mm: " << information.horizontal_sensor_size_mm << '\n';
     str << "    vertical_sensor_size_mm: " << information.vertical_sensor_size_mm << '\n';
     str << "    horizontal_resolution_px: " << information.horizontal_resolution_px << '\n';
     str << "    vertical_resolution_px: " << information.vertical_resolution_px << '\n';
+    str << "    lens_id: " << information.lens_id << '\n';
+    str << "    definition_file_version: " << information.definition_file_version << '\n';
+    str << "    definition_file_uri: " << information.definition_file_uri << '\n';
     str << '}';
     return str;
 }

src/mavsdk/plugins/camera_server/camera_server.cpp

@@ -145,10 +145,66 @@ void CameraServerImpl::disable()
     stop_image_capture_interval();
 }
 
+/**
+ * Parses a firmware version string.
+ *
+ * The string must be in the format "<major>.<minor>.<patch>.<dev>".
+ *
+ * @param [in]  version_str     The version string to be parsed.
+ * @return                      True if parsing was successful, otherwise false.
+ */
+bool CameraServerImpl::parse_version_string(std::string& version_str)
+{
+    uint32_t unused;
+
+    return parse_version_string(version_str, unused);
+}
+
+/**
+ * Parses a firmware version string.
+ *
+ * The string must be in the format "<major>[.<minor>[.<patch>[.<dev>]]]".
+ *
+ * @param [in]  version_str     The version string to be parsed.
+ * @param [out] version         Encoded version integer for passing to MAVSDK messages.
+ * @return                      True if parsing was successful, otherwise false.
+ */
+bool CameraServerImpl::parse_version_string(std::string& version_str, uint32_t& version)
+{
+    // empty string means no version
+    if (version_str.empty()) {
+        version = 0;
+
+        return true;
+    }
+
+    uint8_t major{}, minor{}, patch{}, dev{};
+
+    auto ret = sscanf(version_str.c_str(), "%hhu.%hhu.%hhu.%hhu", &major, &minor, &patch, &dev);
+
+    if (ret == EOF) {
+        return false;
+    }
+
+    // pack version according to MAVLINK spec
+    version = dev << 24 | patch << 16 | minor << 8 | major;
+
+    return true;
+}
+
 CameraServer::Result CameraServerImpl::set_information(CameraServer::Information information)
 {
+    if (!parse_version_string(information.firmware_version)) {
+        LogDebug() << "incorrectly formatted firmware version string: "
+                   << information.firmware_version;
+        return CameraServer::Result::WrongArgument;
+    }
+
+    // TODO: validate information.definition_file_uri
+
     _is_information_set = true;
     _information = information;
+
     return CameraServer::Result::Success;
 }
 
@@ -285,11 +341,10 @@ std::optional<mavlink_message_t> CameraServerImpl::process_camera_information_re
     // FIXME: why is this needed to prevent dropping messages?
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
-    // unsupported items
-    const uint32_t firmware_version = 0;
-    const uint8_t lens_id = 0;
-    const uint16_t camera_definition_version = 0;
-    auto camera_definition_uri = "";
+    // It is safe to ignore the return value of parse_version_string() here
+    // since the string was already validated in set_information().
+    uint32_t firmware_version;
+    parse_version_string(_information.firmware_version, firmware_version);
 
     // capability flags are determined by subscriptions
     uint32_t capability_flags{};
@@ -312,10 +367,10 @@ std::optional<mavlink_message_t> CameraServerImpl::process_camera_information_re
         _information.vertical_sensor_size_mm,
         _information.horizontal_resolution_px,
         _information.vertical_resolution_px,
-        lens_id,
+        _information.lens_id,
         capability_flags,
-        camera_definition_version,
-        camera_definition_uri);
+        _information.definition_file_version,
+        _information.definition_file_uri.c_str());
 
     _parent->send_message(msg);
     LogDebug() << "sent info msg";

src/mavsdk/plugins/camera_server/camera_server_impl.cpp

@@ -49,6 +49,8 @@ class CameraServerImpl : public PluginImplBase {
 
     CameraServer::TakePhotoCallback _take_photo_callback{};
 
+    bool parse_version_string(std::string& version_str);
+    bool parse_version_string(std::string& version_str, uint32_t& version);
     void start_image_capture_interval(float interval, int32_t count, int32_t index);
     void stop_image_capture_interval();
 

src/mavsdk/plugins/camera_server/camera_server_impl.h

@@ -62,11 +62,18 @@ class CameraServer : public PluginBase {
     struct Information {
         std::string vendor_name{}; /**< @brief Name of the camera vendor */
         std::string model_name{}; /**< @brief Name of the camera model */
+        std::string firmware_version{}; /**< @brief Camera firmware version in
+                                           '<major>.<minor>.<patch>.<build>' format */
         float focal_length_mm{}; /**< @brief Focal length */
         float horizontal_sensor_size_mm{}; /**< @brief Horizontal sensor size */
         float vertical_sensor_size_mm{}; /**< @brief Vertical sensor size */
         uint32_t horizontal_resolution_px{}; /**< @brief Horizontal image resolution in pixels */
         uint32_t vertical_resolution_px{}; /**< @brief Vertical image resolution in pixels */
+        uint32_t lens_id{}; /**< @brief Lens ID */
+        uint32_t
+            definition_file_version{}; /**< @brief Camera definition file version (iteration) */
+        std::string
+            definition_file_uri{}; /**< @brief Camera definition URI (http or mavlink ftp) */
     };
 
     /**
@@ -198,7 +205,8 @@ class CameraServer : public PluginBase {
     using ResultCallback = std::function<void(Result)>;
 
     /**
-     * @brief Sets the camera information
+     * @brief Sets the camera information. This must be called as soon as the camera server is
+     * created.
      *
      * This function is blocking.
      *
@@ -207,7 +215,8 @@ class CameraServer : public PluginBase {
     Result set_information(Information information) const;
 
     /**
-     * @brief Sets the camera capture status
+     * @brief Sets image capture in progress status flags. This should be set to true when the
+     * camera is busy taking a photo and false when it is done.
      *
      * This function is blocking.
      *
@@ -222,12 +231,13 @@ class CameraServer : public PluginBase {
     using TakePhotoCallback = std::function<void(Result, int32_t)>;
 
     /**
-     * @brief Subscribe to single-image capture commands
+     * @brief Subscribe to image capture requests. Each request received should respond to using
+     * RespondTakePhoto.
      */
     void subscribe_take_photo(TakePhotoCallback callback);
 
     /**
-     * @brief Respond to a single-image capture command.
+     * @brief Respond to an image capture request from SubscribeTakePhoto.
      *
      * This function is blocking.
      *