Merge pull request #2252 from mavlink/pr-camera-server-zoom

camera_server: implement zoom

examples/CMakeLists.txt

@@ -13,6 +13,7 @@ add_subdirectory(calibrate)
 add_subdirectory(camera)
 add_subdirectory(camera_server)
 add_subdirectory(camera_settings)
+add_subdirectory(camera_zoom)
 add_subdirectory(fly_mission)
 add_subdirectory(fly_multiple_drones)
 add_subdirectory(fly_qgc_mission)

examples/camera_zoom/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(camera_zoom)
+
+add_executable(camera_zoom
+    camera_zoom.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(camera_zoom
+    MAVSDK::mavsdk
+)
+
+if(NOT MSVC)
+    add_compile_options(camera_zoom PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(camera_zoom PRIVATE -WX -W2)
+endif()

examples/camera_zoom/camera_zoom.cpp

@@ -0,0 +1,129 @@
+//
+// Example to demonstrate how to switch to photo mode and take a picture.
+//
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/camera/camera.h>
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <future>
+#include <memory>
+#include <thread>
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::this_thread::sleep_for;
+
+void usage(std::string bin_name)
+{
+    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
+              << "Connection URL format should be :\n"
+              << " For TCP : tcp://[server_host][:server_port]\n"
+              << " For UDP : udp://[bind_host][:bind_port]\n"
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
+              << "For example, to connect to the simulator use URL: udp://:14540\n";
+}
+
+int main(int argc, char** argv)
+{
+    if (argc != 2) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
+    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    std::cout << "Waiting to discover system...\n";
+    auto prom = std::promise<std::shared_ptr<System>>{};
+    auto fut = prom.get_future();
+
+    // We wait for new systems to be discovered, once we find one that has a
+    // camera, we decide to use it.
+    Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&mavsdk, &prom, &handle]() {
+        auto system = mavsdk.systems().back();
+
+        // TODO: filter for camera
+        // if (system->has_camera()) {
+        std::cout << "Discovered camera\n";
+
+        // Unsubscribe again as we only want to find one system.
+        mavsdk.unsubscribe_on_new_system(handle);
+        prom.set_value(system);
+        //}
+    });
+
+    // We usually receive heartbeats at 1Hz, therefore we should find a
+    // system after around 3 seconds max, surely.
+    if (fut.wait_for(seconds(5)) == std::future_status::timeout) {
+        std::cerr << "No camera found, exiting.\n";
+        return 1;
+    }
+
+    // Get discovered system now.
+    auto system = fut.get();
+
+    // Instantiate plugins.
+    auto camera = Camera{system};
+
+    // Zoom in
+    auto result = camera.zoom_range(2.0f);
+    if (result != Camera::Result::Success) {
+        std::cerr << "Zooming failed: " << result << '\n';
+        return 1;
+    }
+
+    // Wait a bit
+    sleep_for(seconds(4));
+
+    // Now try continuous zooming in.
+    result = camera.zoom_in_start();
+    if (result != Camera::Result::Success) {
+        std::cerr << "Zooming in failed: " << result << '\n';
+        return 1;
+    }
+
+    sleep_for(seconds(1));
+
+    // Now try stopping.
+    result = camera.zoom_stop();
+    if (result != Camera::Result::Success) {
+        std::cerr << "Stop zooming failed: " << result << '\n';
+        return 1;
+    }
+
+    sleep_for(seconds(1));
+
+    // And back out.
+    result = camera.zoom_out_start();
+    if (result != Camera::Result::Success) {
+        std::cerr << "Zooming out failed: " << result << '\n';
+        return 1;
+    }
+
+    sleep_for(seconds(1));
+
+    // Stop again.
+    result = camera.zoom_stop();
+    if (result != Camera::Result::Success) {
+        std::cerr << "Stop zooming failed: " << result << '\n';
+        return 1;
+    }
+
+    sleep_for(seconds(1));
+
+    // Zoom back out
+    result = camera.zoom_range(1.0f);
+    if (result != Camera::Result::Success) {
+        std::cerr << "Zooming failed: " << result << '\n';
+        return 1;
+    }
+
+    return 0;
+}

src/mavsdk/plugins/camera/camera_impl.cpp

@@ -1071,6 +1071,8 @@ CameraImpl::camera_result_from_command_result(const MavlinkCommandSender::Result
             // FALLTHROUGH
         case MavlinkCommandSender::Result::Busy:
             return Camera::Result::Error;
+        case MavlinkCommandSender::Result::Failed:
+            return Camera::Result::Error;
         case MavlinkCommandSender::Result::Denied:
             // FALLTHROUGH
         case MavlinkCommandSender::Result::TemporarilyRejected:

src/mavsdk/plugins/camera_server/camera_server.cpp

@@ -205,6 +205,70 @@ CameraServer::respond_reset_settings(CameraFeedback reset_settings_feedback) con
     return _impl->respond_reset_settings(reset_settings_feedback);
 }
 
+CameraServer::ZoomInStartHandle
+CameraServer::subscribe_zoom_in_start(const ZoomInStartCallback& callback)
+{
+    return _impl->subscribe_zoom_in_start(callback);
+}
+
+void CameraServer::unsubscribe_zoom_in_start(ZoomInStartHandle handle)
+{
+    _impl->unsubscribe_zoom_in_start(handle);
+}
+
+CameraServer::Result
+CameraServer::respond_zoom_in_start(CameraFeedback zoom_in_start_feedback) const
+{
+    return _impl->respond_zoom_in_start(zoom_in_start_feedback);
+}
+
+CameraServer::ZoomOutStartHandle
+CameraServer::subscribe_zoom_out_start(const ZoomOutStartCallback& callback)
+{
+    return _impl->subscribe_zoom_out_start(callback);
+}
+
+void CameraServer::unsubscribe_zoom_out_start(ZoomOutStartHandle handle)
+{
+    _impl->unsubscribe_zoom_out_start(handle);
+}
+
+CameraServer::Result
+CameraServer::respond_zoom_out_start(CameraFeedback zoom_out_start_feedback) const
+{
+    return _impl->respond_zoom_out_start(zoom_out_start_feedback);
+}
+
+CameraServer::ZoomStopHandle CameraServer::subscribe_zoom_stop(const ZoomStopCallback& callback)
+{
+    return _impl->subscribe_zoom_stop(callback);
+}
+
+void CameraServer::unsubscribe_zoom_stop(ZoomStopHandle handle)
+{
+    _impl->unsubscribe_zoom_stop(handle);
+}
+
+CameraServer::Result CameraServer::respond_zoom_stop(CameraFeedback zoom_stop_feedback) const
+{
+    return _impl->respond_zoom_stop(zoom_stop_feedback);
+}
+
+CameraServer::ZoomRangeHandle CameraServer::subscribe_zoom_range(const ZoomRangeCallback& callback)
+{
+    return _impl->subscribe_zoom_range(callback);
+}
+
+void CameraServer::unsubscribe_zoom_range(ZoomRangeHandle handle)
+{
+    _impl->unsubscribe_zoom_range(handle);
+}
+
+CameraServer::Result CameraServer::respond_zoom_range(CameraFeedback zoom_range_feedback) const
+{
+    return _impl->respond_zoom_range(zoom_range_feedback);
+}
+
 bool operator==(const CameraServer::Information& lhs, const CameraServer::Information& rhs)
 {
     return (rhs.vendor_name == lhs.vendor_name) && (rhs.model_name == lhs.model_name) &&