camera_server: Add CAMERA_FOV_STATUS request support

cpp/src/mavsdk/plugins/camera_server/camera_server.cpp

@@ -332,6 +332,27 @@ CameraServer::respond_tracking_off_command(CameraFeedback stop_video_feedback) c
     return _impl->respond_tracking_off_command(stop_video_feedback);
 }
 
+CameraServer::Result CameraServer::set_position(Position position) const
+{
+    return _impl->set_position(position);
+}
+
+CameraServer::Result CameraServer::set_attitude_quaternion(Quaternion attitude_quaternion) const
+{
+    return _impl->set_attitude_quaternion(attitude_quaternion);
+}
+
+CameraServer::Result CameraServer::set_zoom_factor(float zoom_factor) const
+{
+    return _impl->set_zoom_factor(zoom_factor);
+}
+
+CameraServer::Result
+CameraServer::set_field_of_view(float horizontal_fov_deg, float vertical_fov_deg) const
+{
+    return _impl->set_field_of_view(horizontal_fov_deg, vertical_fov_deg);
+}
+
 MAVSDK_PUBLIC bool
 operator==(const CameraServer::Information& lhs, const CameraServer::Information& rhs)
 {

cpp/src/mavsdk/plugins/camera_server/camera_server_impl.cpp

@@ -1142,6 +1142,9 @@ CameraServerImpl::process_request_message(const MavlinkCommandReceiver::CommandL
             return _server_component_impl->make_command_ack_message(
                 command, MAV_RESULT::MAV_RESULT_ACCEPTED);
 
+        case MAVLINK_MSG_ID_CAMERA_FOV_STATUS:
+            return send_fov_status(command);
+
         default:
             LogWarn("Got unknown request message!");
             return _server_component_impl->make_command_ack_message(
@@ -1428,6 +1431,136 @@ void CameraServerImpl::send_capture_status()
     });
 }
 
