camera_server: use subscriptions as capability flags

mavlink · Feb 17, 2022 · 6c17373 · 6c17373
1 parent b5ea0d6
commit 6c17373
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 27 deletions.
diff --git a/examples/camera_server/camera_server.cpp b/examples/camera_server/camera_server.cpp
@@ -7,18 +7,6 @@
 #include <mavsdk/plugins/camera/camera.h>
 #include <mavsdk/plugins/camera_server/camera_server.h>
 
-/*
- This example runs a MAVLink "camera" utilizing the MAVSDK server plugins
- on a separate thread. This uses two MAVSDK instances, one GCS, one camera.
-
- The main thread acts as a GCS and reads telemetry, parameters, transmits across
- a mission, clears the mission, arms the vehicle and then triggers a vehicle takeoff.
-
- The camera thread handles all the servers and triggers callbacks, publishes telemetry,
- handles and stores parameters, prints received missions and sets the vehicle height to 10m on
- successful takeoff request.
-*/
-
 using namespace mavsdk;
 
 using std::chrono::duration_cast;
@@ -54,17 +42,7 @@ int main(int argc, char** argv)
 
             all_camera_servers.insert({system->get_system_id(), camera_server});
 
-            camera_server->set_information({
-                .vendor_name = "MAVSDK",
-                .model_name = "Example Camera Server",
-                .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,
-            });
-
-            camera_server->set_in_progress(false);
+            // First add all subscriptions. This defines the camera capabilities.
 
             camera_server->subscribe_take_photo(
                 [camera_server, &all_camera_servers](CameraServer::Result result, int32_t index) {
@@ -99,6 +77,22 @@ int main(int argc, char** argv)
                     }
                 });
 
+            // Then set the initial state of everything.
+
+            camera_server->set_in_progress(false);
+
+            // Finally call set_information() to "activate" the camera plugin.
+
+            camera_server->set_information({
+                .vendor_name = "MAVSDK",
+                .model_name = "Example Camera Server",
+                .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,
+            });
+
             std::cout << "Connected to " << (system->is_standalone() ? "GCS" : "autopilot")
                       << " system ID " << +system->get_system_id() << std::endl;
         }

diff --git a/proto b/proto
diff --git a/src/mavsdk/plugins/camera_server/camera_server_impl.cpp b/src/mavsdk/plugins/camera_server/camera_server_impl.cpp
@@ -291,6 +291,13 @@ std::optional<mavlink_message_t> CameraServerImpl::process_camera_information_re
     const uint16_t camera_definition_version = 0;
     auto camera_definition_uri = "";
 
+    // capability flags are determined by subscriptions
+    uint32_t capability_flags{};
+
+    if (_take_photo_callback) {
+        capability_flags |= CAMERA_CAP_FLAGS::CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
+    }
+
     mavlink_message_t msg{};
     mavlink_msg_camera_information_pack(
         _parent->get_own_system_id(),
@@ -306,7 +313,7 @@ std::optional<mavlink_message_t> CameraServerImpl::process_camera_information_re
         _information.horizontal_resolution_px,
         _information.vertical_resolution_px,
         lens_id,
-        CAMERA_CAP_FLAGS::CAMERA_CAP_FLAGS_CAPTURE_IMAGE,
+        capability_flags,
         camera_definition_version,
         camera_definition_uri);
 
@@ -561,8 +568,7 @@ CameraServerImpl::process_image_start_capture(const MavlinkCommandReceiver::Comm
 
     if (!_take_photo_callback) {
         LogDebug() << "image capture requested with no take photo subscriber";
-        return _parent->make_command_ack_message(
-            command, MAV_RESULT::MAV_RESULT_TEMPORARILY_REJECTED);
+        return _parent->make_command_ack_message(command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
     }
 
     // single image capture