diff --git a/proto b/proto index 4e59e5f91..11be84651 160000 --- a/proto +++ b/proto @@ -1 +1 @@ -Subproject commit 4e59e5f9182d6a3b0e61c0a54ab8e4ff6fcf67af +Subproject commit 11be846515046931953119ea17d78dbc066ea860 diff --git a/src/mavsdk/plugins/camera/camera.cpp b/src/mavsdk/plugins/camera/camera.cpp index d1bc11b86..e29ff39f7 100644 --- a/src/mavsdk/plugins/camera/camera.cpp +++ b/src/mavsdk/plugins/camera/camera.cpp @@ -9,18 +9,21 @@ namespace mavsdk { -using ModeInfo = Camera::ModeInfo; +using Option = Camera::Option; +using Setting = Camera::Setting; +using SettingOptions = Camera::SettingOptions; +using VideoStreamSettings = Camera::VideoStreamSettings; +using VideoStreamInfo = Camera::VideoStreamInfo; +using ModeUpdate = Camera::ModeUpdate; +using VideoStreamUpdate = Camera::VideoStreamUpdate; +using CurrentSettingsUpdate = Camera::CurrentSettingsUpdate; +using PossibleSettingOptionsUpdate = Camera::PossibleSettingOptionsUpdate; using Position = Camera::Position; using Quaternion = Camera::Quaternion; using EulerAngle = Camera::EulerAngle; using CaptureInfo = Camera::CaptureInfo; -using VideoStreamSettings = Camera::VideoStreamSettings; -using VideoStreamInfo = Camera::VideoStreamInfo; using Status = Camera::Status; -using Option = Camera::Option; -using Setting = Camera::Setting; -using SettingOptions = Camera::SettingOptions; using Information = Camera::Information; using CameraList = Camera::CameraList; @@ -141,7 +144,7 @@ void Camera::unsubscribe_mode(ModeHandle handle) _impl->unsubscribe_mode(handle); } -Camera::Mode Camera::get_mode(int32_t camera_id) const +std::pair Camera::get_mode(int32_t camera_id) const { return _impl->get_mode(camera_id); } @@ -157,7 +160,8 @@ void Camera::unsubscribe_video_stream_info(VideoStreamInfoHandle handle) _impl->unsubscribe_video_stream_info(handle); } -Camera::VideoStreamInfo Camera::get_video_stream_info(int32_t camera_id) const +std::pair +Camera::get_video_stream_info(int32_t camera_id) const { return _impl->get_video_stream_info(camera_id); } @@ -182,7 +186,7 @@ void Camera::unsubscribe_status(StatusHandle handle) _impl->unsubscribe_status(handle); } -Camera::Status Camera::get_status(int32_t camera_id) const +std::pair Camera::get_status(int32_t camera_id) const { return _impl->get_status(camera_id); } @@ -389,17 +393,210 @@ Camera::Result Camera::focus_range(int32_t camera_id, float range) const return _impl->focus_range(camera_id, range); } -bool operator==(const Camera::ModeInfo& lhs, const Camera::ModeInfo& rhs) +bool operator==(const Camera::Option& lhs, const Camera::Option& rhs) +{ + return (rhs.option_id == lhs.option_id) && (rhs.option_description == lhs.option_description); +} + +std::ostream& operator<<(std::ostream& str, Camera::Option const& option) +{ + str << std::setprecision(15); + str << "option:" << '\n' << "{\n"; + str << " option_id: " << option.option_id << '\n'; + str << " option_description: " << option.option_description << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::Setting& lhs, const Camera::Setting& rhs) +{ + return (rhs.setting_id == lhs.setting_id) && + (rhs.setting_description == lhs.setting_description) && (rhs.option == lhs.option) && + (rhs.is_range == lhs.is_range); +} + +std::ostream& operator<<(std::ostream& str, Camera::Setting const& setting) +{ + str << std::setprecision(15); + str << "setting:" << '\n' << "{\n"; + str << " setting_id: " << setting.setting_id << '\n'; + str << " setting_description: " << setting.setting_description << '\n'; + str << " option: " << setting.option << '\n'; + str << " is_range: " << setting.is_range << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::SettingOptions& lhs, const Camera::SettingOptions& rhs) +{ + return (rhs.camera_id == lhs.camera_id) && (rhs.setting_id == lhs.setting_id) && + (rhs.setting_description == lhs.setting_description) && (rhs.options == lhs.options) && + (rhs.is_range == lhs.is_range); +} + +std::ostream& operator<<(std::ostream& str, Camera::SettingOptions const& setting_options) +{ + str << std::setprecision(15); + str << "setting_options:" << '\n' << "{\n"; + str << " camera_id: " << setting_options.camera_id << '\n'; + str << " setting_id: " << setting_options.setting_id << '\n'; + str << " setting_description: " << setting_options.setting_description << '\n'; + str << " options: ["; + for (auto it = setting_options.options.begin(); it != setting_options.options.end(); ++it) { + str << *it; + str << (it + 1 != setting_options.options.end() ? ", " : "]\n"); + } + str << " is_range: " << setting_options.is_range << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::VideoStreamSettings& lhs, const Camera::VideoStreamSettings& rhs) +{ + return ((std::isnan(rhs.frame_rate_hz) && std::isnan(lhs.frame_rate_hz)) || + rhs.frame_rate_hz == lhs.frame_rate_hz) && + (rhs.horizontal_resolution_pix == lhs.horizontal_resolution_pix) && + (rhs.vertical_resolution_pix == lhs.vertical_resolution_pix) && + (rhs.bit_rate_b_s == lhs.bit_rate_b_s) && (rhs.rotation_deg == lhs.rotation_deg) && + (rhs.uri == lhs.uri) && + ((std::isnan(rhs.horizontal_fov_deg) && std::isnan(lhs.horizontal_fov_deg)) || + rhs.horizontal_fov_deg == lhs.horizontal_fov_deg); +} + +std::ostream& +operator<<(std::ostream& str, Camera::VideoStreamSettings const& video_stream_settings) +{ + str << std::setprecision(15); + str << "video_stream_settings:" << '\n' << "{\n"; + str << " frame_rate_hz: " << video_stream_settings.frame_rate_hz << '\n'; + str << " horizontal_resolution_pix: " << video_stream_settings.horizontal_resolution_pix + << '\n'; + str << " vertical_resolution_pix: " << video_stream_settings.vertical_resolution_pix << '\n'; + str << " bit_rate_b_s: " << video_stream_settings.bit_rate_b_s << '\n'; + str << " rotation_deg: " << video_stream_settings.rotation_deg << '\n'; + str << " uri: " << video_stream_settings.uri << '\n'; + str << " horizontal_fov_deg: " << video_stream_settings.horizontal_fov_deg << '\n'; + str << '}'; + return str; +} + +std::ostream& +operator<<(std::ostream& str, Camera::VideoStreamInfo::VideoStreamStatus const& video_stream_status) +{ + switch (video_stream_status) { + case Camera::VideoStreamInfo::VideoStreamStatus::NotRunning: + return str << "Not Running"; + case Camera::VideoStreamInfo::VideoStreamStatus::InProgress: + return str << "In Progress"; + default: + return str << "Unknown"; + } +} + +std::ostream& operator<<( + std::ostream& str, Camera::VideoStreamInfo::VideoStreamSpectrum const& video_stream_spectrum) +{ + switch (video_stream_spectrum) { + case Camera::VideoStreamInfo::VideoStreamSpectrum::Unknown: + return str << "Unknown"; + case Camera::VideoStreamInfo::VideoStreamSpectrum::VisibleLight: + return str << "Visible Light"; + case Camera::VideoStreamInfo::VideoStreamSpectrum::Infrared: + return str << "Infrared"; + default: + return str << "Unknown"; + } +} +bool operator==(const Camera::VideoStreamInfo& lhs, const Camera::VideoStreamInfo& rhs) +{ + return (rhs.camera_id == lhs.camera_id) && (rhs.settings == lhs.settings) && + (rhs.status == lhs.status) && (rhs.spectrum == lhs.spectrum); +} + +std::ostream& operator<<(std::ostream& str, Camera::VideoStreamInfo const& video_stream_info) +{ + str << std::setprecision(15); + str << "video_stream_info:" << '\n' << "{\n"; + str << " camera_id: " << video_stream_info.camera_id << '\n'; + str << " settings: " << video_stream_info.settings << '\n'; + str << " status: " << video_stream_info.status << '\n'; + str << " spectrum: " << video_stream_info.spectrum << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::ModeUpdate& lhs, const Camera::ModeUpdate& rhs) { return (rhs.camera_id == lhs.camera_id) && (rhs.mode == lhs.mode); } -std::ostream& operator<<(std::ostream& str, Camera::ModeInfo const& mode_info) +std::ostream& operator<<(std::ostream& str, Camera::ModeUpdate const& mode_update) { str << std::setprecision(15); - str << "mode_info:" << '\n' << "{\n"; - str << " camera_id: " << mode_info.camera_id << '\n'; - str << " mode: " << mode_info.mode << '\n'; + str << "mode_update:" << '\n' << "{\n"; + str << " camera_id: " << mode_update.camera_id << '\n'; + str << " mode: " << mode_update.mode << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::VideoStreamUpdate& lhs, const Camera::VideoStreamUpdate& rhs) +{ + return (rhs.camera_id == lhs.camera_id) && (rhs.video_stream_info == lhs.video_stream_info); +} + +std::ostream& operator<<(std::ostream& str, Camera::VideoStreamUpdate const& video_stream_update) +{ + str << std::setprecision(15); + str << "video_stream_update:" << '\n' << "{\n"; + str << " camera_id: " << video_stream_update.camera_id << '\n'; + str << " video_stream_info: " << video_stream_update.video_stream_info << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::CurrentSettingsUpdate& lhs, const Camera::CurrentSettingsUpdate& rhs) +{ + return (rhs.camera_id == lhs.camera_id) && (rhs.current_settings == lhs.current_settings); +} + +std::ostream& +operator<<(std::ostream& str, Camera::CurrentSettingsUpdate const& current_settings_update) +{ + str << std::setprecision(15); + str << "current_settings_update:" << '\n' << "{\n"; + str << " camera_id: " << current_settings_update.camera_id << '\n'; + str << " current_settings: ["; + for (auto it = current_settings_update.current_settings.begin(); + it != current_settings_update.current_settings.end(); + ++it) { + str << *it; + str << (it + 1 != current_settings_update.current_settings.end() ? ", " : "]\n"); + } + str << '}'; + return str; +} + +bool operator==( + const Camera::PossibleSettingOptionsUpdate& lhs, + const Camera::PossibleSettingOptionsUpdate& rhs) +{ + return (rhs.camera_id == lhs.camera_id) && (rhs.setting_options == lhs.setting_options); +} + +std::ostream& operator<<( + std::ostream& str, Camera::PossibleSettingOptionsUpdate const& possible_setting_options_update) +{ + str << std::setprecision(15); + str << "possible_setting_options_update:" << '\n' << "{\n"; + str << " camera_id: " << possible_setting_options_update.camera_id << '\n'; + str << " setting_options: ["; + for (auto it = possible_setting_options_update.setting_options.begin(); + it != possible_setting_options_update.setting_options.end(); + ++it) { + str << *it; + str << (it + 1 != possible_setting_options_update.setting_options.end() ? ", " : "]\n"); + } str << '}'; return str; } @@ -527,80 +724,6 @@ std::ostream& operator<<(std::ostream& str, Camera::CaptureInfo const& capture_i return str; } -bool operator==(const Camera::VideoStreamSettings& lhs, const Camera::VideoStreamSettings& rhs) -{ - return ((std::isnan(rhs.frame_rate_hz) && std::isnan(lhs.frame_rate_hz)) || - rhs.frame_rate_hz == lhs.frame_rate_hz) && - (rhs.horizontal_resolution_pix == lhs.horizontal_resolution_pix) && - (rhs.vertical_resolution_pix == lhs.vertical_resolution_pix) && - (rhs.bit_rate_b_s == lhs.bit_rate_b_s) && (rhs.rotation_deg == lhs.rotation_deg) && - (rhs.uri == lhs.uri) && - ((std::isnan(rhs.horizontal_fov_deg) && std::isnan(lhs.horizontal_fov_deg)) || - rhs.horizontal_fov_deg == lhs.horizontal_fov_deg); -} - -std::ostream& -operator<<(std::ostream& str, Camera::VideoStreamSettings const& video_stream_settings) -{ - str << std::setprecision(15); - str << "video_stream_settings:" << '\n' << "{\n"; - str << " frame_rate_hz: " << video_stream_settings.frame_rate_hz << '\n'; - str << " horizontal_resolution_pix: " << video_stream_settings.horizontal_resolution_pix - << '\n'; - str << " vertical_resolution_pix: " << video_stream_settings.vertical_resolution_pix << '\n'; - str << " bit_rate_b_s: " << video_stream_settings.bit_rate_b_s << '\n'; - str << " rotation_deg: " << video_stream_settings.rotation_deg << '\n'; - str << " uri: " << video_stream_settings.uri << '\n'; - str << " horizontal_fov_deg: " << video_stream_settings.horizontal_fov_deg << '\n'; - str << '}'; - return str; -} - -std::ostream& -operator<<(std::ostream& str, Camera::VideoStreamInfo::VideoStreamStatus const& video_stream_status) -{ - switch (video_stream_status) { - case Camera::VideoStreamInfo::VideoStreamStatus::NotRunning: - return str << "Not Running"; - case Camera::VideoStreamInfo::VideoStreamStatus::InProgress: - return str << "In Progress"; - default: - return str << "Unknown"; - } -} - -std::ostream& operator<<( - std::ostream& str, Camera::VideoStreamInfo::VideoStreamSpectrum const& video_stream_spectrum) -{ - switch (video_stream_spectrum) { - case Camera::VideoStreamInfo::VideoStreamSpectrum::Unknown: - return str << "Unknown"; - case Camera::VideoStreamInfo::VideoStreamSpectrum::VisibleLight: - return str << "Visible Light"; - case Camera::VideoStreamInfo::VideoStreamSpectrum::Infrared: - return str << "Infrared"; - default: - return str << "Unknown"; - } -} -bool operator==(const Camera::VideoStreamInfo& lhs, const Camera::VideoStreamInfo& rhs) -{ - return (rhs.camera_id == lhs.camera_id) && (rhs.settings == lhs.settings) && - (rhs.status == lhs.status) && (rhs.spectrum == lhs.spectrum); -} - -std::ostream& operator<<(std::ostream& str, Camera::VideoStreamInfo const& video_stream_info) -{ - str << std::setprecision(15); - str << "video_stream_info:" << '\n' << "{\n"; - str << " camera_id: " << video_stream_info.camera_id << '\n'; - str << " settings: " << video_stream_info.settings << '\n'; - str << " status: " << video_stream_info.status << '\n'; - str << " spectrum: " << video_stream_info.spectrum << '\n'; - str << '}'; - return str; -} - std::ostream& operator<<(std::ostream& str, Camera::Status::StorageStatus const& storage_status) { switch (storage_status) { @@ -672,64 +795,6 @@ std::ostream& operator<<(std::ostream& str, Camera::Status const& status) return str; } -bool operator==(const Camera::Option& lhs, const Camera::Option& rhs) -{ - return (rhs.option_id == lhs.option_id) && (rhs.option_description == lhs.option_description); -} - -std::ostream& operator<<(std::ostream& str, Camera::Option const& option) -{ - str << std::setprecision(15); - str << "option:" << '\n' << "{\n"; - str << " option_id: " << option.option_id << '\n'; - str << " option_description: " << option.option_description << '\n'; - str << '}'; - return str; -} - -bool operator==(const Camera::Setting& lhs, const Camera::Setting& rhs) -{ - return (rhs.setting_id == lhs.setting_id) && - (rhs.setting_description == lhs.setting_description) && (rhs.option == lhs.option) && - (rhs.is_range == lhs.is_range); -} - -std::ostream& operator<<(std::ostream& str, Camera::Setting const& setting) -{ - str << std::setprecision(15); - str << "setting:" << '\n' << "{\n"; - str << " setting_id: " << setting.setting_id << '\n'; - str << " setting_description: " << setting.setting_description << '\n'; - str << " option: " << setting.option << '\n'; - str << " is_range: " << setting.is_range << '\n'; - str << '}'; - return str; -} - -bool operator==(const Camera::SettingOptions& lhs, const Camera::SettingOptions& rhs) -{ - return (rhs.camera_id == lhs.camera_id) && (rhs.setting_id == lhs.setting_id) && - (rhs.setting_description == lhs.setting_description) && (rhs.options == lhs.options) && - (rhs.is_range == lhs.is_range); -} - -std::ostream& operator<<(std::ostream& str, Camera::SettingOptions const& setting_options) -{ - str << std::setprecision(15); - str << "setting_options:" << '\n' << "{\n"; - str << " camera_id: " << setting_options.camera_id << '\n'; - str << " setting_id: " << setting_options.setting_id << '\n'; - str << " setting_description: " << setting_options.setting_description << '\n'; - str << " options: ["; - for (auto it = setting_options.options.begin(); it != setting_options.options.end(); ++it) { - str << *it; - str << (it + 1 != setting_options.options.end() ? ", " : "]\n"); - } - str << " is_range: " << setting_options.is_range << '\n'; - str << '}'; - return str; -} - bool operator==(const Camera::Information& lhs, const Camera::Information& rhs) { return (rhs.vendor_name == lhs.vendor_name) && (rhs.model_name == lhs.model_name) && diff --git a/src/mavsdk/plugins/camera/camera_impl.cpp b/src/mavsdk/plugins/camera/camera_impl.cpp index be65536b9..6bbb9829e 100644 --- a/src/mavsdk/plugins/camera/camera_impl.cpp +++ b/src/mavsdk/plugins/camera/camera_impl.cpp @@ -108,55 +108,29 @@ void CameraImpl::init() //_request_missing_capture_info_cookie = // _system_impl->add_call_every([this]() { request_missing_capture_info(); }, 0.5); + _request_slower_call_every_cookie = + _system_impl->add_call_every([this]() { request_slower(); }, 20.0); + _request_faster_call_every_cookie = + _system_impl->add_call_every([this]() { request_faster(); }, 5.0); } void CameraImpl::deinit() { _system_impl->unregister_all_mavlink_message_handlers(this); + _system_impl->cancel_all_param(this); + std::lock_guard lock(_mutex); //_system_impl->remove_call_every(_request_missing_capture_info_cookie); - //_system_impl->remove_call_every(_status.call_every_cookie); - //_system_impl->remove_call_every(_camera_information_call_every_cookie); - //_system_impl->remove_call_every(_mode.call_every_cookie); - //_system_impl->remove_call_every(_video_stream_info.call_every_cookie); - //_system_impl->cancel_all_param(this); - - //{ - // std::lock_guard lock(_status.mutex); - // _status.subscription_callbacks.clear(); - //} - - //{ - // std::lock_guard lock(_mode.mutex); - // _mode.subscription_callbacks.clear(); - //} - - //{ - // std::lock_guard lock(_capture_info.mutex); - // _capture_info.callbacks.clear(); - //} - - //{ - // std::lock_guard lock(_video_stream_info.mutex); - // _video_stream_info.subscription_callbacks.clear(); - //} - - //{ - // std::lock_guard lock(_camera_list.mutex); - // _camera_list.subscription_callbacks.clear(); - //} - - //{ - // std::lock_guard lock(_subscribe_current_settings.mutex); - // _subscribe_current_settings.callbacks.clear(); - //} - - //{ - // std::lock_guard lock(_subscribe_possible_setting_options.mutex); - // _subscribe_possible_setting_options.callbacks.clear(); - //} + _system_impl->remove_call_every(_request_slower_call_every_cookie); + _system_impl->remove_call_every(_request_faster_call_every_cookie); - //_camera_found = false; + _status_subscription_callbacks.clear(); + _mode_subscription_callbacks.clear(); + _capture_info_callbacks.clear(); + _video_stream_info_subscription_callbacks.clear(); + _camera_list_subscription_callbacks.clear(); + _subscribe_current_settings_callbacks.clear(); + _subscribe_possible_setting_options_callbacks.clear(); } void CameraImpl::enable() {} @@ -492,7 +466,6 @@ Camera::Result CameraImpl::stop_photo_interval(int32_t camera_id) Camera::Result CameraImpl::start_video(int32_t camera_id) { - // TODO: check whether video capture is already in progress. // Capture status rate is not set auto cmd_start_video = make_command_start_video(camera_id, 0.f); @@ -503,11 +476,6 @@ Camera::Result CameraImpl::stop_video(int32_t camera_id) { auto cmd_stop_video = make_command_stop_video(camera_id); - { - std::lock_guard lock(_video_stream_info.mutex); - _video_stream_info.data.status = Camera::VideoStreamInfo::VideoStreamStatus::NotRunning; - } - return camera_result_from_command_result(_system_impl->send_command(cmd_stop_video)); } @@ -670,8 +638,6 @@ void CameraImpl::stop_photo_interval_async( void CameraImpl::start_video_async(int32_t camera_id, const Camera::ResultCallback& callback) { - // TODO: check whether video capture is already in progress. - // Capture status rate is not set auto cmd_start_video = make_command_start_video(camera_id, 0.f); @@ -710,104 +676,87 @@ Camera::CameraList CameraImpl::camera_list() Camera::CameraListHandle CameraImpl::subscribe_camera_list(const Camera::CameraListCallback& callback) { - std::lock_guard lock(_mutex); - auto handle = camera_list_subscription_callbacks.subscribe(callback); + std::lock_guard lock(_mutex); + auto handle = _camera_list_subscription_callbacks.subscribe(callback); - notify_camera_list(); + notify_camera_list_with_lock(); return handle; } void CameraImpl::unsubscribe_camera_list(Camera::CameraListHandle handle) { - std::lock_guard lock(_mutex); - camera_list_subscription_callbacks.unsubscribe(handle); + std::lock_guard lock(_mutex); + _camera_list_subscription_callbacks.unsubscribe(handle); } -void CameraImpl::notify_camera_list() +void CameraImpl::notify_camera_list_with_lock() { _system_impl->call_user_callback([&]() { - camera_list_subscription_callbacks.queue( + _camera_list_subscription_callbacks.queue( camera_list(), [this](const auto& func) { func(); }); }); } Camera::Result CameraImpl::start_video_streaming(int32_t camera_id, int32_t stream_id) { - std::lock_guard lock(_video_stream_info.mutex); - - if (_video_stream_info.available && - _video_stream_info.data.status == Camera::VideoStreamInfo::VideoStreamStatus::InProgress) { - return Camera::Result::InProgress; - } - - // TODO Check whether we're in video mode auto command = make_command_start_video_streaming(camera_id, stream_id); - auto result = camera_result_from_command_result(_system_impl->send_command(command)); - // if (result == Camera::Result::Success) { - // Cache video stream info; app may query immediately next. - // TODO: check if we can/should do that. - // auto info = get_video_stream_info(); - //} - return result; + return camera_result_from_command_result(_system_impl->send_command(command)); } Camera::Result CameraImpl::stop_video_streaming(int32_t camera_id, int32_t stream_id) { - // TODO I think we need to maintain current state, whether we issued - // video capture request or video streaming request, etc.We shouldn't - // send stop video streaming if we've not started it! auto command = make_command_stop_video_streaming(camera_id, stream_id); - auto result = camera_result_from_command_result(_system_impl->send_command(command)); - { - std::lock_guard lock(_video_stream_info.mutex); - // TODO: check if we can/should do that. - _video_stream_info.data.status = Camera::VideoStreamInfo::VideoStreamStatus::NotRunning; - } - return result; + return camera_result_from_command_result(_system_impl->send_command(command)); } -void CameraImpl::request_video_stream_info(int32_t camera_id) +void CameraImpl::request_slower() { - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, component_id_for_camera_id(camera_id), nullptr); - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, component_id_for_camera_id(camera_id), nullptr); + std::lock_guard lock(_mutex); + + 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), + nullptr); + } } -Camera::VideoStreamInfo CameraImpl::video_stream_info() +void CameraImpl::request_faster() { - std::lock_guard lock(_video_stream_info.mutex); + std::lock_guard lock(_mutex); + + for (auto& camera : _potential_cameras) { + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, camera.component_id, nullptr); + + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, camera.component_id, nullptr); + + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_STORAGE_INFORMATION, camera.component_id, nullptr); - return _video_stream_info.data; + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_CAMERA_SETTINGS, camera.component_id, nullptr); + } } Camera::VideoStreamInfoHandle CameraImpl::subscribe_video_stream_info(const Camera::VideoStreamInfoCallback& callback) { - std::lock_guard lock(_video_stream_info.mutex); - - auto handle = _video_stream_info.subscription_callbacks.subscribe(callback); + std::lock_guard lock(_mutex); - /* TODO: request for all - if (callback) { - _system_impl->remove_call_every(_video_stream_info.call_every_cookie); - _video_stream_info.call_every_cookie = - _system_impl->add_call_every([this]() { request_video_stream_info(camera_id); }, 1.0); - } else { - _system_impl->remove_call_every(_video_stream_info.call_every_cookie); - } - */ + auto handle = _video_stream_info_subscription_callbacks.subscribe(callback); return handle; } void CameraImpl::unsubscribe_video_stream_info(Camera::VideoStreamInfoHandle handle) { - std::lock_guard lock(_video_stream_info.mutex); - _video_stream_info.subscription_callbacks.unsubscribe(handle); + std::lock_guard lock(_mutex); + _video_stream_info_subscription_callbacks.unsubscribe(handle); } Camera::Result @@ -928,7 +877,7 @@ void CameraImpl::save_camera_mode_with_lock(PotentialCamera& potential_camera, C potential_camera.camera_definition->set_setting("CAM_MODE", value); refresh_params_with_lock(potential_camera); - notify_mode(); + notify_mode_with_lock(); } float CameraImpl::to_mavlink_camera_mode(const Camera::Mode mode) @@ -960,19 +909,18 @@ void CameraImpl::set_mode_async( Camera::ModeHandle CameraImpl::subscribe_mode(const Camera::ModeCallback& callback) { - std::unique_lock lock(_mode.mutex); - auto handle = _mode.subscription_callbacks.subscribe(callback); - lock.unlock(); + std::lock_guard lock(_mutex); + auto handle = _mode_subscription_callbacks.subscribe(callback); - // TODO: implement for all + notify_mode_with_lock(); return handle; } void CameraImpl::unsubscribe_mode(Camera::ModeHandle handle) { - std::lock_guard lock(_mode.mutex); - _mode.subscription_callbacks.unsubscribe(handle); + std::lock_guard lock(_mutex); + _mode_subscription_callbacks.unsubscribe(handle); } #if 0 @@ -987,29 +935,31 @@ void CameraImpl::request_status(int32_t camera_id) Camera::StatusHandle CameraImpl::subscribe_status(const Camera::StatusCallback& callback) { - std::lock_guard lock(_status.mutex); - auto handle = _status.subscription_callbacks.subscribe(callback); + std::lock_guard lock(_mutex); + auto handle = _status_subscription_callbacks.subscribe(callback); + + // TODO: notify? return handle; } void CameraImpl::unsubscribe_status(Camera::StatusHandle handle) { - std::lock_guard lock(_status.mutex); - _status.subscription_callbacks.unsubscribe(handle); + std::lock_guard lock(_mutex); + _status_subscription_callbacks.unsubscribe(handle); } Camera::CaptureInfoHandle CameraImpl::subscribe_capture_info(const Camera::CaptureInfoCallback& callback) { - std::lock_guard lock(_capture_info.mutex); - return _capture_info.callbacks.subscribe(callback); + std::lock_guard lock(_mutex); + return _capture_info_callbacks.subscribe(callback); } void CameraImpl::unsubscribe_capture_info(Camera::CaptureInfoHandle handle) { - std::lock_guard lock(_capture_info.mutex); - _capture_info.callbacks.unsubscribe(handle); + std::lock_guard lock(_mutex); + _capture_info_callbacks.unsubscribe(handle); } void CameraImpl::process_heartbeat(const mavlink_message_t& message) @@ -1022,8 +972,7 @@ void CameraImpl::process_heartbeat(const mavlink_message_t& message) }); if (!found) { - auto new_potential_camera = PotentialCamera{}; - new_potential_camera.component_id = message.compid; + auto new_potential_camera = PotentialCamera{message.compid}; _potential_cameras.emplace_back(std::move(new_potential_camera)); check_potential_cameras_with_lock(); } @@ -1045,30 +994,38 @@ void CameraImpl::process_camera_capture_status(const mavlink_message_t& message) mavlink_camera_capture_status_t camera_capture_status; mavlink_msg_camera_capture_status_decode(&message, &camera_capture_status); - // If image_count got smaller, consider that the storage was formatted. - if (camera_capture_status.image_count < _status.image_count) { - LogDebug() << "Seems like storage was formatted, setting state accordingly"; - reset_following_format_storage(); + std::lock_guard lock(_mutex); + auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(message.compid); + + if (maybe_potential_camera == nullptr) { + return; } - { - std::lock_guard lock(_status.mutex); + // auto& camera = *maybe_potential_camera; - _status.data.video_on = (camera_capture_status.video_status == 1); - _status.data.photo_interval_on = - (camera_capture_status.image_status == 2 || camera_capture_status.image_status == 3); - _status.received_camera_capture_status = true; - _status.data.recording_time_s = - static_cast(camera_capture_status.recording_time_ms) / 1e3f; + // If image_count got smaller, consider that the storage was formatted. + // if (camera_capture_status.image_count < camera.capture_status.image_count) { + // LogDebug() << "Seems like storage was formatted, setting state accordingly"; + // reset_following_format_storage_with_lock(camera); + //} - _status.image_count = camera_capture_status.image_count; + // TODO: enable again + //{ + // std::lock_guard lock(_status.mutex); - if (_status.image_count_at_connection == -1) { - _status.image_count_at_connection = camera_capture_status.image_count; - } - } + // _status.data.video_on = (camera_capture_status.video_status == 1); + // _status.data.photo_interval_on = + // (camera_capture_status.image_status == 2 || camera_capture_status.image_status == 3); + // _status.received_camera_capture_status = true; + // _status.data.recording_time_s = + // static_cast(camera_capture_status.recording_time_ms) / 1e3f; - check_status(); + // _status.image_count = camera_capture_status.image_count; + + // if (_status.image_count_at_connection == -1) { + // _status.image_count_at_connection = camera_capture_status.image_count; + // } + //} } void CameraImpl::process_storage_information(const mavlink_message_t& message) @@ -1088,16 +1045,17 @@ void CameraImpl::process_storage_information(const mavlink_message_t& message) return; } - { - std::lock_guard lock(_status.mutex); - _status.data.storage_status = storage_status_from_mavlink(storage_information.status); - _status.data.available_storage_mib = storage_information.available_capacity; - _status.data.used_storage_mib = storage_information.used_capacity; - _status.data.total_storage_mib = storage_information.total_capacity; - _status.data.storage_id = storage_information.storage_id; - _status.data.storage_type = storage_type_from_mavlink(storage_information.type); - _status.received_storage_information = true; - } + // TODO: enable again + //{ + // std::lock_guard lock(_status.mutex); + // _status.data.storage_status = storage_status_from_mavlink(storage_information.status); + // _status.data.available_storage_mib = storage_information.available_capacity; + // _status.data.used_storage_mib = storage_information.used_capacity; + // _status.data.total_storage_mib = storage_information.total_capacity; + // _status.data.storage_id = storage_information.storage_id; + // _status.data.storage_type = storage_type_from_mavlink(storage_information.type); + // _status.received_storage_information = true; + //} check_status(); } @@ -1146,6 +1104,9 @@ 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 + +#if 0 { mavsdk::Quaternion quaternion{}; quaternion.w = image_captured.q[0]; @@ -1176,7 +1137,7 @@ void CameraImpl::process_camera_image_captured(const mavlink_message_t& message) _captured_request_cv.notify_all(); - std::lock_guard lock(_capture_info.mutex); + 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( @@ -1205,11 +1166,13 @@ void CameraImpl::process_camera_image_captured(const mavlink_message_t& message) _capture_info.missing_image_retries.erase(it); } } +#endif } void CameraImpl::request_missing_capture_info() { - std::lock_guard lock(_capture_info.mutex); +#if 0 + std::lock_guard lock(_capture_info.mutex); for (auto it = _capture_info.missing_image_retries.begin(); it != _capture_info.missing_image_retries.end(); @@ -1220,6 +1183,7 @@ void CameraImpl::request_missing_capture_info() ++it; } } +#endif /* TODO: implement again if (!_capture_info.missing_image_retries.empty()) { @@ -1289,8 +1253,7 @@ void CameraImpl::process_camera_information(const mavlink_message_t& message) }); if (potential_camera == _potential_cameras.end()) { - auto new_potential_camera = PotentialCamera{}; - new_potential_camera.component_id = message.compid; + auto new_potential_camera = PotentialCamera{message.compid}; _potential_cameras.emplace_back(std::move(new_potential_camera)); potential_camera = std::prev(_potential_cameras.end()); } @@ -1308,7 +1271,7 @@ void CameraImpl::check_camera_definition_with_lock(PotentialCamera& potential_ca if (potential_camera.camera_definition_url.empty()) { potential_camera.camera_definition_result = Camera::Result::SettingsUnavailable; - notify_camera_list(); + notify_camera_list_with_lock(); } const auto& info = potential_camera.maybe_information.value(); @@ -1322,11 +1285,11 @@ void CameraImpl::check_camera_definition_with_lock(PotentialCamera& potential_ca } if (cached_file_option) { - LogDebug() << "Using cached file " << cached_file_option.value(); + LogInfo() << "Using cached file " << cached_file_option.value(); load_camera_definition_with_lock(potential_camera, cached_file_option.value()); potential_camera.is_fetching_camera_definition = false; potential_camera.camera_definition_result = Camera::Result::Success; - notify_camera_list(); + notify_camera_list_with_lock(); } else { potential_camera.is_fetching_camera_definition = true; @@ -1367,7 +1330,7 @@ void CameraImpl::check_camera_definition_with_lock(PotentialCamera& potential_ca << std::to_string(curl_code); maybe_potential_camera->is_fetching_camera_definition = false; maybe_potential_camera->camera_definition_result = Camera::Result::Error; - notify_camera_list(); + notify_camera_list_with_lock(); } else if (status == HttpStatus::Finished) { LogDebug() << "File download finished " << download_path; @@ -1380,7 +1343,7 @@ void CameraImpl::check_camera_definition_with_lock(PotentialCamera& potential_ca load_camera_definition_with_lock(*maybe_potential_camera, download_path); maybe_potential_camera->is_fetching_camera_definition = false; maybe_potential_camera->camera_definition_result = Camera::Result::Success; - notify_camera_list(); + notify_camera_list_with_lock(); } }); #endif @@ -1419,7 +1382,7 @@ void CameraImpl::check_camera_definition_with_lock(PotentialCamera& potential_ca LogErr() << "File download failed with result " << client_result; maybe_potential_camera->is_fetching_camera_definition = false; maybe_potential_camera->camera_definition_result = Camera::Result::Error; - notify_camera_list(); + notify_camera_list_with_lock(); return; } @@ -1436,12 +1399,12 @@ void CameraImpl::check_camera_definition_with_lock(PotentialCamera& potential_ca load_camera_definition_with_lock(*maybe_potential_camera, downloaded_filepath); maybe_potential_camera->is_fetching_camera_definition = false; maybe_potential_camera->camera_definition_result = Camera::Result::Success; - notify_camera_list(); + notify_camera_list_with_lock(); }); } else { LogErr() << "Unknown protocol for URL: " << url; potential_camera.camera_definition_result = Camera::Result::ProtocolUnsupported; - notify_camera_list(); + notify_camera_list_with_lock(); } } } @@ -1472,73 +1435,86 @@ void CameraImpl::process_video_information(const mavlink_message_t& message) mavlink_video_stream_information_t received_video_info; mavlink_msg_video_stream_information_decode(&message, &received_video_info); - { - std::lock_guard lock(_video_stream_info.mutex); - // TODO: use stream_id and count - _video_stream_info.data.status = - (received_video_info.flags & VIDEO_STREAM_STATUS_FLAGS_RUNNING ? - Camera::VideoStreamInfo::VideoStreamStatus::InProgress : - Camera::VideoStreamInfo::VideoStreamStatus::NotRunning); - _video_stream_info.data.spectrum = - (received_video_info.flags & VIDEO_STREAM_STATUS_FLAGS_THERMAL ? - Camera::VideoStreamInfo::VideoStreamSpectrum::Infrared : - Camera::VideoStreamInfo::VideoStreamSpectrum::VisibleLight); - - auto& video_stream_info = _video_stream_info.data.settings; - video_stream_info.frame_rate_hz = received_video_info.framerate; - video_stream_info.horizontal_resolution_pix = received_video_info.resolution_h; - video_stream_info.vertical_resolution_pix = received_video_info.resolution_v; - video_stream_info.bit_rate_b_s = received_video_info.bitrate; - video_stream_info.rotation_deg = received_video_info.rotation; - video_stream_info.horizontal_fov_deg = static_cast(received_video_info.hfov); - video_stream_info.uri = received_video_info.uri; - _video_stream_info.available = true; - } - - notify_video_stream_info(); + std::lock_guard lock(_mutex); + auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(message.compid); + + if (maybe_potential_camera == nullptr) { + return; + } + + auto& camera = *maybe_potential_camera; + + // TODO: use stream_id and count + camera.video_stream_info.status = + (received_video_info.flags & VIDEO_STREAM_STATUS_FLAGS_RUNNING ? + Camera::VideoStreamInfo::VideoStreamStatus::InProgress : + Camera::VideoStreamInfo::VideoStreamStatus::NotRunning); + camera.video_stream_info.spectrum = + (received_video_info.flags & VIDEO_STREAM_STATUS_FLAGS_THERMAL ? + Camera::VideoStreamInfo::VideoStreamSpectrum::Infrared : + Camera::VideoStreamInfo::VideoStreamSpectrum::VisibleLight); + + auto& video_stream_info = camera.video_stream_info.settings; + video_stream_info.frame_rate_hz = received_video_info.framerate; + video_stream_info.horizontal_resolution_pix = received_video_info.resolution_h; + video_stream_info.vertical_resolution_pix = received_video_info.resolution_v; + video_stream_info.bit_rate_b_s = received_video_info.bitrate; + video_stream_info.rotation_deg = received_video_info.rotation; + video_stream_info.horizontal_fov_deg = static_cast(received_video_info.hfov); + video_stream_info.uri = received_video_info.uri; + + notify_video_stream_info_with_lock(); } void CameraImpl::process_video_stream_status(const mavlink_message_t& message) { mavlink_video_stream_status_t received_video_stream_status; mavlink_msg_video_stream_status_decode(&message, &received_video_stream_status); - { - std::lock_guard lock(_video_stream_info.mutex); - _video_stream_info.data.status = - (received_video_stream_status.flags & VIDEO_STREAM_STATUS_FLAGS_RUNNING ? - Camera::VideoStreamInfo::VideoStreamStatus::InProgress : - Camera::VideoStreamInfo::VideoStreamStatus::NotRunning); - _video_stream_info.data.spectrum = - (received_video_stream_status.flags & VIDEO_STREAM_STATUS_FLAGS_THERMAL ? - Camera::VideoStreamInfo::VideoStreamSpectrum::Infrared : - Camera::VideoStreamInfo::VideoStreamSpectrum::VisibleLight); - auto& video_stream_info = _video_stream_info.data.settings; - video_stream_info.frame_rate_hz = received_video_stream_status.framerate; - video_stream_info.horizontal_resolution_pix = received_video_stream_status.resolution_h; - video_stream_info.vertical_resolution_pix = received_video_stream_status.resolution_v; - video_stream_info.bit_rate_b_s = received_video_stream_status.bitrate; - video_stream_info.rotation_deg = received_video_stream_status.rotation; - video_stream_info.horizontal_fov_deg = - static_cast(received_video_stream_status.hfov); - _video_stream_info.available = true; + std::lock_guard lock(_mutex); + auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(message.compid); + + if (maybe_potential_camera == nullptr) { + return; } - notify_video_stream_info(); + auto& camera = *maybe_potential_camera; + + camera.video_stream_info.status = + (received_video_stream_status.flags & VIDEO_STREAM_STATUS_FLAGS_RUNNING ? + Camera::VideoStreamInfo::VideoStreamStatus::InProgress : + Camera::VideoStreamInfo::VideoStreamStatus::NotRunning); + camera.video_stream_info.spectrum = + (received_video_stream_status.flags & VIDEO_STREAM_STATUS_FLAGS_THERMAL ? + Camera::VideoStreamInfo::VideoStreamSpectrum::Infrared : + Camera::VideoStreamInfo::VideoStreamSpectrum::VisibleLight); + + auto& video_stream_info = camera.video_stream_info.settings; + video_stream_info.frame_rate_hz = received_video_stream_status.framerate; + video_stream_info.horizontal_resolution_pix = received_video_stream_status.resolution_h; + video_stream_info.vertical_resolution_pix = received_video_stream_status.resolution_v; + video_stream_info.bit_rate_b_s = received_video_stream_status.bitrate; + video_stream_info.rotation_deg = received_video_stream_status.rotation; + video_stream_info.horizontal_fov_deg = static_cast(received_video_stream_status.hfov); + + notify_video_stream_info_with_lock(); } -void CameraImpl::notify_video_stream_info() +void CameraImpl::notify_video_stream_info_with_lock() { - std::lock_guard lock(_video_stream_info.mutex); - - _video_stream_info.subscription_callbacks.queue( - _video_stream_info.data, - [this](const auto& func) { _system_impl->call_user_callback(func); }); + int32_t camera_id = 1; + for (auto& potential_camera : _potential_cameras) { + _video_stream_info_subscription_callbacks.queue( + Camera::VideoStreamUpdate{camera_id, potential_camera.video_stream_info}, + [this](const auto& func) { _system_impl->call_user_callback(func); }); + } + ++camera_id; } void CameraImpl::check_status() { - std::lock_guard lock(_status.mutex); +#if 0 + std::lock_guard lock(_status.mutex); if (_status.received_camera_capture_status && _status.received_storage_information) { _status.subscription_callbacks.queue( @@ -1547,6 +1523,7 @@ void CameraImpl::check_status() _status.received_camera_capture_status = false; _status.received_storage_information = false; } +#endif } void CameraImpl::receive_command_result( @@ -1580,13 +1557,17 @@ void CameraImpl::receive_set_mode_command_result( } } -void CameraImpl::notify_mode() +void CameraImpl::notify_mode_with_lock() { - // TODO implement - // std::lock_guard lock(_mode.mutex); - - //_mode.subscription_callbacks.queue( - // _mode.data, [this](const auto& func) { _system_impl->call_user_callback(func); }); + int32_t camera_id = 1; + for (auto& potential_camera : _potential_cameras) { + if (potential_camera.camera_definition != nullptr) { + _mode_subscription_callbacks.queue( + Camera::ModeUpdate{camera_id, potential_camera.mode}, + [this](const auto& func) { _system_impl->call_user_callback(func); }); + } + ++camera_id; + } } bool CameraImpl::get_possible_options_with_lock( @@ -1890,41 +1871,36 @@ void CameraImpl::get_option_async( Camera::CurrentSettingsHandle CameraImpl::subscribe_current_settings(const Camera::CurrentSettingsCallback& callback) { - std::unique_lock lock(_subscribe_current_settings.mutex); - auto handle = _subscribe_current_settings.callbacks.subscribe(callback); - lock.unlock(); + std::lock_guard lock(_mutex); + auto handle = _subscribe_current_settings_callbacks.subscribe(callback); - notify_current_settings_for_all(); + notify_current_settings_for_all_with_lock(); return handle; } void CameraImpl::unsubscribe_current_settings(Camera::CurrentSettingsHandle handle) { - std::lock_guard lock(_subscribe_current_settings.mutex); - _subscribe_current_settings.callbacks.unsubscribe(handle); + std::lock_guard lock(_mutex); + _subscribe_current_settings_callbacks.unsubscribe(handle); } Camera::PossibleSettingOptionsHandle CameraImpl::subscribe_possible_setting_options( const Camera::PossibleSettingOptionsCallback& callback) { - std::unique_lock lock(_subscribe_possible_setting_options.mutex); - auto handle = _subscribe_possible_setting_options.callbacks.subscribe(callback); - lock.unlock(); + std::lock_guard lock(_mutex); + auto handle = _subscribe_possible_setting_options_callbacks.subscribe(callback); - notify_possible_setting_options_for_all(); + notify_possible_setting_options_for_all_with_lock(); return handle; } void CameraImpl::unsubscribe_possible_setting_options(Camera::PossibleSettingOptionsHandle handle) { - std::lock_guard lock(_subscribe_possible_setting_options.mutex); - _subscribe_possible_setting_options.callbacks.unsubscribe(handle); + _subscribe_possible_setting_options_callbacks.unsubscribe(handle); } -void CameraImpl::notify_current_settings_for_all() +void CameraImpl::notify_current_settings_for_all_with_lock() { - std::lock_guard lock(_mutex); - for (auto& potential_camera : _potential_cameras) { if (potential_camera.camera_definition != nullptr) { notify_current_settings_with_lock(potential_camera); @@ -1932,10 +1908,8 @@ void CameraImpl::notify_current_settings_for_all() } } -void CameraImpl::notify_possible_setting_options_for_all() +void CameraImpl::notify_possible_setting_options_for_all_with_lock() { - std::lock_guard lock(_mutex); - for (auto& potential_camera : _potential_cameras) { if (potential_camera.camera_definition != nullptr) { notify_possible_setting_options_with_lock(potential_camera); @@ -1947,7 +1921,7 @@ void CameraImpl::notify_current_settings_with_lock(PotentialCamera& potential_ca { assert(potential_camera.camera_definition != nullptr); - if (_subscribe_current_settings.callbacks.empty()) { + if (_subscribe_current_settings_callbacks.empty()) { return; } @@ -1959,7 +1933,8 @@ void CameraImpl::notify_current_settings_with_lock(PotentialCamera& potential_ca auto& camera = potential_camera; - std::vector current_settings{}; + Camera::CurrentSettingsUpdate update{}; + update.camera_id = camera_id_for_potential_camera_with_lock(potential_camera); for (auto& possible_setting : possible_setting_options.second) { // use the cache for this, presumably we updated it right before. @@ -1978,30 +1953,34 @@ void CameraImpl::notify_current_settings_with_lock(PotentialCamera& potential_ca setting.option.option_id, setting.option.option_description); } - current_settings.push_back(setting); + update.current_settings.push_back(setting); } } - _subscribe_current_settings.callbacks.queue( - current_settings, [this](const auto& func) { _system_impl->call_user_callback(func); }); + _subscribe_current_settings_callbacks.queue( + update, [this](const auto& func) { _system_impl->call_user_callback(func); }); } void CameraImpl::notify_possible_setting_options_with_lock(PotentialCamera& potential_camera) { assert(potential_camera.camera_definition != nullptr); - if (_subscribe_possible_setting_options.callbacks.empty()) { + if (_subscribe_possible_setting_options_callbacks.empty()) { return; } + Camera::PossibleSettingOptionsUpdate update{}; + update.camera_id = camera_id_for_potential_camera_with_lock(potential_camera); + auto setting_options = get_possible_setting_options_with_lock(potential_camera); if (setting_options.second.empty()) { return; } - _subscribe_possible_setting_options.callbacks.queue( - setting_options.second, - [this](const auto& func) { _system_impl->call_user_callback(func); }); + update.setting_options = setting_options.second; + + _subscribe_possible_setting_options_callbacks.queue( + update, [this](const auto& func) { _system_impl->call_user_callback(func); }); } std::pair> @@ -2142,12 +2121,6 @@ bool CameraImpl::get_option_str_with_lock( return potential_camera.camera_definition->get_option_str(setting_id, option_id, description); } -void CameraImpl::request_camera_settings(int32_t camera_id) -{ - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_CAMERA_SETTINGS, component_id_for_camera_id(camera_id), nullptr); -} - void CameraImpl::request_camera_information(uint8_t component_id) { _system_impl->mavlink_request_message().request( @@ -2168,8 +2141,6 @@ Camera::Result CameraImpl::format_storage(int32_t camera_id, int32_t storage_id) void CameraImpl::format_storage_async( int32_t camera_id, int32_t storage_id, const Camera::ResultCallback& callback) { - - MavlinkCommandSender::CommandLong cmd_format{}; cmd_format.command = MAV_CMD_STORAGE_FORMAT; @@ -2184,7 +2155,8 @@ void CameraImpl::format_storage_async( receive_command_result(result, [this, callback](Camera::Result camera_result) { if (camera_result == Camera::Result::Success) { - reset_following_format_storage(); + // TODO: add in again + // reset_following_format_storage(); } callback(camera_result); @@ -2219,19 +2191,14 @@ void CameraImpl::reset_settings_async(int32_t camera_id, const Camera::ResultCal }); } -void CameraImpl::reset_following_format_storage() +void CameraImpl::reset_following_format_storage_with_lock(PotentialCamera& camera) { - { - std::lock_guard status_lock(_status.mutex); - _status.photo_list.clear(); - _status.image_count = 0; - _status.image_count_at_connection = 0; - } - { - std::lock_guard lock(_capture_info.mutex); - _capture_info.last_advertised_image_index = -1; - _capture_info.missing_image_retries.clear(); - } + UNUSED(camera); + // camera.capture_status.photo_list.clear(); + // camera.capture_status.image_count = 0; + // camera.capture_status.image_count_at_connection = 0; + // camera.capture_info.last_advertised_image_index = -1; + // camera.capture_info.missing_image_retries.clear(); } std::pair> @@ -2253,13 +2220,32 @@ CameraImpl::list_photos(int32_t camera_id, Camera::PhotosRange photos_range) void CameraImpl::list_photos_async( int32_t camera_id, Camera::PhotosRange photos_range, const Camera::ListPhotosCallback& callback) { + UNUSED(camera_id); + UNUSED(photos_range); + if (!callback) { LogWarn() << "Trying to get a photo list with a null callback, ignoring..."; return; } + // TODO: enable again with a camera to test against. + _system_impl->call_user_callback([callback]() { + LogErr() << "Photo list not implemented."; + callback(Camera::Result::ProtocolUnsupported, std::vector{}); + }); + +#if 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) { + LogWarn() << "Invalid camera ID: " << camera_id; + return; + } + auto& camera = *maybe_potential_camera; + { - std::lock_guard status_lock(_status.mutex); + std::lock_guard status_lock(_status.mutex); if (_status.is_fetching_photos) { _system_impl->call_user_callback([callback]() { @@ -2313,7 +2299,7 @@ void CameraImpl::list_photos_async( // Timeout if the request times out that many times const auto request_try_limit = 10; if (request_try_number >= request_try_limit) { - std::lock_guard status_lock(_status.mutex); + std::lock_guard status_lock(_status.mutex); _status.is_fetching_photos = false; _system_impl->call_user_callback([callback]() { callback(Camera::Result::Timeout, std::vector{}); @@ -2332,7 +2318,7 @@ void CameraImpl::list_photos_async( } if (safety_count == safety_count_boundary) { - std::lock_guard status_lock(_status.mutex); + std::lock_guard status_lock(_status.mutex); _status.is_fetching_photos = false; _system_impl->call_user_callback([callback]() { callback(Camera::Result::Error, std::vector{}); @@ -2343,7 +2329,7 @@ void CameraImpl::list_photos_async( std::vector photo_list; { - std::lock_guard status_lock(_status.mutex); + std::lock_guard status_lock(_status.mutex); for (const auto& capture_info : _status.photo_list) { if (capture_info.first >= start_index) { @@ -2359,13 +2345,60 @@ void CameraImpl::list_photos_async( }); } }).detach(); +#endif } -Camera::Mode CameraImpl::get_mode(int32_t camera_id) {} +std::pair CameraImpl::get_mode(int32_t camera_id) +{ + std::pair result{}; + + std::lock_guard lock(_mutex); + + auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(camera_id); + if (maybe_potential_camera == nullptr) { + result.first = Camera::Result::CameraIdInvalid; + } else { + result.first = Camera::Result::Success; + result.second = maybe_potential_camera->mode; + } + + return result; +} -Camera::Status CameraImpl::get_status(int32_t camera_id) {} +std::pair CameraImpl::get_status(int32_t camera_id) +{ + std::pair result{}; -Camera::VideoStreamInfo CameraImpl::get_video_stream_info(int32_t camera_id) {} + std::lock_guard lock(_mutex); + + auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(camera_id); + if (maybe_potential_camera == nullptr) { + result.first = Camera::Result::CameraIdInvalid; + } else { + result.first = Camera::Result::Success; + result.second = maybe_potential_camera->status; + } + + return result; +} + +std::pair +CameraImpl::get_video_stream_info(int32_t camera_id) +{ + std::pair result{}; + + std::lock_guard lock(_mutex); + + auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(camera_id); + if (maybe_potential_camera == nullptr) { + result.first = Camera::Result::CameraIdInvalid; + } else { + result.first = Camera::Result::Success; + result.second = maybe_potential_camera->video_stream_info; + } + + return result; +} std::pair> CameraImpl::get_current_settings(int32_t camera_id) @@ -2421,45 +2454,6 @@ CameraImpl::get_current_settings(int32_t camera_id) return result; } -// std::pair> -// CameraImpl::get_possible_setting_options(int32_t camera_id) -//{ -// std::pair> result; -// -// std::lock_guard lock(_potential_cameras_mutex); -// auto maybe_potential_camera = maybe_potential_camera_for_camera_id_with_lock(camera_id); -// if (maybe_potential_camera == nullptr) { -// result.first = Camera::Result::CameraIdInvalid; -// return result; -// } -// -// auto& camera& = *maybe_potential_camera; -// if (!camera.is_fetching_camera_definition) { -// result.first = Camera::Result::SettingsLoading; -// return result; -// } -// -// if (camera.camera_definition == nullptr) { -// result.first = Camera::Result::SettingsUnavailable; -// return result; -// } -// -// std::unordered_map cd_settings{}; -// camera.camera_definition->get_possible_settings(cd_settings); -// -// for (const auto& cd_setting : cd_settings) { -// if (cd_setting.first == "CAM_MODE") { -// // We ignore the mode param. -// continue; -// } -// -// result.second.push_back(cd_setting.first); -// } -// -// result.first = Camera::Result::Success; -// return result; -// } - uint8_t CameraImpl::component_id_for_camera_id(int32_t camera_id) { if (camera_id == 0) { diff --git a/src/mavsdk/plugins/camera/camera_impl.h b/src/mavsdk/plugins/camera/camera_impl.h index aeb1dc798..1fe3b0f4e 100644 --- a/src/mavsdk/plugins/camera/camera_impl.h +++ b/src/mavsdk/plugins/camera/camera_impl.h @@ -1,7 +1,10 @@ #pragma once #include +#include #include +#include +#include #include "camera_definition.h" #include "file_cache.h" @@ -10,7 +13,6 @@ #include "plugin_impl_base.h" #include "system.h" #include "callback_list.h" -#include namespace mavsdk { @@ -93,7 +95,7 @@ class CameraImpl : public PluginImplBase { subscribe_video_stream_info(const Camera::VideoStreamInfoCallback& callback); void unsubscribe_video_stream_info(Camera::VideoStreamInfoHandle handle); - Camera::VideoStreamInfo get_video_stream_info(int32_t camera_id); + std::pair get_video_stream_info(int32_t camera_id); Camera::Result start_video_streaming(int32_t camera_id, int32_t stream_id); Camera::Result stop_video_streaming(int32_t camera_id, int32_t stream_id); @@ -102,7 +104,7 @@ class CameraImpl : public PluginImplBase { void set_mode_async(int32_t camera_id, Camera::Mode mode, const Camera::ResultCallback& callback); - Camera::Mode get_mode(int32_t camera_id); + std::pair get_mode(int32_t camera_id); Camera::ModeHandle subscribe_mode(const Camera::ModeCallback& callback); void unsubscribe_mode(Camera::ModeHandle handle); @@ -113,7 +115,7 @@ class CameraImpl : public PluginImplBase { Camera::StatusHandle subscribe_status(const Camera::StatusCallback& callback); void unsubscribe_status(Camera::StatusHandle handle); - Camera::Status get_status(int32_t camera_id); + std::pair get_status(int32_t camera_id); void get_setting_async( int32_t camera_id, @@ -159,7 +161,27 @@ class CameraImpl : public PluginImplBase { CameraImpl& operator=(const CameraImpl&) = delete; private: + // struct CaptureStatus { + // // Duplicate right now. + // Camera::Status data{}; + // std::unique_ptr fetch_thread{}; + + // bool received_camera_capture_status{false}; + // bool received_storage_information{false}; + // int image_count{-1}; + // int image_count_at_connection{-1}; + // std::map photo_list; + // bool is_fetching_photos{false}; + // }; + + // struct CaptureInfo { + // int last_advertised_image_index{-1}; + // std::map missing_image_retries{}; + // }; + struct PotentialCamera { + explicit PotentialCamera(uint8_t new_component_id) : component_id(new_component_id) {} + std::optional maybe_information; std::unique_ptr camera_definition{}; std::string camera_definition_url{}; @@ -167,16 +189,23 @@ class CameraImpl : public PluginImplBase { Camera::Result camera_definition_result{Camera::Result::Unknown}; bool is_fetching_camera_definition{false}; size_t camera_definition_fetch_count{0}; - using CameraDefinitionCallback = std::function; - CameraDefinitionCallback camera_definition_callback{}; bool information_requested{false}; bool is_valid{false}; - uint8_t component_id; + // CaptureStatus capture_status{}; + // CaptureInfo capture_info{}; + // std::condition_variable _captured_request_cv; + // std::mutex _captured_request_mutex; + + Camera::Status status{}; + Camera::VideoStreamInfo video_stream_info{}; Camera::Mode mode{Camera::Mode::Unknown}; + uint16_t capture_sequence{0}; + uint8_t component_id; + bool operator==(const PotentialCamera& other) const { return this->component_id == other.component_id; @@ -239,23 +268,25 @@ class CameraImpl : public PluginImplBase { void process_camera_information(const mavlink_message_t& message); void process_video_information(const mavlink_message_t& message); void process_video_stream_status(const mavlink_message_t& message); - void reset_following_format_storage(); + + void reset_following_format_storage_with_lock(PotentialCamera& potential_camera); void check_potential_cameras_with_lock(); void check_camera_definition_with_lock(PotentialCamera& potential_camera); void load_camera_definition_with_lock( PotentialCamera& potential_camera, const std::filesystem::path& path); - void notify_mode(); - void notify_video_stream_info(); + void notify_mode_with_lock(); - void notify_current_settings_for_all(); + void notify_video_stream_info_with_lock(); + + void notify_current_settings_for_all_with_lock(); void notify_current_settings_with_lock(PotentialCamera& potential_camera); - void notify_possible_setting_options_for_all(); + void notify_possible_setting_options_for_all_with_lock(); void notify_possible_setting_options_with_lock(PotentialCamera& potential_camera); - void notify_camera_list(); + void notify_camera_list_with_lock(); void check_status(); @@ -271,11 +302,11 @@ class CameraImpl : public PluginImplBase { CallEveryHandler::Cookie _camera_information_call_every_cookie{}; CallEveryHandler::Cookie _request_missing_capture_info_cookie{}; - void request_camera_settings(int32_t camera_id); + // void request_camera_settings(int32_t camera_id); void request_camera_information(uint8_t component_id); - void request_video_stream_info(int32_t camera_id); - void request_video_stream_status(int32_t camera_id); - void request_status(int32_t camera_id); + // void request_video_stream_info(int32_t camera_id); + // void request_video_stream_status(int32_t camera_id); + // void request_status(int32_t camera_id); MavlinkCommandSender::CommandLong make_command_take_photo(int32_t camera_id, float interval_s, float no_of_photos); @@ -311,6 +342,9 @@ class CameraImpl : public PluginImplBase { MavlinkCommandSender::CommandLong make_command_focus_stop(int32_t camera_id); MavlinkCommandSender::CommandLong make_command_focus_range(int32_t camera_id, float range); + void request_slower(); + void request_faster(); + void request_missing_capture_info(); uint8_t component_id_for_camera_id(int32_t camera_id); @@ -318,73 +352,29 @@ class CameraImpl : public PluginImplBase { 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 potential_cameras_for_camera_id_with_lock(int32_t camera_id); static std::string get_filename_from_path(const std::string& path); std::mutex _mutex; std::vector _potential_cameras; - CallbackList camera_list_subscription_callbacks{}; + CallbackList _camera_list_subscription_callbacks{}; + CallbackList + _subscribe_possible_setting_options_callbacks{}; + CallbackList _subscribe_current_settings_callbacks{}; + CallbackList _mode_subscription_callbacks{}; + CallbackList _capture_info_callbacks{}; + CallbackList _video_stream_info_subscription_callbacks{}; CallEveryHandler::Cookie _check_potential_cameras_call_every_cookie{}; std::optional _file_cache{}; std::filesystem::path _tmp_download_path{}; - bool _is_fetching_camera_definition{false}; - bool _has_camera_definition_timed_out{false}; - size_t _camera_definition_fetch_count{0}; - using CameraDefinitionCallback = std::function; - CameraDefinitionCallback _camera_definition_callback{}; - - struct { - std::mutex mutex{}; - Camera::Status data{}; - bool received_camera_capture_status{false}; - bool received_storage_information{false}; - int image_count{-1}; - int image_count_at_connection{-1}; - std::map photo_list; - bool is_fetching_photos{false}; - - CallbackList subscription_callbacks{}; - CallEveryHandler::Cookie call_every_cookie{}; - } _status{}; - - static constexpr double DEFAULT_TIMEOUT_S = 3.0; - - struct { - std::mutex mutex{}; - CallbackList subscription_callbacks{}; - CallEveryHandler::Cookie call_every_cookie{}; - } _mode{}; - - struct { - std::mutex mutex{}; - CallbackList callbacks{}; - int last_advertised_image_index{-1}; - std::map missing_image_retries{}; - } _capture_info{}; - - struct { - std::mutex mutex{}; - Camera::VideoStreamInfo data{}; - bool available{false}; - CallEveryHandler::Cookie call_every_cookie{}; - CallbackList subscription_callbacks{}; - } _video_stream_info{}; - - struct { - std::mutex mutex{}; - CallbackList> callbacks{}; - } _subscribe_current_settings{}; - - struct { - std::mutex mutex{}; - CallbackList> callbacks{}; - } _subscribe_possible_setting_options{}; - - std::condition_variable _captured_request_cv; - std::mutex _captured_request_mutex; + CallbackList _status_subscription_callbacks{}; + + CallEveryHandler::Cookie _request_slower_call_every_cookie{}; + CallEveryHandler::Cookie _request_faster_call_every_cookie{}; std::unique_ptr _http_loader{nullptr}; diff --git a/src/mavsdk/plugins/camera/include/plugins/camera/camera.h b/src/mavsdk/plugins/camera/include/plugins/camera/camera.h index c82b24097..c4263aa09 100644 --- a/src/mavsdk/plugins/camera/include/plugins/camera/camera.h +++ b/src/mavsdk/plugins/camera/include/plugins/camera/camera.h @@ -95,27 +95,268 @@ class Camera : public PluginBase { */ friend std::ostream& operator<<(std::ostream& str, Camera::PhotosRange const& photos_range); + /** + * @brief Type to represent a setting option. + */ + struct Option { + std::string option_id{}; /**< @brief Name of the option (machine readable) */ + std::string option_description{}; /**< @brief Description of the option (human readable) */ + }; + + /** + * @brief Equal operator to compare two `Camera::Option` objects. + * + * @return `true` if items are equal. + */ + friend bool operator==(const Camera::Option& lhs, const Camera::Option& rhs); + + /** + * @brief Stream operator to print information about a `Camera::Option`. + * + * @return A reference to the stream. + */ + friend std::ostream& operator<<(std::ostream& str, Camera::Option const& option); + + /** + * @brief Type to represent a setting with a selected option. + */ + struct Setting { + std::string setting_id{}; /**< @brief Name of a setting (machine readable) */ + std::string setting_description{}; /**< @brief Description of the setting (human readable). + This field is meant to be read from the drone, ignore + it when setting. */ + Option option{}; /**< @brief Selected option */ + bool is_range{}; /**< @brief If option is given as a range. This field is meant to be read + from the drone, ignore it when setting. */ + }; + + /** + * @brief Equal operator to compare two `Camera::Setting` objects. + * + * @return `true` if items are equal. + */ + friend bool operator==(const Camera::Setting& lhs, const Camera::Setting& rhs); + + /** + * @brief Stream operator to print information about a `Camera::Setting`. + * + * @return A reference to the stream. + */ + friend std::ostream& operator<<(std::ostream& str, Camera::Setting const& setting); + + /** + * @brief Type to represent a setting with a list of options to choose from. + */ + struct SettingOptions { + int32_t camera_id{}; /**< @brief Camera ID */ + std::string setting_id{}; /**< @brief Name of the setting (machine readable) */ + std::string + setting_description{}; /**< @brief Description of the setting (human readable) */ + std::vector