+CameraServer::Result CameraServerImpl::set_position(CameraServer::Position position)
+{
+    std::lock_guard<std::mutex> lg{_mutex};
+    _position = position;
+    _is_position_set = true;
+    return CameraServer::Result::Success;
+}
+
+CameraServer::Result
+CameraServerImpl::set_attitude_quaternion(CameraServer::Quaternion attitude_quaternion)
+{
+    std::lock_guard<std::mutex> lg{_mutex};
+    _attitude_quaternion = attitude_quaternion;
+    _is_attitude_quaternion_set = true;
+    return CameraServer::Result::Success;
+}
+
+CameraServer::Result CameraServerImpl::set_zoom_factor(float zoom_factor)
+{
+    if (!std::isfinite(zoom_factor) || zoom_factor < 1.0f) {
+        LogWarn("Invalid zoom factor: {}", zoom_factor);
+        return CameraServer::Result::WrongArgument;
+    }
+    std::lock_guard<std::mutex> lg{_mutex};
+    _zoom_factor = zoom_factor;
+    _is_zoom_factor_set = true;
+    return CameraServer::Result::Success;
+}
+
+CameraServer::Result
+CameraServerImpl::set_field_of_view(float horizontal_fov_deg, float vertical_fov_deg)
+{
+    if (!std::isfinite(horizontal_fov_deg) || horizontal_fov_deg <= 0.0f ||
+        horizontal_fov_deg >= 180.0f) {
+        LogWarn("Invalid horizontal FOV: {}", horizontal_fov_deg);
+        return CameraServer::Result::WrongArgument;
+    }
+
+    if (!std::isfinite(vertical_fov_deg) || vertical_fov_deg <= 0.0f ||
+        vertical_fov_deg >= 180.0f) {
+        LogWarn("Invalid vertical FOV: {}", vertical_fov_deg);
+        return CameraServer::Result::WrongArgument;
+    }
+
+    std::lock_guard<std::mutex> lg{_mutex};
+    _horizontal_fov_deg = horizontal_fov_deg;
+    _vertical_fov_deg = vertical_fov_deg;
+    _is_fov_set = true;
+    return CameraServer::Result::Success;
+}
+
+std::optional<mavlink_command_ack_t>
+CameraServerImpl::send_fov_status(const MavlinkCommandReceiver::CommandLong& command)
+{
+    std::lock_guard<std::mutex> lg{_mutex};
+
+    if (!_is_information_set) {
+        return _server_component_impl->make_command_ack_message(
+            command, MAV_RESULT::MAV_RESULT_TEMPORARILY_REJECTED);
+    }
+
+    float hfov_deg;
+    float vfov_deg;
+    if (_is_zoom_factor_set) {
+        const float focal_length_mm = _information.focal_length_mm * _zoom_factor;
+        hfov_deg = 2.0f *
+                   std::atan((_information.horizontal_sensor_size_mm / 2.0f) / focal_length_mm) *
+                   (180.0f / static_cast<float>(M_PI));
+        vfov_deg = 2.0f *
+                   std::atan((_information.vertical_sensor_size_mm / 2.0f) / focal_length_mm) *
+                   (180.0f / static_cast<float>(M_PI));
+    } else if (_is_fov_set) {
+        hfov_deg = _horizontal_fov_deg;
+        vfov_deg = _vertical_fov_deg;
+    } else {
+        // Fall back to base FOV derived from camera information (no zoom applied)
+        hfov_deg =
+            2.0f *
+            std::atan(
+                (_information.horizontal_sensor_size_mm / 2.0f) / _information.focal_length_mm) *
+            (180.0f / static_cast<float>(M_PI));
+        vfov_deg =
+            2.0f *
+            std::atan(
+                (_information.vertical_sensor_size_mm / 2.0f) / _information.focal_length_mm) *
+            (180.0f / static_cast<float>(M_PI));
+    }
+
+    const int32_t lat_camera =
+        _is_position_set ? static_cast<int32_t>(_position.latitude_deg * 1e7) : INT32_MAX;
+    const int32_t lon_camera =
+        _is_position_set ? static_cast<int32_t>(_position.longitude_deg * 1e7) : INT32_MAX;
+    const int32_t alt_camera =
+        _is_position_set ? static_cast<int32_t>(_position.absolute_altitude_m * 1e3) : INT32_MAX;
+
+    const float q[4] = {
+        _is_attitude_quaternion_set ? _attitude_quaternion.w : 1.0f,
+        _is_attitude_quaternion_set ? _attitude_quaternion.x : 0.0f,
+        _is_attitude_quaternion_set ? _attitude_quaternion.y : 0.0f,
+        _is_attitude_quaternion_set ? _attitude_quaternion.z : 0.0f,
+    };
+
+    auto ack =
+        _server_component_impl->make_command_ack_message(command, MAV_RESULT::MAV_RESULT_ACCEPTED);
+    _server_component_impl->send_command_ack(ack);
+
+    _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
+        mavlink_message_t message{};
+        mavlink_msg_camera_fov_status_pack_chan(
+            mavlink_address.system_id,
+            mavlink_address.component_id,
+            channel,
+            &message,
+            static_cast<uint32_t>(_server_component_impl->get_time().elapsed_s() * 1e3),
+            lat_camera,
+            lon_camera,
+            alt_camera,
+            INT32_MAX, // lat_image: unknown, requires terrain intersection
+            INT32_MAX, // lon_image: unknown, requires terrain intersection
+            INT32_MAX, // alt_image: unknown, requires terrain intersection
+            q,
+            hfov_deg,
+            vfov_deg,
+            0 /* camera_device_id: 0 = MAVLink camera */);
+        return message;
+    });
+
+    return std::nullopt; // ACK already sent
+}
+
 std::optional<mavlink_command_ack_t>
 CameraServerImpl::process_reset_camera_settings(const MavlinkCommandReceiver::CommandLong& command)
 {

cpp/src/mavsdk/plugins/camera_server/camera_server_impl.hpp

@@ -24,6 +24,10 @@ class CameraServerImpl : public ServerPluginImplBase {
     CameraServer::Result set_information(CameraServer::Information information);
     CameraServer::Result set_video_streaming(CameraServer::VideoStreaming video_streaming);
     CameraServer::Result set_in_progress(bool in_progress);
+    CameraServer::Result set_position(CameraServer::Position position);
+    CameraServer::Result set_attitude_quaternion(CameraServer::Quaternion attitude_quaternion);
+    CameraServer::Result set_zoom_factor(float zoom_factor);
+    CameraServer::Result set_field_of_view(float horizontal_fov_deg, float vertical_fov_deg);
 
     CameraServer::TakePhotoHandle
     subscribe_take_photo(const CameraServer::TakePhotoCallback& callback);
@@ -203,7 +207,14 @@ class CameraServerImpl : public ServerPluginImplBase {
 
     void send_capture_status();
 
+    std::optional<mavlink_command_ack_t>
+    send_fov_status(const MavlinkCommandReceiver::CommandLong& command);
+
     bool _is_information_set{};
+    bool _is_position_set{};
+    bool _is_attitude_quaternion_set{};
+    bool _is_zoom_factor_set{};
+    bool _is_fov_set{};
 
     std::mutex _mutex{};
 
@@ -219,6 +230,11 @@ class CameraServerImpl : public ServerPluginImplBase {
     CallEveryHandler::Cookie _capture_status_timer_cookie{};
 
     CameraServer::Information _information{};
+    CameraServer::Position _position{};
+    CameraServer::Quaternion _attitude_quaternion{};
+    float _zoom_factor{};
+    float _horizontal_fov_deg{};
+    float _vertical_fov_deg{};
     bool _is_video_streaming_set{};
     CameraServer::VideoStreaming _video_streaming{};
 

cpp/src/mavsdk/plugins/camera_server/include/plugins/camera_server/camera_server.hpp

@@ -1355,6 +1355,70 @@ class MAVSDK_PUBLIC CameraServer : public ServerPluginBase {
 
 
 
+
+
+    /**
+     * @brief Set the camera's GPS position.
+     *
+     * This function is blocking.
+     *
+
+     * @return Result of request.
+
+     */
+    Result set_position(Position position) const;
+
+
+
+
+
+
+    /**
+     * @brief Set the camera's attitude quaternion.
+     *
+     * This function is blocking.
+     *
+
+     * @return Result of request.
+
+     */
+    Result set_attitude_quaternion(Quaternion attitude_quaternion) const;
+
+
+
+
+
+
+    /**
+     * @brief Set the camera's zoom factor for CAMERA_FOV_STATUS reporting.
+     *
+     * This function is blocking.
+     *
+
+     * @return Result of request.
+
+     */
+    Result set_zoom_factor(float zoom_factor) const;
+
+
+
+
+
+
+    /**
+     * @brief Set the field of view explicitly, for cameras that do not report a zoom factor.
+     *
+     * This function is blocking.
+     *
+
+     * @return Result of request.
+
+     */
+    Result set_field_of_view(float horizontal_fov_deg, float vertical_fov_deg) const;
+
+
+
+
     /**
      * @brief Copy constructor.
      */