diff --git a/examples/camera/camera.cpp b/examples/camera/camera.cpp index f7cac0e675..00a32218e2 100644 --- a/examples/camera/camera.cpp +++ b/examples/camera/camera.cpp @@ -73,8 +73,22 @@ int main(int argc, char** argv) auto telemetry = Telemetry{system}; auto camera = Camera{system}; + // Wait for camera to be discovered. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // We expect to find one camera. + if (camera.camera_list().cameras.size() == 0) { + std::cerr << "No camera found, exiting.\n"; + return 1; + } + + if (camera.camera_list().cameras.size() > 1) { + std::cout << "More than one camera found, using first one discovered.\n"; + } + const auto component_id = camera.camera_list().cameras[0].component_id; + // First, make sure camera is in photo mode. - const auto mode_result = camera.set_mode(Camera::Mode::Photo); + const auto mode_result = camera.set_mode(component_id, Camera::Mode::Photo); if (mode_result != Camera::Result::Success) { std::cerr << "Could not switch to Photo mode: " << mode_result; return 1; @@ -85,7 +99,7 @@ int main(int argc, char** argv) std::cout << "Image captured, stored at: " << capture_info.file_url << '\n'; }); - const auto photo_result = camera.take_photo(); + const auto photo_result = camera.take_photo(component_id); if (photo_result != Camera::Result::Success) { std::cerr << "Taking Photo failed: " << mode_result; return 1; diff --git a/examples/camera_server/camera_client.cpp b/examples/camera_server/camera_client.cpp index e115485d68..ac921a8423 100644 --- a/examples/camera_server/camera_client.cpp +++ b/examples/camera_server/camera_client.cpp @@ -6,7 +6,7 @@ #include #include -static void do_camera_operation(mavsdk::Camera& camera); +static void do_camera_operation(int32_t component_id, mavsdk::Camera& camera); int main(int argc, const char* argv[]) { @@ -44,17 +44,35 @@ int main(int argc, const char* argv[]) auto system = fut.get(); auto camera = mavsdk::Camera{system}; - camera.subscribe_information([](mavsdk::Camera::Information info) { - std::cout << "Camera information:" << std::endl; - std::cout << info << std::endl; + + camera.subscribe_camera_list([](mavsdk::Camera::CameraList list) { + std::cout << "Cameras found:" << std::endl; + for (auto& camera : list.cameras) { + std::cout << camera << '\n'; + } }); - camera.subscribe_status([](mavsdk::Camera::Status status) { - std::cout << "Camera status:" << std::endl; - std::cout << status << std::endl; + // Wait for camera to be discovered. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // We expect to find one camera. + if (camera.camera_list().cameras.size() == 0) { + std::cerr << "No camera found, exiting.\n"; + return 1; + } + + if (camera.camera_list().cameras.size() > 1) { + std::cout << "More than one camera found, using first one discovered.\n"; + } + + const auto component_id = camera.camera_list().cameras[0].component_id; + + camera.subscribe_storage([](mavsdk::Camera::StorageUpdate update) { + std::cout << "Camera storage update:" << std::endl; + std::cout << update << std::endl; }); - do_camera_operation(camera); + do_camera_operation(component_id, camera); // for test subscribe camera status, so don't end the process while (true) { @@ -63,36 +81,36 @@ int main(int argc, const char* argv[]) return 0; } -void do_camera_operation(mavsdk::Camera& camera) +void do_camera_operation(int32_t component_id, mavsdk::Camera& camera) { // switch to photo mode to take photo - auto operation_result = camera.set_mode(mavsdk::Camera::Mode::Photo); + auto operation_result = camera.set_mode(component_id, mavsdk::Camera::Mode::Photo); std::cout << "Set camera to photo mode result : " << operation_result << std::endl; - operation_result = camera.take_photo(); + operation_result = camera.take_photo(component_id); std::cout << "Take photo result : " << operation_result << std::endl; // switch to video mode to recording video - operation_result = camera.set_mode(mavsdk::Camera::Mode::Video); + operation_result = camera.set_mode(component_id, mavsdk::Camera::Mode::Video); std::cout << "Set camera to video mode result : " << operation_result << std::endl; - operation_result = camera.start_video(); + operation_result = camera.start_video(component_id); std::cout << "Start video result : " << operation_result << std::endl; - operation_result = camera.stop_video(); + operation_result = camera.stop_video(component_id); std::cout << "Stop video result : " << operation_result << std::endl; // the camera can have multi video stream so may need the stream id - operation_result = camera.start_video_streaming(2); + operation_result = camera.start_video_streaming(component_id, 0); std::cout << "start video streaming result : " << operation_result << std::endl; - operation_result = camera.stop_video_streaming(2); + operation_result = camera.stop_video_streaming(component_id, 0); std::cout << "stop video streaming result : " << operation_result << std::endl; // format the storage with special storage id test - operation_result = camera.format_storage(11); + operation_result = camera.format_storage(component_id, 0); std::cout << "format storage result : " << operation_result << std::endl; - operation_result = camera.reset_settings(); + operation_result = camera.reset_settings(component_id); std::cout << "Reset camera settings result : " << operation_result << std::endl; } diff --git a/examples/camera_settings/camera_settings.cpp b/examples/camera_settings/camera_settings.cpp index cfe91f82b0..97245e38e6 100644 --- a/examples/camera_settings/camera_settings.cpp +++ b/examples/camera_settings/camera_settings.cpp @@ -47,7 +47,7 @@ void show_settings(const CurrentSettings& current_settings) } } -void change_camera_mode(Camera& camera) +void change_camera_mode(int32_t component_id, Camera& camera) { while (true) { std::cout << "Possible modes:\n" @@ -60,21 +60,28 @@ void change_camera_mode(Camera& camera) std::cin >> input; if (input == "1") { - camera.set_mode(Camera::Mode::Photo); + camera.set_mode(component_id, Camera::Mode::Photo); return; } else if (input == "2") { - camera.set_mode(Camera::Mode::Video); + camera.set_mode(component_id, Camera::Mode::Video); return; } } } bool choose_setting( - Camera& camera, const std::string& setting_id, const std::string& current_option_id) + int32_t component_id, + Camera& camera, + const std::string& setting_id, + const std::string& current_option_id) { - const auto setting_options = camera.possible_setting_options(); + const auto setting_options = camera.get_possible_setting_options(component_id); + if (setting_options.first != Camera::Result::Success) { + std::cerr << "Could not get settings\n"; + return false; + } - for (const auto& setting_option : setting_options) { + for (const auto& setting_option : setting_options.second) { if (setting_option.setting_id == setting_id) { std::cout << "Options for " << setting_option.setting_description << ":\n"; unsigned index = 1; @@ -108,7 +115,7 @@ bool choose_setting( setting.setting_id = setting_id; setting.option.option_id = setting_option.options[input_int].option_id; - camera.set_setting(setting); + camera.set_setting(component_id, setting); return true; } } @@ -116,7 +123,8 @@ bool choose_setting( return false; } -void change_camera_setting(Camera& camera, const CurrentSettings& current_settings) +void change_camera_setting( + int32_t component_id, Camera& camera, const CurrentSettings& current_settings) { while (true) { std::cout << "Possible settings:\n"; @@ -153,6 +161,7 @@ void change_camera_setting(Camera& camera, const CurrentSettings& current_settin } if (choose_setting( + component_id, camera, temp_settings[input_int].setting_id, temp_settings[input_int].option.option_id)) { @@ -237,13 +246,28 @@ int main(int argc, char** argv) auto telemetry = Telemetry{system}; auto camera = Camera{system}; + // Wait for camera to be discovered. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // We expect to find one camera. + if (camera.camera_list().cameras.size() == 0) { + std::cerr << "No camera found, exiting.\n"; + return 1; + } + + if (camera.camera_list().cameras.size() > 1) { + std::cout << "More than one camera found, using first one discovered.\n"; + } + const auto component_id = camera.camera_list().cameras[0].component_id; + CurrentSettings current_settings; // Subscribe to current settings and cache them. - camera.subscribe_current_settings([¤t_settings](std::vector settings) { - std::lock_guard lock(current_settings.mutex); - current_settings.settings = settings; - }); + camera.subscribe_current_settings( + [¤t_settings](const Camera::CurrentSettingsUpdate& update) { + std::lock_guard lock(current_settings.mutex); + current_settings.settings = update.current_settings; + }); while (true) { switch (get_input()) { @@ -251,10 +275,10 @@ int main(int argc, char** argv) show_settings(current_settings); break; case Input::ChangeCameraMode: - change_camera_mode(camera); + change_camera_mode(component_id, camera); break; case Input::ChangeSetting: - change_camera_setting(camera, current_settings); + change_camera_setting(component_id, camera, current_settings); break; case Input::Quit: return 0; diff --git a/examples/camera_zoom/camera_zoom.cpp b/examples/camera_zoom/camera_zoom.cpp index ee2f4d4cf0..f75ff7bb54 100644 --- a/examples/camera_zoom/camera_zoom.cpp +++ b/examples/camera_zoom/camera_zoom.cpp @@ -72,8 +72,22 @@ int main(int argc, char** argv) // Instantiate plugins. auto camera = Camera{system}; + // Wait for camera to be discovered. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // We expect to find one camera. + if (camera.camera_list().cameras.size() == 0) { + std::cerr << "No camera found, exiting.\n"; + return 1; + } + + if (camera.camera_list().cameras.size() > 1) { + std::cout << "More than one camera found, using first one discovered.\n"; + } + const auto component_id = camera.camera_list().cameras[0].component_id; + // Zoom in - auto result = camera.zoom_range(2.0f); + auto result = camera.zoom_range(component_id, 2.0f); if (result != Camera::Result::Success) { std::cerr << "Zooming failed: " << result << '\n'; return 1; @@ -83,7 +97,7 @@ int main(int argc, char** argv) sleep_for(seconds(4)); // Now try continuous zooming in. - result = camera.zoom_in_start(); + result = camera.zoom_in_start(component_id); if (result != Camera::Result::Success) { std::cerr << "Zooming in failed: " << result << '\n'; return 1; @@ -92,7 +106,7 @@ int main(int argc, char** argv) sleep_for(seconds(1)); // Now try stopping. - result = camera.zoom_stop(); + result = camera.zoom_stop(component_id); if (result != Camera::Result::Success) { std::cerr << "Stop zooming failed: " << result << '\n'; return 1; @@ -101,7 +115,7 @@ int main(int argc, char** argv) sleep_for(seconds(1)); // And back out. - result = camera.zoom_out_start(); + result = camera.zoom_out_start(component_id); if (result != Camera::Result::Success) { std::cerr << "Zooming out failed: " << result << '\n'; return 1; @@ -110,7 +124,7 @@ int main(int argc, char** argv) sleep_for(seconds(1)); // Stop again. - result = camera.zoom_stop(); + result = camera.zoom_stop(component_id); if (result != Camera::Result::Success) { std::cerr << "Stop zooming failed: " << result << '\n'; return 1; @@ -119,7 +133,7 @@ int main(int argc, char** argv) sleep_for(seconds(1)); // Zoom back out - result = camera.zoom_range(1.0f); + result = camera.zoom_range(component_id, 1.0f); if (result != Camera::Result::Success) { std::cerr << "Zooming failed: " << result << '\n'; return 1; diff --git a/proto b/proto index 01a546d4f8..0fe3a36908 160000 --- a/proto +++ b/proto @@ -1 +1 @@ -Subproject commit 01a546d4f83e06ecde6a005bfbbbd067aaa49ac5 +Subproject commit 0fe3a36908ebfce285c2854e82d5709b5400c019 diff --git a/src/integration_tests/CMakeLists.txt b/src/integration_tests/CMakeLists.txt index 169afb9835..ebb5235f76 100644 --- a/src/integration_tests/CMakeLists.txt +++ b/src/integration_tests/CMakeLists.txt @@ -11,21 +11,11 @@ add_executable(integration_tests_runner action_goto.cpp action_hold.cpp calibration.cpp - camera_capture_info.cpp - camera_mode.cpp - camera_settings.cpp - camera_status.cpp - camera_take_photo.cpp - camera_take_photo_interval.cpp - camera_format.cpp - camera_reset_settings.cpp - camera_test_helpers.cpp connection.cpp follow_me.cpp geofence.cpp info.cpp offboard_attitude.cpp - #logging.cpp # Not fully implemented log_files.cpp mission_cancellation.cpp mission_change_speed.cpp diff --git a/src/integration_tests/camera_capture_info.cpp b/src/integration_tests/camera_capture_info.cpp deleted file mode 100644 index 264f30e049..0000000000 --- a/src/integration_tests/camera_capture_info.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include "integration_test_helper.h" -#include "mavsdk.h" -#include -#include -#include -#include -#include "plugins/camera/camera.h" - -using namespace mavsdk; - -TEST(CameraTest, CaptureInfo) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - ASSERT_EQ(mavsdk.systems().size(), 1); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = Camera{system}; - - std::this_thread::sleep_for(std::chrono::seconds(2)); - - EXPECT_EQ(camera.take_photo(), Camera::Result::Success); - - bool received_capture_info = false; - camera.subscribe_capture_info([&received_capture_info](Camera::CaptureInfo capture_info) { - received_capture_info = true; - LogInfo() << capture_info; - }); - - std::this_thread::sleep_for(std::chrono::seconds(5)); - EXPECT_TRUE(received_capture_info); -} diff --git a/src/integration_tests/camera_format.cpp b/src/integration_tests/camera_format.cpp deleted file mode 100644 index 7bfa706666..0000000000 --- a/src/integration_tests/camera_format.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "integration_test_helper.h" -#include "mavsdk.h" -#include -#include -#include -#include "plugins/camera/camera.h" - -using namespace mavsdk; - -TEST(CameraTest, Format) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - - auto camera = std::make_shared(system); - - EXPECT_EQ(Camera::Result::Success, camera->format_storage(1)); -} diff --git a/src/integration_tests/camera_mode.cpp b/src/integration_tests/camera_mode.cpp deleted file mode 100644 index 1fe6a111d8..0000000000 --- a/src/integration_tests/camera_mode.cpp +++ /dev/null @@ -1,104 +0,0 @@ -#include -#include -#include "integration_test_helper.h" -#include "mavsdk.h" -#include "system.h" -#include "camera_test_helpers.h" - -using namespace mavsdk; - -TEST(CameraTest, SetModeSync) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - bool received_mode_change = false; - Camera::Mode last_mode = Camera::Mode::Unknown; - - camera->subscribe_mode([&received_mode_change, &last_mode](Camera::Mode mode) { - LogInfo() << "Got mode: " << int(mode); - received_mode_change = true; - last_mode = mode; - }); - - std::vector modes_to_test; - modes_to_test.push_back(Camera::Mode::Photo); - modes_to_test.push_back(Camera::Mode::Video); - modes_to_test.push_back(Camera::Mode::Photo); - modes_to_test.push_back(Camera::Mode::Video); - - for (auto mode : modes_to_test) { - auto result = camera->set_mode(mode); - EXPECT_EQ(result, Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - EXPECT_TRUE(received_mode_change); - EXPECT_EQ(last_mode, mode); - received_mode_change = false; - } -} - -TEST(CameraTest, SetModeAsync) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(2)); - Camera::Mode new_mode = camera->mode(); - EXPECT_EQ(new_mode, Camera::Mode::Photo); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Video), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(2)); - new_mode = camera->mode(); - EXPECT_EQ(new_mode, Camera::Mode::Video); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(2)); - new_mode = camera->mode(); - EXPECT_EQ(new_mode, Camera::Mode::Photo); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(2)); - new_mode = camera->mode(); - EXPECT_EQ(new_mode, Camera::Mode::Photo); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Video), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(2)); - new_mode = camera->mode(); - EXPECT_EQ(new_mode, Camera::Mode::Video); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Video), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(2)); - new_mode = camera->mode(); - EXPECT_EQ(new_mode, Camera::Mode::Video); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Video), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(2)); - new_mode = camera->mode(); - EXPECT_EQ(new_mode, Camera::Mode::Video); - std::this_thread::sleep_for(std::chrono::seconds(2)); -} diff --git a/src/integration_tests/camera_reset_settings.cpp b/src/integration_tests/camera_reset_settings.cpp deleted file mode 100644 index a5becaaeff..0000000000 --- a/src/integration_tests/camera_reset_settings.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "integration_test_helper.h" -#include "mavsdk.h" -#include -#include -#include -#include "plugins/camera/camera.h" - -using namespace mavsdk; - -TEST(CameraTest, ResetSettings) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - - auto camera = std::make_shared(system); - - EXPECT_EQ(Camera::Result::Success, camera->reset_settings()); -} diff --git a/src/integration_tests/camera_settings.cpp b/src/integration_tests/camera_settings.cpp deleted file mode 100644 index d0516df5c4..0000000000 --- a/src/integration_tests/camera_settings.cpp +++ /dev/null @@ -1,363 +0,0 @@ -#include -#include -#include -#include -#include -#include "mavsdk.h" -#include "system.h" -#include "integration_test_helper.h" -#include "camera_test_helpers.h" -#include "unused.h" - -using namespace mavsdk; - -// To run specific tests for Yuneec cameras. -const static bool is_e90 = false; -const static bool is_e50 = false; -const static bool is_et = false; - -void contains_num_options( - const std::vector& settings, std::string setting_id, unsigned num) -{ - const auto it = - std::find_if(settings.begin(), settings.end(), [&setting_id](Camera::SettingOptions elem) { - return elem.setting_id == setting_id; - }); - const bool found = it != settings.end(); - if (num > 0) { - EXPECT_TRUE(found); - if (found) { - EXPECT_EQ(it->options.size(), num); - } - } else { - EXPECT_FALSE(found); - } -} - -TEST(CameraTest, ShowSettingsAndOptions) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - // Wait for download to happen. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - if (is_e90 || is_e50 || is_et) { - // Set to photo mode - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto settings = camera->possible_setting_options(); - EXPECT_GT(settings.size(), 0); - - LogDebug() << "Possible settings in photo mode: "; - for (auto setting : settings) { - LogDebug() << "- " << setting; - } - - if (is_e90) { - EXPECT_EQ(settings.size(), 10); - } else if (is_e50) { - EXPECT_EQ(settings.size(), 6); - } else if (is_et) { - EXPECT_EQ(settings.size(), 5); - } - - EXPECT_EQ(camera->set_mode(Camera::Mode::Video), Camera::Result::Success); - - std::this_thread::sleep_for(std::chrono::seconds(2)); - - settings = camera->possible_setting_options(); - EXPECT_GT(settings.size(), 0); - - LogDebug() << "Possible settings in video mode: "; - for (auto setting : settings) { - LogDebug() << "-" << setting; - } - - if (is_e90) { - EXPECT_EQ(settings.size(), 8); - } else if (is_e50) { - EXPECT_EQ(settings.size(), 5); - } else if (is_et) { - EXPECT_EQ(settings.size(), 5); - } - - if (is_e90) { - // Try something that is specific to the camera mode. - contains_num_options(settings, "CAM_VIDRES", 32); - } else if (is_e50) { - // Try something that is specific to the camera mode. - contains_num_options(settings, "CAM_VIDRES", 12); - } - - if (is_e90) { - // This param is not applicable, so we should get an empty vector back. - contains_num_options(settings, "CAM_PHOTOQUAL", 0); - } - - // The same should happen with a param that does not exist at all. - contains_num_options(settings, "CAM_BLABLA", 0); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - - if (is_e90) { - // Try something that is specific to the camera mode. - contains_num_options(settings, "CAM_PHOTOQUAL", 4); - } - - // This param is not applicable, so we should get an empty vector back. - contains_num_options(settings, "CAM_VIDRES", 0); - - // The same should happen with a param that does not exist at all. - contains_num_options(settings, "CAM_BLABLA", 0); - } -} - -TEST(CameraTest, SetSettings) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult connection_ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(connection_ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - // We need to wait for the camera definition to be ready - // because we don't have a check yet. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - - // Try setting garbage first - EXPECT_EQ(set_setting(camera, "DOES_NOT", "EXIST"), Camera::Result::Error); - - if (is_e90) { - std::string value_set; - EXPECT_EQ(set_setting(camera, "CAM_PHOTOQUAL", "1"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_PHOTOQUAL", value_set), Camera::Result::Success); - EXPECT_STREQ("1", value_set.c_str()); - - EXPECT_EQ(set_setting(camera, "CAM_COLORMODE", "5"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_COLORMODE", value_set), Camera::Result::Success); - EXPECT_STREQ("5", value_set.c_str()); - - EXPECT_EQ(set_setting(camera, "CAM_COLORMODE", "1"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_COLORMODE", value_set), Camera::Result::Success); - EXPECT_STREQ("1", value_set.c_str()); - - EXPECT_EQ(set_setting(camera, "CAM_COLORMODE", "3"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_COLORMODE", value_set), Camera::Result::Success); - EXPECT_STREQ("3", value_set.c_str()); - - // Let's check the manual exposure mode first. - std::this_thread::sleep_for(std::chrono::seconds(2)); - EXPECT_EQ(set_setting(camera, "CAM_EXPMODE", "1"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_EXPMODE", value_set), Camera::Result::Success); - EXPECT_STREQ("1", value_set.c_str()); - - auto settings = camera->possible_setting_options(); - - // Try something that is specific to the camera mode. - contains_num_options(settings, "CAM_SHUTTERSPD", 19); - contains_num_options(settings, "CAM_ISO", 10); - - // But not EV and metering - contains_num_options(settings, "CAM_EV", 0); - contains_num_options(settings, "CAM_METERING", 0); - - // Now we'll try the same for Auto exposure mode - EXPECT_EQ(set_setting(camera, "CAM_EXPMODE", "0"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_EXPMODE", value_set), Camera::Result::Success); - EXPECT_STREQ("0", value_set.c_str()); - - settings = camera->possible_setting_options(); - - // Shutter speed and ISO don't have options in Auto mode. - contains_num_options(settings, "CAM_SHUTTERSPD", 0); - contains_num_options(settings, "CAM_ISO", 0); - - // But not EV and metering - contains_num_options(settings, "CAM_EV", 13); - contains_num_options(settings, "CAM_METERING", 3); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Video), Camera::Result::Success); - - UNUSED(camera->possible_setting_options()); - - // This should fail in video mode. - EXPECT_EQ(set_setting(camera, "CAM_PHOTOQUAL", "1"), Camera::Result::Error); - - // Let's check the manual exposure mode first. - - EXPECT_EQ(set_setting(camera, "CAM_EXPMODE", "1"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_EXPMODE", value_set), Camera::Result::Success); - EXPECT_STREQ("1", value_set.c_str()); - - // FIXME: otherwise the camera is too slow - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // But a video setting should work - EXPECT_EQ(set_setting(camera, "CAM_VIDRES", "30"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_VIDRES", value_set), Camera::Result::Success); - EXPECT_STREQ("30", value_set.c_str()); - - settings = camera->possible_setting_options(); - - // Shutter speed and ISO don't have options in Auto mode. - contains_num_options(settings, "CAM_SHUTTERSPD", 15); - - // FIXME: otherwise the camera is too slow - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // But a video setting should work - EXPECT_EQ(set_setting(camera, "CAM_VIDRES", "0"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_VIDRES", value_set), Camera::Result::Success); - EXPECT_STREQ("0", value_set.c_str()); - - settings = camera->possible_setting_options(); - - // Shutter speed and ISO don't have options in Auto mode. - contains_num_options(settings, "CAM_SHUTTERSPD", 12); - - // Back to auto exposure mode - std::this_thread::sleep_for(std::chrono::seconds(2)); - EXPECT_EQ(set_setting(camera, "CAM_EXPMODE", "0"), Camera::Result::Success); - EXPECT_EQ(get_setting(camera, "CAM_EXPMODE", value_set), Camera::Result::Success); - EXPECT_STREQ("0", value_set.c_str()); - } -} - -static void -receive_current_settings(bool& subscription_called, const std::vector& settings) -{ - LogDebug() << "Received current options:"; - EXPECT_TRUE(settings.size() > 0); - for (auto& setting : settings) { - LogDebug() << "Got setting '" << setting.setting_description << "' with selected option '" - << setting.option.option_description << "'"; - - // Check human readable strings too. - if (setting.setting_id == "CAM_SHUTTERSPD") { - EXPECT_STREQ(setting.setting_description.c_str(), "Shutter Speed"); - } else if (setting.setting_id == "CAM_EXPMODE") { - EXPECT_STREQ(setting.setting_description.c_str(), "Exposure Mode"); - } - } - subscription_called = true; -} - -TEST(CameraTest, SubscribeCurrentSettings) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult connection_ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(connection_ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - // We need to wait for the camera definition to be ready - // because we don't have a check yet. - std::this_thread::sleep_for(std::chrono::seconds(2)); - // Reset exposure mode - EXPECT_EQ(set_setting(camera, "CAM_EXPMODE", "0"), Camera::Result::Success); - - bool subscription_called = false; - camera->subscribe_current_settings( - [&subscription_called](const std::vector& settings) { - receive_current_settings(subscription_called, settings); - }); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(1)); - EXPECT_TRUE(subscription_called); - - subscription_called = false; - EXPECT_EQ(set_setting(camera, "CAM_EXPMODE", "1"), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(1)); - EXPECT_TRUE(subscription_called); -} - -static void receive_possible_setting_options( - bool& subscription_called, const std::vector& settings_options) -{ - LogDebug() << "Received possible options:"; - EXPECT_TRUE(settings_options.size() > 0); - for (auto& setting_options : settings_options) { - LogDebug() << "Got setting '" << setting_options.setting_description << "' with options:"; - - // Check human readable strings too. - if (setting_options.setting_id == "CAM_SHUTTERSPD") { - EXPECT_STREQ(setting_options.setting_description.c_str(), "Shutter Speed"); - } else if (setting_options.setting_id == "CAM_EXPMODE") { - EXPECT_STREQ(setting_options.setting_description.c_str(), "Exposure Mode"); - } - - EXPECT_TRUE(setting_options.options.size() > 0); - for (auto& option : setting_options.options) { - LogDebug() << " - '" << option.option_description << "'"; - if (setting_options.setting_id == "CAM_SHUTTERSPD" && option.option_id == "0.0025") { - EXPECT_STREQ(option.option_description.c_str(), "1/400"); - } else if (setting_options.setting_id == "CAM_WBMODE" && option.option_id == "2") { - EXPECT_STREQ(option.option_description.c_str(), "Sunrise"); - } - } - } - subscription_called = true; -} - -TEST(CameraTest, SubscribePossibleSettings) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult connection_ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(connection_ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - // We need to wait for the camera definition to be ready - // because we don't have a check yet. - std::this_thread::sleep_for(std::chrono::seconds(2)); - // Reset exposure mode - EXPECT_EQ(set_setting(camera, "CAM_EXPMODE", "0"), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - bool subscription_called = false; - camera->subscribe_possible_setting_options( - [&subscription_called](const std::vector& possible_settings) { - receive_possible_setting_options(subscription_called, possible_settings); - }); - - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(1)); - EXPECT_TRUE(subscription_called); - - subscription_called = false; - EXPECT_EQ(set_setting(camera, "CAM_EXPMODE", "1"), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(1)); - EXPECT_TRUE(subscription_called); -} diff --git a/src/integration_tests/camera_status.cpp b/src/integration_tests/camera_status.cpp deleted file mode 100644 index 8ba706850a..0000000000 --- a/src/integration_tests/camera_status.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "integration_test_helper.h" -#include "mavsdk.h" -#include -#include -#include -#include "plugins/camera/camera.h" - -using namespace mavsdk; - -static void print_camera_status(const Camera::Status& status); - -TEST(CameraTest, Status) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - bool received_status = false; - camera->subscribe_status([&received_status](Camera::Status status) { - received_status = true; - print_camera_status(status); - }); - std::this_thread::sleep_for(std::chrono::seconds(5)); - EXPECT_TRUE(received_status); -} - -static void print_camera_status(const Camera::Status& status) -{ - std::string storage_status_str = ""; - switch (status.storage_status) { - case Camera::Status::StorageStatus::NotAvailable: - storage_status_str = "not available"; - break; - case Camera::Status::StorageStatus::Unformatted: - storage_status_str = "unformatted"; - break; - case Camera::Status::StorageStatus::Formatted: - storage_status_str = "formatted"; - break; - case Camera::Status::StorageStatus::NotSupported: - storage_status_str = "not supported"; - break; - } - - LogDebug() << "Status"; - LogDebug() << "------"; - LogDebug() << "Video: " << (status.video_on ? "on" : "off"); - LogDebug() << "Photo interval: " << (status.photo_interval_on ? "on" : "off"); - LogDebug() << "Storage status: " << storage_status_str; - LogDebug() << "Used: " << status.used_storage_mib / 1024.0f << " GiB"; - LogDebug() << "Available: " << status.available_storage_mib / 1024.0f << " GiB"; - LogDebug() << "Total: " << status.total_storage_mib / 1024.0f << " GiB"; - LogDebug() << "Folder: " << status.media_folder_name; - LogDebug() << "------"; -} diff --git a/src/integration_tests/camera_take_photo.cpp b/src/integration_tests/camera_take_photo.cpp deleted file mode 100644 index 875442a7fb..0000000000 --- a/src/integration_tests/camera_take_photo.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include "integration_test_helper.h" -#include "mavsdk.h" -#include "system.h" -#include "plugins/camera/camera.h" -#include "plugins/camera_server/camera_server.h" - -using namespace mavsdk; - -static void receive_capture_info(Camera::CaptureInfo capture_info, bool& received_capture_info); - -TEST(CameraTest, TakePhotoSingle) -{ - Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}}; - - Mavsdk mavsdk_camera{Mavsdk::Configuration{ComponentType::Camera}}; - - ASSERT_EQ(mavsdk_groundstation.add_any_connection("udp://:17000"), ConnectionResult::Success); - ASSERT_EQ(mavsdk_camera.add_any_connection("udp://127.0.0.1:17000"), ConnectionResult::Success); - - auto camera_server = CameraServer{mavsdk_camera.server_component()}; - camera_server.subscribe_take_photo([&camera_server](int32_t index) { - LogInfo() << "Let's take photo " << index; - - CameraServer::CaptureInfo info; - info.index = index; - info.is_success = true; - - camera_server.respond_take_photo(CameraServer::CameraFeedback::Ok, info); - }); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - ASSERT_EQ(mavsdk_groundstation.systems().size(), 1); - auto system = mavsdk_groundstation.systems().at(0); - ASSERT_TRUE(system->has_camera()); - - auto camera = Camera{system}; - - // We want to take the picture in photo mode. - EXPECT_EQ(camera.set_mode(Camera::Mode::Photo), Camera::Result::Success); - - bool received_capture_info = false; - camera.subscribe_capture_info([&received_capture_info](Camera::CaptureInfo capture_info) { - receive_capture_info(capture_info, received_capture_info); - }); - - EXPECT_EQ(camera.take_photo(), Camera::Result::Success); - std::this_thread::sleep_for(std::chrono::seconds(1)); - EXPECT_TRUE(received_capture_info); -} - -TEST(CameraTest, TakePhotosMultiple) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - const int num_photos_to_take = 3; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - // We want to take the picture in photo mode. - EXPECT_EQ(camera->set_mode(Camera::Mode::Photo), Camera::Result::Success); - - std::this_thread::sleep_for(std::chrono::seconds(1)); - - bool received_capture_info = false; - camera->subscribe_capture_info([&received_capture_info](Camera::CaptureInfo capture_info) { - receive_capture_info(capture_info, received_capture_info); - }); - - for (unsigned i = 0; i < num_photos_to_take; ++i) { - const auto result = camera->take_photo(); - EXPECT_EQ(result, Camera::Result::Success); - LogDebug() << "taking picture: " << i; - std::this_thread::sleep_for(std::chrono::seconds(5)); - EXPECT_TRUE(received_capture_info); - received_capture_info = false; - } -} - -void receive_capture_info(Camera::CaptureInfo capture_info, bool& received_capture_info) -{ - LogInfo() << "New capture at " << capture_info.position.latitude_deg << ", " - << capture_info.position.longitude_deg << ", " - << capture_info.position.absolute_altitude_m << " m, " - << capture_info.position.relative_altitude_m << " m (relative to home)."; - LogInfo() << "Time: " << capture_info.time_utc_us << " us."; - LogInfo() << "Attitude: " << capture_info.attitude_quaternion.w << ", " - << capture_info.attitude_quaternion.x << ", " << capture_info.attitude_quaternion.y - << ", " << capture_info.attitude_quaternion.z << "."; - LogInfo() << "Result: " << (capture_info.is_success ? "success" : "fail") << "."; - LogInfo() << "Saved to " << capture_info.file_url << " (" << capture_info.index << ")."; - - received_capture_info = true; -} diff --git a/src/integration_tests/camera_take_photo_interval.cpp b/src/integration_tests/camera_take_photo_interval.cpp deleted file mode 100644 index 4dc8767bc1..0000000000 --- a/src/integration_tests/camera_take_photo_interval.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include -#include -#include -#include -#include -#include "integration_test_helper.h" -#include "mavsdk.h" -#include "system.h" -#include "camera_test_helpers.h" - -using namespace mavsdk; -static void receive_camera_result(Camera::Result result); - -static void check_interval_on(std::shared_ptr camera, bool on); - -static std::atomic _received_result{false}; - -TEST(CameraTest, TakePhotoInterval) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_camera()); - auto camera = std::make_shared(system); - - // We want to take the pictures in photo mode. - camera->set_mode(Camera::Mode::Photo); - - check_interval_on(camera, false); - - std::this_thread::sleep_for(std::chrono::seconds(1)); - - _received_result = false; - camera->start_photo_interval_async( - 2.0f, [](Camera::Result result) { return receive_camera_result(result); }); - - // Wait for 3 photos - std::this_thread::sleep_for(std::chrono::seconds(7)); - - check_interval_on(camera, true); - - // Wait for another photo - std::this_thread::sleep_for(std::chrono::seconds(2)); - - // Then, stop it again. - camera->stop_photo_interval_async( - [](Camera::Result result) { return receive_camera_result(result); }); - - std::this_thread::sleep_for(std::chrono::seconds(1)); - check_interval_on(camera, false); - - EXPECT_TRUE(_received_result); -} - -void receive_camera_result(Camera::Result result) -{ - _received_result = true; - EXPECT_EQ(result, Camera::Result::Success); -} - -void check_interval_on(std::shared_ptr camera, bool on) -{ - auto prom = std::make_shared>(); - auto ret = prom->get_future(); - - // Check if status is correct - Camera::StatusHandle handle = - camera->subscribe_status([prom, on, camera, &handle](Camera::Status status) { - camera->unsubscribe_status(handle); - EXPECT_EQ(status.photo_interval_on, on); - prom->set_value(); - }); - - // Block now for a while to wait for result. - auto status = ret.wait_for(std::chrono::seconds(2)); - - EXPECT_EQ(status, std::future_status::ready); -} diff --git a/src/integration_tests/camera_test_helpers.cpp b/src/integration_tests/camera_test_helpers.cpp deleted file mode 100644 index adbfd81f76..0000000000 --- a/src/integration_tests/camera_test_helpers.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include -#include -#include "integration_test_helper.h" -#include "camera_test_helpers.h" - -using namespace mavsdk; - -Camera::Result set_setting( - std::shared_ptr camera, const std::string& setting_id, const std::string& option_id) -{ - Camera::Setting new_setting{}; - new_setting.setting_id = setting_id; - new_setting.option.option_id = option_id; - return camera->set_setting(new_setting); -} - -mavsdk::Camera::Result get_setting( - std::shared_ptr camera, const std::string& setting_id, std::string& option_id) -{ - Camera::Setting new_setting{}; - new_setting.setting_id = setting_id; - auto result_pair = camera->get_setting(new_setting); - option_id = result_pair.second.option.option_id; - return result_pair.first; -} diff --git a/src/integration_tests/camera_test_helpers.h b/src/integration_tests/camera_test_helpers.h deleted file mode 100644 index bec703ef23..0000000000 --- a/src/integration_tests/camera_test_helpers.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include "mavsdk.h" -#include "plugins/camera/camera.h" - -mavsdk::Camera::Result set_setting( - std::shared_ptr camera, - const std::string& setting_id, - const std::string& option_id); - -mavsdk::Camera::Result get_setting( - std::shared_ptr camera, const std::string& setting_id, std::string& option_d); diff --git a/src/integration_tests/logging.cpp b/src/integration_tests/logging.cpp deleted file mode 100644 index 96ff41f17f..0000000000 --- a/src/integration_tests/logging.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include -#include "mavsdk.h" -#include "integration_test_helper.h" -#include "plugins/logging/logging.h" - -using namespace mavsdk; - -TEST(SitlTest, PX4Logging) -{ - Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - - ConnectionResult ret = mavsdk.add_any_connection("udpin://0.0.0.0:14540"); - ASSERT_EQ(ret, ConnectionResult::Success); - - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto system = mavsdk.systems().at(0); - ASSERT_TRUE(system->has_autopilot()); - auto logging = std::make_shared(system); - Logging::Result log_ret = logging->start_logging(); - - if (log_ret == Logging::Result::COMMAND_DENIED) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - logging->stop_logging(); - // ASSERT_EQ(log_ret, Logging::Result::SUCCESS); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - log_ret = logging->start_logging(); - } - - ASSERT_EQ(log_ret, Logging::Result::SUCCESS); - - for (unsigned i = 0; i < 10; ++i) { - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - log_ret = logging->stop_logging(); - ASSERT_EQ(log_ret, Logging::Result::SUCCESS); -} diff --git a/src/integration_tests/offboard_position.cpp b/src/integration_tests/offboard_position.cpp index 06f420245c..02a2a5da5e 100644 --- a/src/integration_tests/offboard_position.cpp +++ b/src/integration_tests/offboard_position.cpp @@ -2,7 +2,6 @@ #include #include "integration_test_helper.h" #include "mavsdk.h" -#include "mavsdk_math.h" #include "plugins/action/action.h" #include "plugins/telemetry/telemetry.h" #include "plugins/offboard/offboard.h" @@ -63,7 +62,7 @@ TEST(SitlTest, OffboardPositionNED) const float radius = 10.0f; const float step = 0.01f; float angle = 0.0f; - while (angle <= 2.0f * static_cast(PI)) { + while (angle <= 2.0f * static_cast(M_PI)) { float x = radius * cosf(angle); float y = radius * sinf(angle); diff --git a/src/integration_tests/offboard_velocity.cpp b/src/integration_tests/offboard_velocity.cpp index 32552f8f79..fcb3b960a3 100644 --- a/src/integration_tests/offboard_velocity.cpp +++ b/src/integration_tests/offboard_velocity.cpp @@ -2,7 +2,6 @@ #include #include "integration_test_helper.h" #include "mavsdk.h" -#include "mavsdk_math.h" #include "plugins/action/action.h" #include "plugins/telemetry/telemetry.h" #include "plugins/offboard/offboard.h" @@ -60,7 +59,7 @@ TEST(SitlTest, OffboardVelocityNED) { const float step_size = 0.01f; - const float one_cycle = 2.0f * static_cast(PI); + const float one_cycle = 2.0f * static_cast(M_PI); const unsigned steps = 2 * unsigned(one_cycle / step_size); for (unsigned i = 0; i < steps; ++i) { diff --git a/src/mavsdk/core/CMakeLists.txt b/src/mavsdk/core/CMakeLists.txt index ff2a460de5..42756570f6 100644 --- a/src/mavsdk/core/CMakeLists.txt +++ b/src/mavsdk/core/CMakeLists.txt @@ -26,7 +26,7 @@ target_sources(mavsdk fs_utils.cpp hostname_to_ip.cpp inflate_lzma.cpp - math_conversions.cpp + math_utils.cpp mavsdk.cpp mavsdk_impl.cpp mavlink_channels.cpp @@ -63,6 +63,7 @@ target_sources(mavsdk cli_arg.cpp geometry.cpp mavsdk_time.cpp + string_utils.cpp timesync.cpp ) @@ -188,8 +189,7 @@ list(APPEND UNIT_TEST_SOURCES ${PROJECT_SOURCE_DIR}/mavsdk/core/file_cache_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/locked_queue_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/geometry_test.cpp - ${PROJECT_SOURCE_DIR}/mavsdk/core/math_conversions_test.cpp - ${PROJECT_SOURCE_DIR}/mavsdk/core/mavsdk_math_test.cpp + ${PROJECT_SOURCE_DIR}/mavsdk/core/math_utils_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/mavsdk_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/mavsdk_time_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_channels_test.cpp @@ -202,5 +202,6 @@ list(APPEND UNIT_TEST_SOURCES ${PROJECT_SOURCE_DIR}/mavsdk/core/timeout_handler_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/unittests_main.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_parameter_cache_test.cpp + ${PROJECT_SOURCE_DIR}/mavsdk/core/string_utils_test.cpp ) set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE) diff --git a/src/mavsdk/core/curl_wrapper.cpp b/src/mavsdk/core/curl_wrapper.cpp index 94cf34debb..8c99cbaba3 100644 --- a/src/mavsdk/core/curl_wrapper.cpp +++ b/src/mavsdk/core/curl_wrapper.cpp @@ -78,14 +78,14 @@ static int download_progress_update( } if (dltotal == 0 || dlnow == 0) { - return myp->progress_callback(0, HttpStatus::Idle, CURLcode::CURLE_OK); - } - - int percentage = static_cast(100 / dltotal * dlnow); + myp->progress_callback(0, HttpStatus::Idle, CURLcode::CURLE_OK); + } else { + int percentage = static_cast(100 * dlnow / dltotal); - if (percentage > myp->progress_in_percentage) { - myp->progress_in_percentage = percentage; - return myp->progress_callback(percentage, HttpStatus::Downloading, CURLcode::CURLE_OK); + if (percentage > myp->progress_in_percentage) { + myp->progress_in_percentage = percentage; + myp->progress_callback(percentage, HttpStatus::Downloading, CURLcode::CURLE_OK); + } } return 0; diff --git a/src/mavsdk/core/curl_wrapper_types.h b/src/mavsdk/core/curl_wrapper_types.h index 3be133e42b..1a4ae7ab92 100644 --- a/src/mavsdk/core/curl_wrapper_types.h +++ b/src/mavsdk/core/curl_wrapper_types.h @@ -6,7 +6,7 @@ namespace mavsdk { enum class HttpStatus { Idle = 0, Downloading = 1, Uploading = 2, Finished = 3, Error = 4 }; -using ProgressCallback = std::function; +using ProgressCallback = std::function; struct UpProgress { int progress_in_percentage = 0; diff --git a/src/mavsdk/core/fs_utils.cpp b/src/mavsdk/core/fs_utils.cpp index e376cd4abe..1b86dde212 100644 --- a/src/mavsdk/core/fs_utils.cpp +++ b/src/mavsdk/core/fs_utils.cpp @@ -1,8 +1,11 @@ #include "fs_utils.h" #include "log.h" +#include +#include #include #include +#include #if defined(LINUX) || defined(APPLE) #include @@ -112,4 +115,17 @@ std::optional create_tmp_directory(const std::string& pre LogErr() << "Could not create a temporary directory, aborting."; return std::nullopt; } + +std::string replace_non_ascii_and_whitespace(const std::string& input) +{ + std::string result = input; + std::transform(result.begin(), result.end(), result.begin(), [](unsigned char ch) { + if (ch > 127 || std::isspace(ch)) { + return '_'; + } + return static_cast(ch); + }); + return result; +} + } // namespace mavsdk diff --git a/src/mavsdk/core/fs_utils.h b/src/mavsdk/core/fs_utils.h index 86bdb471e1..cae2abed2e 100644 --- a/src/mavsdk/core/fs_utils.h +++ b/src/mavsdk/core/fs_utils.h @@ -17,4 +17,6 @@ std::optional get_cache_directory(); */ std::optional create_tmp_directory(const std::string& prefix); +std::string replace_non_ascii_and_whitespace(const std::string& input); + } // namespace mavsdk diff --git a/src/mavsdk/core/geometry.cpp b/src/mavsdk/core/geometry.cpp index d68959a01f..5acc78989e 100644 --- a/src/mavsdk/core/geometry.cpp +++ b/src/mavsdk/core/geometry.cpp @@ -1,5 +1,5 @@ #include "geometry.h" -#include "mavsdk_math.h" +#include "math_utils.h" #include namespace mavsdk::geometry { diff --git a/src/mavsdk/core/http_loader.cpp b/src/mavsdk/core/http_loader.cpp index b47208db59..c364a2f995 100644 --- a/src/mavsdk/core/http_loader.cpp +++ b/src/mavsdk/core/http_loader.cpp @@ -46,9 +46,7 @@ bool HttpLoader::download_sync(const std::string& url, const std::string& local_ } void HttpLoader::download_async( - const std::string& url, - const std::string& local_path, - const ProgressCallback& progress_callback) + const std::string& url, const std::string& local_path, ProgressCallback progress_callback) { auto work_item = std::make_shared(url, local_path, progress_callback); _work_queue.enqueue(work_item); diff --git a/src/mavsdk/core/http_loader.h b/src/mavsdk/core/http_loader.h index 1755f7362d..094dfb9c0b 100644 --- a/src/mavsdk/core/http_loader.h +++ b/src/mavsdk/core/http_loader.h @@ -18,7 +18,7 @@ class HttpLoader { HttpLoader(const std::shared_ptr& curl_wrapper); #endif - explicit HttpLoader(); + HttpLoader(); ~HttpLoader(); void start(); @@ -29,13 +29,7 @@ class HttpLoader { void download_async( const std::string& url, const std::string& local_path, - const ProgressCallback& progress_callback = nullptr); - - bool upload_sync(const std::string& target_url, const std::string& local_path); - void upload_async( - const std::string& target_url, - const std::string& local_path, - const ProgressCallback& progress_callback = nullptr); + ProgressCallback progress_callback = nullptr); // Non-copyable HttpLoader(const HttpLoader&) = delete; diff --git a/src/mavsdk/core/math_conversions.h b/src/mavsdk/core/math_conversions.h deleted file mode 100644 index d6f1e81e8c..0000000000 --- a/src/mavsdk/core/math_conversions.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -namespace mavsdk { - -struct Quaternion { - float w; - float x; - float y; - float z; -}; - -struct EulerAngle { - float roll_deg; - float pitch_deg; - float yaw_deg; -}; - -bool operator==(const Quaternion& lhs, const Quaternion& rhs); -bool operator==(const EulerAngle& lhs, const EulerAngle& rhs); - -EulerAngle to_euler_angle_from_quaternion(Quaternion quaternion); -Quaternion to_quaternion_from_euler_angle(EulerAngle euler_angle); - -Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs); - -} // namespace mavsdk diff --git a/src/mavsdk/core/math_conversions.cpp b/src/mavsdk/core/math_utils.cpp similarity index 98% rename from src/mavsdk/core/math_conversions.cpp rename to src/mavsdk/core/math_utils.cpp index 895a7e4151..1dca42450a 100644 --- a/src/mavsdk/core/math_conversions.cpp +++ b/src/mavsdk/core/math_utils.cpp @@ -1,5 +1,4 @@ -#include "mavsdk_math.h" -#include "math_conversions.h" +#include "math_utils.h" #include namespace mavsdk { diff --git a/src/mavsdk/core/mavsdk_math.h b/src/mavsdk/core/math_utils.h similarity index 59% rename from src/mavsdk/core/mavsdk_math.h rename to src/mavsdk/core/math_utils.h index 5839f145f2..4c08584aa0 100644 --- a/src/mavsdk/core/mavsdk_math.h +++ b/src/mavsdk/core/math_utils.h @@ -2,6 +2,27 @@ namespace mavsdk { +struct Quaternion { + float w; + float x; + float y; + float z; +}; + +struct EulerAngle { + float roll_deg; + float pitch_deg; + float yaw_deg; +}; + +bool operator==(const Quaternion& lhs, const Quaternion& rhs); +bool operator==(const EulerAngle& lhs, const EulerAngle& rhs); + +EulerAngle to_euler_angle_from_quaternion(Quaternion quaternion); +Quaternion to_quaternion_from_euler_angle(EulerAngle euler_angle); + +Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs); + // Instead of using the constant from math.h or cmath we define it ourselves. This way // we don't import all the other C math functions and make sure to use the C++ functions // from the standard library (e.g. std::abs() instead of abs()). diff --git a/src/mavsdk/core/math_conversions_test.cpp b/src/mavsdk/core/math_utils_test.cpp similarity index 72% rename from src/mavsdk/core/math_conversions_test.cpp rename to src/mavsdk/core/math_utils_test.cpp index 8ad5347a7c..1a9b595e98 100644 --- a/src/mavsdk/core/math_conversions_test.cpp +++ b/src/mavsdk/core/math_utils_test.cpp @@ -1,11 +1,15 @@ -#include "math_conversions.h" -#include +#include "math_utils.h" #include "mavlink_include.h" -#include "mavsdk_math.h" +#include +#include using namespace mavsdk; -TEST(MathConversions, QuaternionToEulerAnglesAndBackBaseCase) +#ifndef M_PI_F +#define M_PI_F static_cast(M_PI) +#endif + +TEST(MathUtils, QuaternionToEulerAnglesAndBackBaseCase) { Quaternion q1; q1.w = 1.0f; @@ -56,7 +60,7 @@ TEST(MathConversions, QuaternionToEulerAnglesAndBackBaseCase) // Example taken from https://quaternions.online/ with Euler angle ZYX-order. // The accuracy was not great and I had to increase the test margins quite a // bit to make the test pass. I'm not sure where the errors come from. -TEST(MathConversions, QuaternionToEulerAndBackSomeCase) +TEST(MathUtils, QuaternionToEulerAndBackSomeCase) { Quaternion q1; q1.w = 0.143f; @@ -107,7 +111,7 @@ TEST(MathConversions, QuaternionToEulerAndBackSomeCase) EXPECT_NEAR(q2.z, q2_mavlink[3], 0.01f); } -TEST(MathConversions, QuaternionRotation) +TEST(MathUtils, QuaternionRotation) { // Define a sample quaternion auto sample_q = to_quaternion_from_euler_angle(EulerAngle{30.f, 60.f, -45.f}); @@ -125,3 +129,38 @@ TEST(MathConversions, QuaternionRotation) EXPECT_NEAR(expected_q.y, rotated_q.y, 1e-3); EXPECT_NEAR(expected_q.z, rotated_q.z, 1e-3); } + +TEST(MathUtils, OurPi) +{ + ASSERT_DOUBLE_EQ(PI, M_PI); +} + +TEST(MathUtils, RadDegDouble) +{ + ASSERT_DOUBLE_EQ(0.0, to_rad_from_deg(0.0)); + ASSERT_DOUBLE_EQ(M_PI / 2.0, to_rad_from_deg(90.0)); + ASSERT_DOUBLE_EQ(M_PI, to_rad_from_deg(180.0)); + ASSERT_DOUBLE_EQ(-M_PI, to_rad_from_deg(-180.0)); + ASSERT_DOUBLE_EQ(2.0 * M_PI, to_rad_from_deg(360.0)); + + ASSERT_DOUBLE_EQ(0.0, to_deg_from_rad(0.0)); + ASSERT_DOUBLE_EQ(90.0, to_deg_from_rad(M_PI / 2.0)); + ASSERT_DOUBLE_EQ(180, to_deg_from_rad(M_PI)); + ASSERT_DOUBLE_EQ(-180, to_deg_from_rad(-M_PI)); + ASSERT_DOUBLE_EQ(360.0, to_deg_from_rad(2.0 * M_PI)); +} + +TEST(MathUtils, RadDegFloat) +{ + ASSERT_FLOAT_EQ(0.0f, to_rad_from_deg(0.0f)); + ASSERT_FLOAT_EQ(M_PI_F / 2.0f, to_rad_from_deg(90.0f)); + ASSERT_FLOAT_EQ(M_PI_F, to_rad_from_deg(180.0f)); + ASSERT_FLOAT_EQ(-M_PI_F, to_rad_from_deg(-180.0f)); + ASSERT_FLOAT_EQ(2.0f * M_PI_F, to_rad_from_deg(360.0f)); + + ASSERT_FLOAT_EQ(0.0f, to_deg_from_rad(0.0f)); + ASSERT_FLOAT_EQ(90.0f, to_deg_from_rad(M_PI_F / 2.0f)); + ASSERT_FLOAT_EQ(180.0f, to_deg_from_rad(M_PI_F)); + ASSERT_FLOAT_EQ(-180.0f, to_deg_from_rad(-M_PI_F)); + ASSERT_FLOAT_EQ(360.0f, to_deg_from_rad(2.0f * M_PI_F)); +} diff --git a/src/mavsdk/core/mavlink_command_sender.cpp b/src/mavsdk/core/mavlink_command_sender.cpp index 5ed98b3406..9bcacf1ee1 100644 --- a/src/mavsdk/core/mavlink_command_sender.cpp +++ b/src/mavsdk/core/mavlink_command_sender.cpp @@ -36,49 +36,55 @@ MavlinkCommandSender::~MavlinkCommandSender() } } -MavlinkCommandSender::Result -MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandInt& command) +MavlinkCommandSender::Result MavlinkCommandSender::send_command( + const MavlinkCommandSender::CommandInt& command, unsigned retries) { // We wrap the async call with a promise and future. auto prom = std::make_shared>(); auto res = prom->get_future(); - queue_command_async(command, [prom](Result result, float progress) { - UNUSED(progress); - // We can only fulfill the promise once in C++11. - // Therefore, we have to ignore the IN_PROGRESS state and wait - // for the final result. - if (result != Result::InProgress) { - prom->set_value(result); - } - }); + queue_command_async( + command, + [prom](Result result, float progress) { + UNUSED(progress); + // We can only fulfill the promise once in C++11. + // Therefore, we have to ignore the IN_PROGRESS state and wait + // for the final result. + if (result != Result::InProgress) { + prom->set_value(result); + } + }, + retries); // Block now to wait for result. return res.get(); } -MavlinkCommandSender::Result -MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandLong& command) +MavlinkCommandSender::Result MavlinkCommandSender::send_command( + const MavlinkCommandSender::CommandLong& command, unsigned retries) { // We wrap the async call with a promise and future. auto prom = std::make_shared>(); auto res = prom->get_future(); - queue_command_async(command, [prom](Result result, float progress) { - UNUSED(progress); - // We can only fulfill the promise once in C++11. - // Therefore, we have to ignore the IN_PROGRESS state and wait - // for the final result. - if (result != Result::InProgress) { - prom->set_value(result); - } - }); + queue_command_async( + command, + [prom](Result result, float progress) { + UNUSED(progress); + // We can only fulfill the promise once in C++11. + // Therefore, we have to ignore the IN_PROGRESS state and wait + // for the final result. + if (result != Result::InProgress) { + prom->set_value(result); + } + }, + retries); return res.get(); } void MavlinkCommandSender::queue_command_async( - const CommandInt& command, const CommandResultCallback& callback) + const CommandInt& command, const CommandResultCallback& callback, unsigned retries) { if (_command_debugging) { LogDebug() << "COMMAND_INT " << static_cast(command.command) << " to send to " @@ -106,11 +112,12 @@ void MavlinkCommandSender::queue_command_async( new_work->command = command; new_work->identification = identification; new_work->callback = callback; + new_work->retries_to_do = retries; _work_queue.push_back(new_work); } void MavlinkCommandSender::queue_command_async( - const CommandLong& command, const CommandResultCallback& callback) + const CommandLong& command, const CommandResultCallback& callback, unsigned retries) { if (_command_debugging) { LogDebug() << "COMMAND_LONG " << static_cast(command.command) << " to send to " @@ -139,6 +146,7 @@ void MavlinkCommandSender::queue_command_async( new_work->identification = identification; new_work->callback = callback; new_work->time_started = _system_impl.get_time().steady_time(); + new_work->retries_to_do = retries; _work_queue.push_back(new_work); } @@ -364,12 +372,12 @@ void MavlinkCommandSender::receive_timeout(const CommandIdentification& identifi LogErr() << "No command, that's awkward"; continue; } - LogErr() << "Retrying failed for REQUEST_MESSAGE command for message: " - << work->identification.maybe_param1 << ", to (" - << std::to_string(target_sysid) << "/" << std::to_string(target_compid) - << ")"; + LogWarn() << "Retrying failed for REQUEST_MESSAGE command for message: " + << work->identification.maybe_param1 << ", to (" + << std::to_string(target_sysid) << "/" << std::to_string(target_compid) + << ")"; } else { - LogErr() << "Retrying failed for command: " << work->identification.command << ")"; + LogWarn() << "Retrying failed for command: " << work->identification.command << ")"; } temp_callback = work->callback; diff --git a/src/mavsdk/core/mavlink_command_sender.h b/src/mavsdk/core/mavlink_command_sender.h index 143c39975e..4bafdaeaea 100644 --- a/src/mavsdk/core/mavlink_command_sender.h +++ b/src/mavsdk/core/mavlink_command_sender.h @@ -21,6 +21,8 @@ class MavlinkCommandSender { explicit MavlinkCommandSender(SystemImpl& system_impl); ~MavlinkCommandSender(); + static constexpr unsigned DEFAULT_RETRIES = 3; + enum class Result { Success = 0, NoSystem, @@ -73,11 +75,17 @@ class MavlinkCommandSender { } params{}; }; - Result send_command(const CommandInt& command); - Result send_command(const CommandLong& command); + Result send_command(const CommandInt& command, unsigned retries = DEFAULT_RETRIES); + Result send_command(const CommandLong& command, unsigned retries = DEFAULT_RETRIES); - void queue_command_async(const CommandInt& command, const CommandResultCallback& callback); - void queue_command_async(const CommandLong& command, const CommandResultCallback& callback); + void queue_command_async( + const CommandInt& command, + const CommandResultCallback& callback, + unsigned retries = DEFAULT_RETRIES); + void queue_command_async( + const CommandLong& command, + const CommandResultCallback& callback, + unsigned retries = DEFAULT_RETRIES); void do_work(); @@ -116,7 +124,7 @@ class MavlinkCommandSender { SteadyTimePoint time_started{}; TimeoutHandler::Cookie timeout_cookie{}; double timeout_s{0.5}; - int retries_to_do{3}; + int retries_to_do; bool already_sent{false}; }; diff --git a/src/mavsdk/core/mavlink_ftp_client.cpp b/src/mavsdk/core/mavlink_ftp_client.cpp index c2cdce698a..4cea3f91f4 100644 --- a/src/mavsdk/core/mavlink_ftp_client.cpp +++ b/src/mavsdk/core/mavlink_ftp_client.cpp @@ -381,6 +381,7 @@ void MavlinkFtpClient::process_mavlink_ftp_message(const mavlink_message_t& msg) bool MavlinkFtpClient::download_start(Work& work, DownloadItem& item) { fs::path local_path = fs::path(item.local_folder) / fs::path(item.remote_path).filename(); + fs::create_directories(fs::path(item.local_folder)); if (_debugging) { LogDebug() << "Trying to open write to local path: " << local_path.string(); diff --git a/src/mavsdk/core/mavlink_ftp_server.cpp b/src/mavsdk/core/mavlink_ftp_server.cpp index c48d00bb2b..c53bad424f 100644 --- a/src/mavsdk/core/mavlink_ftp_server.cpp +++ b/src/mavsdk/core/mavlink_ftp_server.cpp @@ -256,6 +256,7 @@ MavlinkFtpServer::_path_from_string(const std::string& payload_path) // No permission whatsoever if the root dir is not set. if (_root_dir.empty()) { + LogWarn() << "Root dir not set!"; return ServerResult::ERR_FAIL; } @@ -285,10 +286,13 @@ void MavlinkFtpServer::set_root_directory(const std::string& root_dir) { std::lock_guard lock(_mutex); - std::error_code ignored; - _root_dir = fs::canonical(fs::path(root_dir), ignored).string(); + std::error_code ec; + _root_dir = fs::canonical(fs::path(root_dir), ec).string(); + if (ec) { + LogWarn() << "Root dir could not be made absolute: " << ec.message(); + } if (_debugging) { - LogDebug() << "Set root dir to: " << _root_dir; + LogDebug() << "Set root dir to: " << _root_dir << " from: " << root_dir; } } diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index dc51ca181e..19d78dad04 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -681,7 +681,10 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag // out of scope. auto work_queue_guard = std::make_unique::Guard>(_work_queue); const auto work = work_queue_guard->get_front(); + if (!work) { + // update existing param + find_and_call_subscriptions_value_changed(safe_param_id, received_value); return; } @@ -845,8 +848,6 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag } }}, work->work_item_variant); - - // find_and_call_subscriptions_value_changed(safe_param_id, received_value); } void MavlinkParameterClient::process_param_ext_value(const mavlink_message_t& message) @@ -871,9 +872,13 @@ void MavlinkParameterClient::process_param_ext_value(const mavlink_message_t& me // See comments on process_param_value for use of unique_ptr auto work_queue_guard = std::make_unique::Guard>(_work_queue); auto work = work_queue_guard->get_front(); + if (!work) { + // update existing param + find_and_call_subscriptions_value_changed(safe_param_id, received_value); return; } + if (!work->already_requested) { return; } @@ -952,9 +957,6 @@ void MavlinkParameterClient::process_param_ext_value(const mavlink_message_t& me }, }, work->work_item_variant); - - // TODO I think we need to consider more edge cases here - // find_and_call_subscriptions_value_changed(safe_param_id, received_value); } void MavlinkParameterClient::process_param_ext_ack(const mavlink_message_t& message) diff --git a/src/mavsdk/core/mavlink_parameter_server.cpp b/src/mavsdk/core/mavlink_parameter_server.cpp index 2b5dd44b5e..c649eb6c39 100644 --- a/src/mavsdk/core/mavlink_parameter_server.cpp +++ b/src/mavsdk/core/mavlink_parameter_server.cpp @@ -3,6 +3,7 @@ #include "mavlink_parameter_helper.h" #include "plugin_base.h" #include +#include namespace mavsdk { @@ -25,7 +26,7 @@ MavlinkParameterServer::MavlinkParameterServer( const auto& param_values = optional_param_values.value(); for (const auto& [key, value] : param_values) { const auto result = provide_server_param(key, value); - if (result != Result::Success) { + if (result != Result::Ok) { LogDebug() << "Cannot add parameter:" << key << ":" << value << " " << result; } } @@ -85,30 +86,39 @@ MavlinkParameterServer::provide_server_param(const std::string& name, const Para // first we try to add it as a new parameter switch (_param_cache.add_new_param(name, param_value)) { case MavlinkParameterCache::AddNewParamResult::Ok: - return Result::Success; + return Result::Ok; case MavlinkParameterCache::AddNewParamResult::AlreadyExists: - return Result::ParamExistsAlready; + // then, to not change the public api behaviour, try updating its value. + switch (_param_cache.update_existing_param(name, param_value)) { + case MavlinkParameterCache::UpdateExistingParamResult::Ok: + find_and_call_subscriptions_value_changed(name, param_value); + { + auto new_work = std::make_shared( + name, + param_value, + WorkItemValue{ + std::numeric_limits::max(), + std::numeric_limits::max(), + _last_extended}); + _work_queue.push_back(new_work); + } + return Result::OkExistsAlready; + case MavlinkParameterCache::UpdateExistingParamResult::MissingParam: + return Result::ParamNotFound; + case MavlinkParameterCache::UpdateExistingParamResult::WrongType: + return Result::WrongType; + default: + LogErr() << "Unknown update_existing_param result"; + assert(false); + return Result::Unknown; + } case MavlinkParameterCache::AddNewParamResult::TooManyParams: return Result::TooManyParams; default: LogErr() << "Unknown add_new_param result"; assert(false); + return Result::Unknown; } - - // then, to not change the public api behaviour, try updating its value. - switch (_param_cache.update_existing_param(name, param_value)) { - case MavlinkParameterCache::UpdateExistingParamResult::Ok: - return Result::Success; - case MavlinkParameterCache::UpdateExistingParamResult::MissingParam: - return Result::ParamNotFound; - case MavlinkParameterCache::UpdateExistingParamResult::WrongType: - return Result::WrongType; - default: - LogErr() << "Unknown update_existing_param result"; - assert(false); - } - - return Result::Unknown; } MavlinkParameterServer::Result @@ -157,7 +167,7 @@ MavlinkParameterServer::retrieve_server_param(const std::string& name) // This parameter exists, check its type const auto& param = param_opt.value(); if (param.value.is()) { - return {Result::Success, param.value.get()}; + return {Result::Ok, param.value.get()}; } return {Result::WrongType, {}}; } @@ -183,9 +193,10 @@ MavlinkParameterServer::retrieve_server_param_int(const std::string& name) void MavlinkParameterServer::process_param_set_internally( const std::string& param_id, const ParamValue& value_to_set, bool extended) { - // TODO: add debugging env - LogDebug() << "Param set request" << (extended ? " extended" : "") << ": " << param_id - << " with " << value_to_set; + if (_parameter_debugging) { + LogDebug() << "Param set request" << (extended ? " extended" : "") << ": " << param_id + << " with " << value_to_set; + } std::lock_guard lock(_all_params_mutex); // for checking if the update actually changed the value const auto opt_before_update = _param_cache.param_by_id(param_id, extended); @@ -223,6 +234,7 @@ void MavlinkParameterServer::process_param_set_internally( return; } case MavlinkParameterCache::UpdateExistingParamResult::Ok: { + LogWarn() << "Update existing params!"; const auto updated_parameter = _param_cache.param_by_id(param_id, extended).value(); // The param set doesn't differentiate between an update that actually changed the value // e.g. 0 to 1 and an update that had no effect e.g. 0 to 0. @@ -294,6 +306,7 @@ void MavlinkParameterServer::process_param_ext_set(const mavlink_message_t& mess LogWarn() << "Invalid Param Set ext Request: " << safe_param_id; return; } + process_param_set_internally(safe_param_id, value_to_set, true); } @@ -332,7 +345,6 @@ void MavlinkParameterServer::process_param_request_read(const mavlink_message_t& void MavlinkParameterServer::process_param_ext_request_read(const mavlink_message_t& message) { - LogDebug() << "process param_ext_request_read"; mavlink_param_ext_request_read_t read_request{}; mavlink_msg_param_ext_request_read_decode(&message, &read_request); if (!target_matches(read_request.target_system, read_request.target_component, true)) { @@ -377,6 +389,8 @@ void MavlinkParameterServer::internal_process_param_request_read_by_id( auto new_work = std::make_shared( param.id, param.value, WorkItemValue{param.index, param_count, extended}); _work_queue.push_back(new_work); + + _last_extended = extended; } void MavlinkParameterServer::internal_process_param_request_read_by_index( @@ -479,6 +493,7 @@ void MavlinkParameterServer::do_work() return; } } else { + LogWarn() << "sending not extended message"; float param_value; if (_sender.autopilot() == Autopilot::ArduPilot) { param_value = work->param_value.get_4_float_bytes_cast(); @@ -534,8 +549,10 @@ void MavlinkParameterServer::do_work() std::ostream& operator<<(std::ostream& str, const MavlinkParameterServer::Result& result) { switch (result) { - case MavlinkParameterServer::Result::Success: - return str << "Success"; + case MavlinkParameterServer::Result::Ok: + return str << "Ok"; + case MavlinkParameterServer::Result::OkExistsAlready: + return str << "OkExistsAlready"; case MavlinkParameterServer::Result::WrongType: return str << "WrongType"; case MavlinkParameterServer::Result::ParamNameTooLong: diff --git a/src/mavsdk/core/mavlink_parameter_server.h b/src/mavsdk/core/mavlink_parameter_server.h index b71194bd76..ea38ae4388 100644 --- a/src/mavsdk/core/mavlink_parameter_server.h +++ b/src/mavsdk/core/mavlink_parameter_server.h @@ -33,12 +33,12 @@ class MavlinkParameterServer : public MavlinkParameterSubscription { ~MavlinkParameterServer(); enum class Result { - Success, + Ok, + OkExistsAlready, WrongType, ParamNameTooLong, NotFound, ParamValueTooLong, - ParamExistsAlready, TooManyParams, ParamNotFound, Unknown, @@ -55,6 +55,7 @@ class MavlinkParameterServer : public MavlinkParameterSubscription { std::pair retrieve_server_param_int(const std::string& name); std::pair retrieve_server_param_custom(const std::string& name); + void set_extended_protocol(bool extended_protocol) { _extended_protocol = extended_protocol; }; void do_work(); friend std::ostream& operator<<(std::ostream&, const Result&); @@ -117,7 +118,9 @@ class MavlinkParameterServer : public MavlinkParameterSubscription { LockedQueue _work_queue{}; + bool _extended_protocol = true; bool _parameter_debugging = false; + bool _last_extended = true; }; } // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_parameter_subscription.cpp b/src/mavsdk/core/mavlink_parameter_subscription.cpp index d59a9039d7..4fb8452e0b 100644 --- a/src/mavsdk/core/mavlink_parameter_subscription.cpp +++ b/src/mavsdk/core/mavlink_parameter_subscription.cpp @@ -1,40 +1,8 @@ #include "mavlink_parameter_subscription.h" -namespace mavsdk { +#include -template -void MavlinkParameterSubscription::subscribe_param_changed( - const std::string& name, const ParamChangedCallback& callback, const void* cookie) -{ - std::lock_guard lock(_param_changed_subscriptions_mutex); - if (callback != nullptr) { - ParamChangedSubscription subscription{name, callback, cookie}; - // This is just to let the upper level know of what probably is a bug, but we check again - // when actually calling the callback We also cannot assume here that the user called - // provide_param before subscribe_param_changed, so the only thing that makes sense is to - // log a warning, but then continue anyways. - /*std::lock_guard lock2(_all_params_mutex); - if (_all_params.find(name) != _all_params.end()) { - const auto curr_value = _all_params.at(name); - if (!curr_value.template is_same_type_templated()) { - LogDebug() - << "You just registered a param changed callback where the type does not match - the type already stored"; - } - }*/ - _param_changed_subscriptions.push_back(subscription); - } else { - for (auto it = _param_changed_subscriptions.begin(); - it != _param_changed_subscriptions.end(); - /* ++it */) { - if (it->param_name == name && it->cookie == cookie) { - it = _param_changed_subscriptions.erase(it); - } else { - ++it; - } - } - } -} +namespace mavsdk { void MavlinkParameterSubscription::subscribe_param_float_changed( const std::string& name, const ParamFloatChangedCallback& callback, const void* cookie) @@ -62,6 +30,7 @@ void MavlinkParameterSubscription::find_and_call_subscriptions_value_changed( if (subscription.param_name != param_name) { continue; } + LogDebug() << "Param " << param_name << " changed to " << value; // We have a subscription on this param name, now check if the subscription is for the right // type and call the callback when matching if (std::get_if(&subscription.callback) && value.get_float()) { @@ -83,14 +52,12 @@ void MavlinkParameterSubscription::unsubscribe_all_params_changed(const void* co { std::lock_guard lock(_param_changed_subscriptions_mutex); - for (auto it = _param_changed_subscriptions.begin(); it != _param_changed_subscriptions.end(); - /* it++ */) { - if (it->cookie == cookie) { - it = _param_changed_subscriptions.erase(it); - } else { - it++; - } - } + _param_changed_subscriptions.erase( + std::remove_if( + _param_changed_subscriptions.begin(), + _param_changed_subscriptions.end(), + [&](const auto& subscription) { return subscription.cookie == cookie; }), + _param_changed_subscriptions.end()); } } // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_parameter_subscription.h b/src/mavsdk/core/mavlink_parameter_subscription.h index ddd7ce4605..d064fdbd3d 100644 --- a/src/mavsdk/core/mavlink_parameter_subscription.h +++ b/src/mavsdk/core/mavlink_parameter_subscription.h @@ -2,7 +2,8 @@ #include "param_value.h" #include -#include +#include +#include namespace mavsdk { @@ -16,6 +17,30 @@ class MavlinkParameterSubscription { public: template using ParamChangedCallback = std::function; + using ParamChangedCallbacks = std::variant< + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback, + ParamChangedCallback>; + + struct ParamChangedSubscription { + std::string param_name; + ParamChangedCallbacks callback; + const void* cookie; + ParamChangedSubscription( + std::string param_name1, ParamChangedCallbacks callback1, const void* cookie1) : + param_name(std::move(param_name1)), + callback(std::move(callback1)), + cookie(cookie1){}; + }; + /** * Subscribe to changes on the parameter referenced by @param name. * If the value for this parameter changes, the given callback is called provided that the @@ -23,15 +48,46 @@ class MavlinkParameterSubscription { */ template void subscribe_param_changed( - const std::string& name, const ParamChangedCallback& callback, const void* cookie); + const std::string& name, const ParamChangedCallback& callback, const void* cookie) + { + std::lock_guard lock(_param_changed_subscriptions_mutex); + if (callback != nullptr) { + ParamChangedSubscription subscription{name, callback, cookie}; + // This is just to let the upper level know of what probably is a bug, but we check + // again when actually calling the callback We also cannot assume here that the user + // called provide_param before subscribe_param_changed, so the only thing that makes + // sense is to log a warning, but then continue anyways. + /*std::lock_guard lock2(_all_params_mutex); + if (_all_params.find(name) != _all_params.end()) { + const auto curr_value = _all_params.at(name); + if (!curr_value.template is_same_type_templated()) { + LogDebug() + << "You just registered a param changed callback where the type does not + match the type already stored"; + } + }*/ + _param_changed_subscriptions.push_back(subscription); + } else { + for (auto it = _param_changed_subscriptions.begin(); + it != _param_changed_subscriptions.end(); + /* ++it */) { + if (it->param_name == name && it->cookie == cookie) { + it = _param_changed_subscriptions.erase(it); + } else { + ++it; + } + } + } + } using ParamFloatChangedCallback = ParamChangedCallback; + using ParamIntChangedCallback = ParamChangedCallback; + using ParamCustomChangedCallback = ParamChangedCallback; + void subscribe_param_float_changed( const std::string& name, const ParamFloatChangedCallback& callback, const void* cookie); - using ParamIntChangedCallback = ParamChangedCallback; void subscribe_param_int_changed( const std::string& name, const ParamIntChangedCallback& callback, const void* cookie); - using ParamCustomChangedCallback = ParamChangedCallback; void subscribe_param_custom_changed( const std::string& name, const ParamCustomChangedCallback& callback, const void* cookie); @@ -47,20 +103,8 @@ class MavlinkParameterSubscription { const std::string& param_name, const ParamValue& new_param_value); private: - using ParamChangedCallbacks = std:: - variant; - struct ParamChangedSubscription { - const std::string param_name; - const ParamChangedCallbacks callback; - const void* const cookie; - explicit ParamChangedSubscription( - std::string param_name1, ParamChangedCallbacks callback1, const void* cookie1) : - param_name(std::move(param_name1)), - callback(std::move(callback1)), - cookie(cookie1){}; - }; std::mutex _param_changed_subscriptions_mutex{}; - std::list _param_changed_subscriptions{}; + std::vector _param_changed_subscriptions{}; }; } // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_request_message.cpp b/src/mavsdk/core/mavlink_request_message.cpp index cf11280121..a7b6509003 100644 --- a/src/mavsdk/core/mavlink_request_message.cpp +++ b/src/mavsdk/core/mavlink_request_message.cpp @@ -89,11 +89,13 @@ void MavlinkRequestMessage::send_request_using_new_command(WorkItem& item) if (result != MavlinkCommandSender::Result::InProgress) { handle_command_result(message_id, result); } - }); + }, + 1); } bool MavlinkRequestMessage::try_sending_request_using_old_command(WorkItem& item) { + LogInfo() << "Try old command"; MavlinkCommandSender::CommandLong command_request_message{}; command_request_message.target_system_id = _system_impl.get_system_id(); command_request_message.target_component_id = item.target_component; @@ -175,7 +177,8 @@ bool MavlinkRequestMessage::try_sending_request_using_old_command(WorkItem& item if (result != MavlinkCommandSender::Result::InProgress) { handle_command_result(message_id, result); } - }); + }, + 1); return true; } @@ -232,10 +235,6 @@ void MavlinkRequestMessage::handle_command_result( 1.0); return; - case MavlinkCommandSender::Result::NoSystem: - // FALLTHROUGH - case MavlinkCommandSender::Result::ConnectionError: - // FALLTHROUGH case MavlinkCommandSender::Result::Busy: // FALLTHROUGH case MavlinkCommandSender::Result::Denied: @@ -245,6 +244,24 @@ void MavlinkRequestMessage::handle_command_result( case MavlinkCommandSender::Result::Timeout: // FALLTHROUGH case MavlinkCommandSender::Result::TemporarilyRejected: + // It looks like we should try again + if (++it->retries > RETRIES) { + // We have already retried, let's give up. + auto temp_callback = it->callback; + _message_handler.unregister_one(it->message_id, this); + _work_items.erase(it); + lock.unlock(); + if (temp_callback) { + temp_callback(MavlinkCommandSender::Result::Timeout, {}); + } + } else { + send_request(*it); + } + return; + + case MavlinkCommandSender::Result::NoSystem: + // FALLTHROUGH + case MavlinkCommandSender::Result::ConnectionError: // FALLTHROUGH case MavlinkCommandSender::Result::Failed: // FALLTHROUGH @@ -280,7 +297,7 @@ void MavlinkRequestMessage::handle_timeout(uint32_t message_id, uint8_t target_c continue; } - if (++it->retries > 3) { + if (++it->retries > RETRIES) { // We have already retried, let's give up. auto temp_callback = it->callback; _message_handler.unregister_one(it->message_id, this); diff --git a/src/mavsdk/core/mavlink_request_message.h b/src/mavsdk/core/mavlink_request_message.h index bf5dd48fb0..9397684785 100644 --- a/src/mavsdk/core/mavlink_request_message.h +++ b/src/mavsdk/core/mavlink_request_message.h @@ -32,6 +32,8 @@ class MavlinkRequestMessage { uint32_t param2 = 0); private: + static constexpr unsigned RETRIES = 4; + struct WorkItem { uint32_t message_id{0}; uint8_t target_component{0}; diff --git a/src/mavsdk/core/mavsdk_math_test.cpp b/src/mavsdk/core/mavsdk_math_test.cpp deleted file mode 100644 index f9076f6db5..0000000000 --- a/src/mavsdk/core/mavsdk_math_test.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "mavsdk_math.h" -#include -#include - -using namespace mavsdk; - -#ifndef M_PI_F -#define M_PI_F static_cast(M_PI) -#endif - -TEST(MATH, OurPi) -{ - ASSERT_DOUBLE_EQ(PI, M_PI); -} - -TEST(Math, RadDegDouble) -{ - ASSERT_DOUBLE_EQ(0.0, to_rad_from_deg(0.0)); - ASSERT_DOUBLE_EQ(M_PI / 2.0, to_rad_from_deg(90.0)); - ASSERT_DOUBLE_EQ(M_PI, to_rad_from_deg(180.0)); - ASSERT_DOUBLE_EQ(-M_PI, to_rad_from_deg(-180.0)); - ASSERT_DOUBLE_EQ(2.0 * M_PI, to_rad_from_deg(360.0)); - - ASSERT_DOUBLE_EQ(0.0, to_deg_from_rad(0.0)); - ASSERT_DOUBLE_EQ(90.0, to_deg_from_rad(M_PI / 2.0)); - ASSERT_DOUBLE_EQ(180, to_deg_from_rad(M_PI)); - ASSERT_DOUBLE_EQ(-180, to_deg_from_rad(-M_PI)); - ASSERT_DOUBLE_EQ(360.0, to_deg_from_rad(2.0 * M_PI)); -} - -TEST(Math, RadDegFloat) -{ - ASSERT_FLOAT_EQ(0.0f, to_rad_from_deg(0.0f)); - ASSERT_FLOAT_EQ(M_PI_F / 2.0f, to_rad_from_deg(90.0f)); - ASSERT_FLOAT_EQ(M_PI_F, to_rad_from_deg(180.0f)); - ASSERT_FLOAT_EQ(-M_PI_F, to_rad_from_deg(-180.0f)); - ASSERT_FLOAT_EQ(2.0f * M_PI_F, to_rad_from_deg(360.0f)); - - ASSERT_FLOAT_EQ(0.0f, to_deg_from_rad(0.0f)); - ASSERT_FLOAT_EQ(90.0f, to_deg_from_rad(M_PI_F / 2.0f)); - ASSERT_FLOAT_EQ(180.0f, to_deg_from_rad(M_PI_F)); - ASSERT_FLOAT_EQ(-180.0f, to_deg_from_rad(-M_PI_F)); - ASSERT_FLOAT_EQ(360.0f, to_deg_from_rad(2.0f * M_PI_F)); -} \ No newline at end of file diff --git a/src/mavsdk/core/param_value.cpp b/src/mavsdk/core/param_value.cpp index 136fdfdbac..dedd4ca01e 100644 --- a/src/mavsdk/core/param_value.cpp +++ b/src/mavsdk/core/param_value.cpp @@ -488,7 +488,7 @@ bool ParamValue::set_int(int new_value) _value = static_cast(new_value); return true; } else if (std::get_if(&_value)) { - _value = static_cast(new_value); + _value = new_value; return true; } else { return false; @@ -580,15 +580,33 @@ std::array ParamValue::get_128_bytes() const (std::get_if(&_value) && std::get_if(&rhs._value)) || (std::get_if(&_value) && std::get_if(&rhs._value))) { return true; - } else { - LogWarn() << "Comparison type mismatch between " << typestr() << " and " << rhs.typestr(); - return false; } + + if ((std::get_if(&_value) || std::get_if(&_value) || + std::get_if(&_value) || std::get_if(&_value) || + std::get_if(&_value) || std::get_if(&_value) || + std::get_if(&_value) || std::get_if(&_value)) && + (std::get_if(&rhs._value) || std::get_if(&rhs._value) || + std::get_if(&rhs._value) || std::get_if(&rhs._value) || + std::get_if(&rhs._value) || std::get_if(&rhs._value) || + std::get_if(&rhs._value) || std::get_if(&rhs._value))) { + LogDebug() << "Ignoring int mismatch between " << typestr() << " and " << rhs.typestr(); + return true; + } + + if ((std::get_if(&_value) || std::get_if(&_value)) && + (std::get_if(&rhs._value) || std::get_if(&rhs._value))) { + LogDebug() << "Ignoring float/double mismatch between " << typestr() << " and " + << rhs.typestr(); + return true; + } + + LogWarn() << "Comparison type mismatch between " << typestr() << " and " << rhs.typestr(); + return false; } bool ParamValue::operator==(const std::string& value_str) const { - // LogDebug() << "Compare " << value_str() << " and " << rhs.value_str(); if (std::get_if(&_value)) { return std::get(_value) == std::stoi(value_str); } else if (std::get_if(&_value)) { diff --git a/src/mavsdk/core/string_utils.cpp b/src/mavsdk/core/string_utils.cpp new file mode 100644 index 0000000000..bb60de14fb --- /dev/null +++ b/src/mavsdk/core/string_utils.cpp @@ -0,0 +1,18 @@ +#include "string_utils.h" + +namespace mavsdk { + +bool starts_with(const std::string& str, const std::string& prefix) +{ + return str.compare(0, prefix.size(), prefix) == 0; +} + +std::string strip_prefix(const std::string& str, const std::string& prefix) +{ + if (starts_with(str, prefix)) { + return str.substr(prefix.size()); + } + return str; // If no known prefix is found, return the original string +} + +} // namespace mavsdk diff --git a/src/mavsdk/core/string_utils.h b/src/mavsdk/core/string_utils.h new file mode 100644 index 0000000000..8e1fcdd255 --- /dev/null +++ b/src/mavsdk/core/string_utils.h @@ -0,0 +1,11 @@ +#pragma once + +#include + +namespace mavsdk { + +bool starts_with(const std::string& str, const std::string& prefix); + +std::string strip_prefix(const std::string& str, const std::string& prefix); + +} // namespace mavsdk diff --git a/src/mavsdk/core/string_utils_test.cpp b/src/mavsdk/core/string_utils_test.cpp new file mode 100644 index 0000000000..02373d2b37 --- /dev/null +++ b/src/mavsdk/core/string_utils_test.cpp @@ -0,0 +1,24 @@ +#include "string_utils.h" +#include + +using namespace mavsdk; + +TEST(StringUtils, StartsWith) +{ + EXPECT_TRUE(starts_with("blafoo", "bla")); + EXPECT_FALSE(starts_with("blafoo", "1234")); + EXPECT_FALSE(starts_with("blafoo", "foo")); + EXPECT_TRUE(starts_with("", "")); + EXPECT_FALSE(starts_with("", "b")); + EXPECT_TRUE(starts_with("b", "")); +} + +TEST(StringUtils, StripPrefix) +{ + EXPECT_EQ(strip_prefix("blafoo", "bla"), "foo"); + EXPECT_EQ(strip_prefix("blafoo", "1234"), "blafoo"); + EXPECT_EQ(strip_prefix("blafoo", "foo"), "blafoo"); + EXPECT_EQ(strip_prefix("", ""), ""); + EXPECT_EQ(strip_prefix("", "b"), ""); + EXPECT_EQ(strip_prefix("f", ""), "f"); +} diff --git a/src/mavsdk/core/system_impl.h b/src/mavsdk/core/system_impl.h index 1286123a23..047c6e5b12 100644 --- a/src/mavsdk/core/system_impl.h +++ b/src/mavsdk/core/system_impl.h @@ -136,6 +136,8 @@ class SystemImpl { bool is_armed() const { return _armed; } + MavlinkParameterClient* param_sender(uint8_t component_id, bool extended); + MavlinkParameterClient::Result set_param( const std::string& name, ParamValue value, @@ -349,8 +351,6 @@ class SystemImpl { MavlinkStatustextHandler _statustext_handler{}; - MavlinkParameterClient* param_sender(uint8_t component_id, bool extended); - struct StatustextCallback { std::function callback; void* cookie; diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index 4ec7d31740..52ec10b459 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -1,6 +1,6 @@ #include "action_impl.h" #include "mavsdk_impl.h" -#include "mavsdk_math.h" +#include "math_utils.h" #include "flight_mode.h" #include "px4_custom_mode.h" #include diff --git a/src/mavsdk/plugins/camera/camera.cpp b/src/mavsdk/plugins/camera/camera.cpp index 89555b6117..388df9e2ee 100644 --- a/src/mavsdk/plugins/camera/camera.cpp +++ b/src/mavsdk/plugins/camera/camera.cpp @@ -9,17 +9,24 @@ namespace mavsdk { +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 Storage = Camera::Storage; +using StorageUpdate = Camera::StorageUpdate; +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; Camera::Camera(System& system) : PluginBase(), _impl{std::make_unique(system)} {} @@ -30,125 +37,117 @@ Camera::Camera(std::shared_ptr system) : Camera::~Camera() {} -void Camera::prepare_async(const ResultCallback callback) +void Camera::take_photo_async(int32_t component_id, const ResultCallback callback) { - _impl->prepare_async(callback); + _impl->take_photo_async(component_id, callback); } -Camera::Result Camera::prepare() const +Camera::Result Camera::take_photo(int32_t component_id) const { - return _impl->prepare(); + return _impl->take_photo(component_id); } -void Camera::take_photo_async(const ResultCallback callback) +void Camera::start_photo_interval_async( + int32_t component_id, float interval_s, const ResultCallback callback) { - _impl->take_photo_async(callback); + _impl->start_photo_interval_async(component_id, interval_s, callback); } -Camera::Result Camera::take_photo() const +Camera::Result Camera::start_photo_interval(int32_t component_id, float interval_s) const { - return _impl->take_photo(); + return _impl->start_photo_interval(component_id, interval_s); } -void Camera::start_photo_interval_async(float interval_s, const ResultCallback callback) +void Camera::stop_photo_interval_async(int32_t component_id, const ResultCallback callback) { - _impl->start_photo_interval_async(interval_s, callback); + _impl->stop_photo_interval_async(component_id, callback); } -Camera::Result Camera::start_photo_interval(float interval_s) const +Camera::Result Camera::stop_photo_interval(int32_t component_id) const { - return _impl->start_photo_interval(interval_s); + return _impl->stop_photo_interval(component_id); } -void Camera::stop_photo_interval_async(const ResultCallback callback) +void Camera::start_video_async(int32_t component_id, const ResultCallback callback) { - _impl->stop_photo_interval_async(callback); + _impl->start_video_async(component_id, callback); } -Camera::Result Camera::stop_photo_interval() const +Camera::Result Camera::start_video(int32_t component_id) const { - return _impl->stop_photo_interval(); + return _impl->start_video(component_id); } -void Camera::start_video_async(const ResultCallback callback) +void Camera::stop_video_async(int32_t component_id, const ResultCallback callback) { - _impl->start_video_async(callback); + _impl->stop_video_async(component_id, callback); } -Camera::Result Camera::start_video() const +Camera::Result Camera::stop_video(int32_t component_id) const { - return _impl->start_video(); + return _impl->stop_video(component_id); } -void Camera::stop_video_async(const ResultCallback callback) +Camera::Result Camera::start_video_streaming(int32_t component_id, int32_t stream_id) const { - _impl->stop_video_async(callback); + return _impl->start_video_streaming(component_id, stream_id); } -Camera::Result Camera::stop_video() const +Camera::Result Camera::stop_video_streaming(int32_t component_id, int32_t stream_id) const { - return _impl->stop_video(); + return _impl->stop_video_streaming(component_id, stream_id); } -Camera::Result Camera::start_video_streaming(int32_t stream_id) const +void Camera::set_mode_async(int32_t component_id, Mode mode, const ResultCallback callback) { - return _impl->start_video_streaming(stream_id); + _impl->set_mode_async(component_id, mode, callback); } -Camera::Result Camera::stop_video_streaming(int32_t stream_id) const +Camera::Result Camera::set_mode(int32_t component_id, Mode mode) const { - return _impl->stop_video_streaming(stream_id); + return _impl->set_mode(component_id, mode); } -void Camera::set_mode_async(Mode mode, const ResultCallback callback) +void Camera::list_photos_async( + int32_t component_id, PhotosRange photos_range, const ListPhotosCallback callback) { - _impl->set_mode_async(mode, callback); -} - -Camera::Result Camera::set_mode(Mode mode) const -{ - return _impl->set_mode(mode); -} - -void Camera::list_photos_async(PhotosRange photos_range, const ListPhotosCallback callback) -{ - _impl->list_photos_async(photos_range, callback); + _impl->list_photos_async(component_id, photos_range, callback); } std::pair> -Camera::list_photos(PhotosRange photos_range) const +Camera::list_photos(int32_t component_id, PhotosRange photos_range) const { - return _impl->list_photos(photos_range); + return _impl->list_photos(component_id, photos_range); } -Camera::ModeHandle Camera::subscribe_mode(const ModeCallback& callback) +Camera::CameraListHandle Camera::subscribe_camera_list(const CameraListCallback& callback) { - return _impl->subscribe_mode(callback); + return _impl->subscribe_camera_list(callback); } -void Camera::unsubscribe_mode(ModeHandle handle) +void Camera::unsubscribe_camera_list(CameraListHandle handle) { - _impl->unsubscribe_mode(handle); + _impl->unsubscribe_camera_list(handle); } -Camera::Mode Camera::mode() const +Camera::CameraList Camera::camera_list() const { - return _impl->mode(); + return _impl->camera_list(); } -Camera::InformationHandle Camera::subscribe_information(const InformationCallback& callback) +Camera::ModeHandle Camera::subscribe_mode(const ModeCallback& callback) { - return _impl->subscribe_information(callback); + return _impl->subscribe_mode(callback); } -void Camera::unsubscribe_information(InformationHandle handle) +void Camera::unsubscribe_mode(ModeHandle handle) { - _impl->unsubscribe_information(handle); + _impl->unsubscribe_mode(handle); } -Camera::Information Camera::information() const +std::pair Camera::get_mode(int32_t component_id) const { - return _impl->information(); + return _impl->get_mode(component_id); } Camera::VideoStreamInfoHandle @@ -162,9 +161,10 @@ void Camera::unsubscribe_video_stream_info(VideoStreamInfoHandle handle) _impl->unsubscribe_video_stream_info(handle); } -Camera::VideoStreamInfo Camera::video_stream_info() const +std::pair +Camera::get_video_stream_info(int32_t component_id) const { - return _impl->video_stream_info(); + return _impl->get_video_stream_info(component_id); } Camera::CaptureInfoHandle Camera::subscribe_capture_info(const CaptureInfoCallback& callback) @@ -177,19 +177,19 @@ void Camera::unsubscribe_capture_info(CaptureInfoHandle handle) _impl->unsubscribe_capture_info(handle); } -Camera::StatusHandle Camera::subscribe_status(const StatusCallback& callback) +Camera::StorageHandle Camera::subscribe_storage(const StorageCallback& callback) { - return _impl->subscribe_status(callback); + return _impl->subscribe_storage(callback); } -void Camera::unsubscribe_status(StatusHandle handle) +void Camera::unsubscribe_storage(StorageHandle handle) { - _impl->unsubscribe_status(handle); + _impl->unsubscribe_storage(handle); } -Camera::Status Camera::status() const +std::pair Camera::get_storage(int32_t component_id) const { - return _impl->status(); + return _impl->get_storage(component_id); } Camera::CurrentSettingsHandle @@ -203,6 +203,12 @@ void Camera::unsubscribe_current_settings(CurrentSettingsHandle handle) _impl->unsubscribe_current_settings(handle); } +std::pair> +Camera::get_current_settings(int32_t component_id) const +{ + return _impl->get_current_settings(component_id); +} + Camera::PossibleSettingOptionsHandle Camera::subscribe_possible_setting_options(const PossibleSettingOptionsCallback& callback) { @@ -214,284 +220,234 @@ void Camera::unsubscribe_possible_setting_options(PossibleSettingOptionsHandle h _impl->unsubscribe_possible_setting_options(handle); } -std::vector Camera::possible_setting_options() const -{ - return _impl->possible_setting_options(); -} - -void Camera::set_setting_async(Setting setting, const ResultCallback callback) +std::pair> +Camera::get_possible_setting_options(int32_t component_id) const { - _impl->set_setting_async(setting, callback); + return _impl->get_possible_setting_options(component_id); } -Camera::Result Camera::set_setting(Setting setting) const +void Camera::set_setting_async(int32_t component_id, Setting setting, const ResultCallback callback) { - return _impl->set_setting(setting); + _impl->set_setting_async(component_id, setting, callback); } -void Camera::get_setting_async(Setting setting, const GetSettingCallback callback) +Camera::Result Camera::set_setting(int32_t component_id, Setting setting) const { - _impl->get_setting_async(setting, callback); + return _impl->set_setting(component_id, setting); } -std::pair Camera::get_setting(Setting setting) const +void Camera::get_setting_async( + int32_t component_id, Setting setting, const GetSettingCallback callback) { - return _impl->get_setting(setting); + _impl->get_setting_async(component_id, setting, callback); } -void Camera::format_storage_async(int32_t storage_id, const ResultCallback callback) +std::pair +Camera::get_setting(int32_t component_id, Setting setting) const { - _impl->format_storage_async(storage_id, callback); + return _impl->get_setting(component_id, setting); } -Camera::Result Camera::format_storage(int32_t storage_id) const +void Camera::format_storage_async( + int32_t component_id, int32_t storage_id, const ResultCallback callback) { - return _impl->format_storage(storage_id); + _impl->format_storage_async(component_id, storage_id, callback); } -Camera::Result Camera::select_camera(int32_t camera_id) const +Camera::Result Camera::format_storage(int32_t component_id, int32_t storage_id) const { - return _impl->select_camera(camera_id); + return _impl->format_storage(component_id, storage_id); } -void Camera::reset_settings_async(const ResultCallback callback) +void Camera::reset_settings_async(int32_t component_id, const ResultCallback callback) { - _impl->reset_settings_async(callback); + _impl->reset_settings_async(component_id, callback); } -Camera::Result Camera::reset_settings() const +Camera::Result Camera::reset_settings(int32_t component_id) const { - return _impl->reset_settings(); + return _impl->reset_settings(component_id); } -void Camera::zoom_in_start_async(const ResultCallback callback) +void Camera::zoom_in_start_async(int32_t component_id, const ResultCallback callback) { - _impl->zoom_in_start_async(callback); + _impl->zoom_in_start_async(component_id, callback); } -Camera::Result Camera::zoom_in_start() const +Camera::Result Camera::zoom_in_start(int32_t component_id) const { - return _impl->zoom_in_start(); + return _impl->zoom_in_start(component_id); } -void Camera::zoom_out_start_async(const ResultCallback callback) +void Camera::zoom_out_start_async(int32_t component_id, const ResultCallback callback) { - _impl->zoom_out_start_async(callback); + _impl->zoom_out_start_async(component_id, callback); } -Camera::Result Camera::zoom_out_start() const +Camera::Result Camera::zoom_out_start(int32_t component_id) const { - return _impl->zoom_out_start(); + return _impl->zoom_out_start(component_id); } -void Camera::zoom_stop_async(const ResultCallback callback) +void Camera::zoom_stop_async(int32_t component_id, const ResultCallback callback) { - _impl->zoom_stop_async(callback); + _impl->zoom_stop_async(component_id, callback); } -Camera::Result Camera::zoom_stop() const +Camera::Result Camera::zoom_stop(int32_t component_id) const { - return _impl->zoom_stop(); + return _impl->zoom_stop(component_id); } -void Camera::zoom_range_async(float range, const ResultCallback callback) +void Camera::zoom_range_async(int32_t component_id, float range, const ResultCallback callback) { - _impl->zoom_range_async(range, callback); + _impl->zoom_range_async(component_id, range, callback); } -Camera::Result Camera::zoom_range(float range) const +Camera::Result Camera::zoom_range(int32_t component_id, float range) const { - return _impl->zoom_range(range); + return _impl->zoom_range(component_id, range); } void Camera::track_point_async( - float point_x, float point_y, float radius, const ResultCallback callback) + int32_t component_id, float point_x, float point_y, float radius, const ResultCallback callback) { - _impl->track_point_async(point_x, point_y, radius, callback); + _impl->track_point_async(component_id, point_x, point_y, radius, callback); } -Camera::Result Camera::track_point(float point_x, float point_y, float radius) const +Camera::Result +Camera::track_point(int32_t component_id, float point_x, float point_y, float radius) const { - return _impl->track_point(point_x, point_y, radius); + return _impl->track_point(component_id, point_x, point_y, radius); } void Camera::track_rectangle_async( + int32_t component_id, float top_left_x, float top_left_y, float bottom_right_x, float bottom_right_y, const ResultCallback callback) { - _impl->track_rectangle_async(top_left_x, top_left_y, bottom_right_x, bottom_right_y, callback); + _impl->track_rectangle_async( + component_id, top_left_x, top_left_y, bottom_right_x, bottom_right_y, callback); } Camera::Result Camera::track_rectangle( - float top_left_x, float top_left_y, float bottom_right_x, float bottom_right_y) const -{ - return _impl->track_rectangle(top_left_x, top_left_y, bottom_right_x, bottom_right_y); -} - -void Camera::track_stop_async(const ResultCallback callback) -{ - _impl->track_stop_async(callback); -} - -Camera::Result Camera::track_stop() const + int32_t component_id, + float top_left_x, + float top_left_y, + float bottom_right_x, + float bottom_right_y) const { - return _impl->track_stop(); + return _impl->track_rectangle( + component_id, top_left_x, top_left_y, bottom_right_x, bottom_right_y); } -void Camera::focus_in_start_async(const ResultCallback callback) +void Camera::track_stop_async(int32_t component_id, const ResultCallback callback) { - _impl->focus_in_start_async(callback); + _impl->track_stop_async(component_id, callback); } -Camera::Result Camera::focus_in_start() const +Camera::Result Camera::track_stop(int32_t component_id) const { - return _impl->focus_in_start(); + return _impl->track_stop(component_id); } -void Camera::focus_out_start_async(const ResultCallback callback) +void Camera::focus_in_start_async(int32_t component_id, const ResultCallback callback) { - _impl->focus_out_start_async(callback); + _impl->focus_in_start_async(component_id, callback); } -Camera::Result Camera::focus_out_start() const +Camera::Result Camera::focus_in_start(int32_t component_id) const { - return _impl->focus_out_start(); + return _impl->focus_in_start(component_id); } -void Camera::focus_stop_async(const ResultCallback callback) +void Camera::focus_out_start_async(int32_t component_id, const ResultCallback callback) { - _impl->focus_stop_async(callback); + _impl->focus_out_start_async(component_id, callback); } -Camera::Result Camera::focus_stop() const +Camera::Result Camera::focus_out_start(int32_t component_id) const { - return _impl->focus_stop(); + return _impl->focus_out_start(component_id); } -void Camera::focus_range_async(float range, const ResultCallback callback) +void Camera::focus_stop_async(int32_t component_id, const ResultCallback callback) { - _impl->focus_range_async(range, callback); + _impl->focus_stop_async(component_id, callback); } -Camera::Result Camera::focus_range(float range) const +Camera::Result Camera::focus_stop(int32_t component_id) const { - return _impl->focus_range(range); + return _impl->focus_stop(component_id); } -std::ostream& operator<<(std::ostream& str, Camera::Result const& result) +void Camera::focus_range_async(int32_t component_id, float range, const ResultCallback callback) { - switch (result) { - case Camera::Result::Unknown: - return str << "Unknown"; - case Camera::Result::Success: - return str << "Success"; - case Camera::Result::InProgress: - return str << "In Progress"; - case Camera::Result::Busy: - return str << "Busy"; - case Camera::Result::Denied: - return str << "Denied"; - case Camera::Result::Error: - return str << "Error"; - case Camera::Result::Timeout: - return str << "Timeout"; - case Camera::Result::WrongArgument: - return str << "Wrong Argument"; - case Camera::Result::NoSystem: - return str << "No System"; - case Camera::Result::ProtocolUnsupported: - return str << "Protocol Unsupported"; - default: - return str << "Unknown"; - } + _impl->focus_range_async(component_id, range, callback); } -bool operator==(const Camera::Position& lhs, const Camera::Position& rhs) +Camera::Result Camera::focus_range(int32_t component_id, float range) const { - return ((std::isnan(rhs.latitude_deg) && std::isnan(lhs.latitude_deg)) || - rhs.latitude_deg == lhs.latitude_deg) && - ((std::isnan(rhs.longitude_deg) && std::isnan(lhs.longitude_deg)) || - rhs.longitude_deg == lhs.longitude_deg) && - ((std::isnan(rhs.absolute_altitude_m) && std::isnan(lhs.absolute_altitude_m)) || - rhs.absolute_altitude_m == lhs.absolute_altitude_m) && - ((std::isnan(rhs.relative_altitude_m) && std::isnan(lhs.relative_altitude_m)) || - rhs.relative_altitude_m == lhs.relative_altitude_m); + return _impl->focus_range(component_id, range); } -std::ostream& operator<<(std::ostream& str, Camera::Position const& position) -{ - str << std::setprecision(15); - str << "position:" << '\n' << "{\n"; - str << " latitude_deg: " << position.latitude_deg << '\n'; - str << " longitude_deg: " << position.longitude_deg << '\n'; - str << " absolute_altitude_m: " << position.absolute_altitude_m << '\n'; - str << " relative_altitude_m: " << position.relative_altitude_m << '\n'; - str << '}'; - return str; -} - -bool operator==(const Camera::Quaternion& lhs, const Camera::Quaternion& rhs) +bool operator==(const Camera::Option& lhs, const Camera::Option& rhs) { - return ((std::isnan(rhs.w) && std::isnan(lhs.w)) || rhs.w == lhs.w) && - ((std::isnan(rhs.x) && std::isnan(lhs.x)) || rhs.x == lhs.x) && - ((std::isnan(rhs.y) && std::isnan(lhs.y)) || rhs.y == lhs.y) && - ((std::isnan(rhs.z) && std::isnan(lhs.z)) || rhs.z == lhs.z); + return (rhs.option_id == lhs.option_id) && (rhs.option_description == lhs.option_description); } -std::ostream& operator<<(std::ostream& str, Camera::Quaternion const& quaternion) +std::ostream& operator<<(std::ostream& str, Camera::Option const& option) { str << std::setprecision(15); - str << "quaternion:" << '\n' << "{\n"; - str << " w: " << quaternion.w << '\n'; - str << " x: " << quaternion.x << '\n'; - str << " y: " << quaternion.y << '\n'; - str << " z: " << quaternion.z << '\n'; + 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::EulerAngle& lhs, const Camera::EulerAngle& rhs) +bool operator==(const Camera::Setting& lhs, const Camera::Setting& rhs) { - return ((std::isnan(rhs.roll_deg) && std::isnan(lhs.roll_deg)) || - rhs.roll_deg == lhs.roll_deg) && - ((std::isnan(rhs.pitch_deg) && std::isnan(lhs.pitch_deg)) || - rhs.pitch_deg == lhs.pitch_deg) && - ((std::isnan(rhs.yaw_deg) && std::isnan(lhs.yaw_deg)) || rhs.yaw_deg == lhs.yaw_deg); + 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::EulerAngle const& euler_angle) +std::ostream& operator<<(std::ostream& str, Camera::Setting const& setting) { str << std::setprecision(15); - str << "euler_angle:" << '\n' << "{\n"; - str << " roll_deg: " << euler_angle.roll_deg << '\n'; - str << " pitch_deg: " << euler_angle.pitch_deg << '\n'; - str << " yaw_deg: " << euler_angle.yaw_deg << '\n'; + 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::CaptureInfo& lhs, const Camera::CaptureInfo& rhs) +bool operator==(const Camera::SettingOptions& lhs, const Camera::SettingOptions& rhs) { - return (rhs.position == lhs.position) && (rhs.attitude_quaternion == lhs.attitude_quaternion) && - (rhs.attitude_euler_angle == lhs.attitude_euler_angle) && - (rhs.time_utc_us == lhs.time_utc_us) && (rhs.is_success == lhs.is_success) && - (rhs.index == lhs.index) && (rhs.file_url == lhs.file_url); + return (rhs.component_id == lhs.component_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::CaptureInfo const& capture_info) +std::ostream& operator<<(std::ostream& str, Camera::SettingOptions const& setting_options) { str << std::setprecision(15); - str << "capture_info:" << '\n' << "{\n"; - str << " position: " << capture_info.position << '\n'; - str << " attitude_quaternion: " << capture_info.attitude_quaternion << '\n'; - str << " attitude_euler_angle: " << capture_info.attitude_euler_angle << '\n'; - str << " time_utc_us: " << capture_info.time_utc_us << '\n'; - str << " is_success: " << capture_info.is_success << '\n'; - str << " index: " << capture_info.index << '\n'; - str << " file_url: " << capture_info.file_url << '\n'; + str << "setting_options:" << '\n' << "{\n"; + str << " component_id: " << setting_options.component_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; } @@ -554,14 +510,15 @@ std::ostream& operator<<( } bool operator==(const Camera::VideoStreamInfo& lhs, const Camera::VideoStreamInfo& rhs) { - return (rhs.settings == lhs.settings) && (rhs.status == lhs.status) && - (rhs.spectrum == lhs.spectrum); + return (rhs.stream_id == lhs.stream_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 << " stream_id: " << video_stream_info.stream_id << '\n'; str << " settings: " << video_stream_info.settings << '\n'; str << " status: " << video_stream_info.status << '\n'; str << " spectrum: " << video_stream_info.spectrum << '\n'; @@ -569,44 +526,76 @@ std::ostream& operator<<(std::ostream& str, Camera::VideoStreamInfo const& video return str; } -std::ostream& operator<<(std::ostream& str, Camera::Status::StorageStatus const& storage_status) +bool operator==(const Camera::ModeUpdate& lhs, const Camera::ModeUpdate& rhs) +{ + return (rhs.component_id == lhs.component_id) && (rhs.mode == lhs.mode); +} + +std::ostream& operator<<(std::ostream& str, Camera::ModeUpdate const& mode_update) +{ + str << std::setprecision(15); + str << "mode_update:" << '\n' << "{\n"; + str << " component_id: " << mode_update.component_id << '\n'; + str << " mode: " << mode_update.mode << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::VideoStreamUpdate& lhs, const Camera::VideoStreamUpdate& rhs) +{ + return (rhs.component_id == lhs.component_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 << " component_id: " << video_stream_update.component_id << '\n'; + str << " video_stream_info: " << video_stream_update.video_stream_info << '\n'; + str << '}'; + return str; +} + +std::ostream& operator<<(std::ostream& str, Camera::Storage::StorageStatus const& storage_status) { switch (storage_status) { - case Camera::Status::StorageStatus::NotAvailable: + case Camera::Storage::StorageStatus::NotAvailable: return str << "Not Available"; - case Camera::Status::StorageStatus::Unformatted: + case Camera::Storage::StorageStatus::Unformatted: return str << "Unformatted"; - case Camera::Status::StorageStatus::Formatted: + case Camera::Storage::StorageStatus::Formatted: return str << "Formatted"; - case Camera::Status::StorageStatus::NotSupported: + case Camera::Storage::StorageStatus::NotSupported: return str << "Not Supported"; default: return str << "Unknown"; } } -std::ostream& operator<<(std::ostream& str, Camera::Status::StorageType const& storage_type) +std::ostream& operator<<(std::ostream& str, Camera::Storage::StorageType const& storage_type) { switch (storage_type) { - case Camera::Status::StorageType::Unknown: + case Camera::Storage::StorageType::Unknown: return str << "Unknown"; - case Camera::Status::StorageType::UsbStick: + case Camera::Storage::StorageType::UsbStick: return str << "Usb Stick"; - case Camera::Status::StorageType::Sd: + case Camera::Storage::StorageType::Sd: return str << "Sd"; - case Camera::Status::StorageType::Microsd: + case Camera::Storage::StorageType::Microsd: return str << "Microsd"; - case Camera::Status::StorageType::Hd: + case Camera::Storage::StorageType::Hd: return str << "Hd"; - case Camera::Status::StorageType::Other: + case Camera::Storage::StorageType::Other: return str << "Other"; default: return str << "Unknown"; } } -bool operator==(const Camera::Status& lhs, const Camera::Status& rhs) +bool operator==(const Camera::Storage& lhs, const Camera::Storage& rhs) { - return (rhs.video_on == lhs.video_on) && (rhs.photo_interval_on == lhs.photo_interval_on) && + return (rhs.component_id == lhs.component_id) && (rhs.video_on == lhs.video_on) && + (rhs.photo_interval_on == lhs.photo_interval_on) && ((std::isnan(rhs.used_storage_mib) && std::isnan(lhs.used_storage_mib)) || rhs.used_storage_mib == lhs.used_storage_mib) && ((std::isnan(rhs.available_storage_mib) && std::isnan(lhs.available_storage_mib)) || @@ -620,84 +609,213 @@ bool operator==(const Camera::Status& lhs, const Camera::Status& rhs) (rhs.storage_type == lhs.storage_type); } -std::ostream& operator<<(std::ostream& str, Camera::Status const& status) +std::ostream& operator<<(std::ostream& str, Camera::Storage const& storage) { str << std::setprecision(15); - str << "status:" << '\n' << "{\n"; - str << " video_on: " << status.video_on << '\n'; - str << " photo_interval_on: " << status.photo_interval_on << '\n'; - str << " used_storage_mib: " << status.used_storage_mib << '\n'; - str << " available_storage_mib: " << status.available_storage_mib << '\n'; - str << " total_storage_mib: " << status.total_storage_mib << '\n'; - str << " recording_time_s: " << status.recording_time_s << '\n'; - str << " media_folder_name: " << status.media_folder_name << '\n'; - str << " storage_status: " << status.storage_status << '\n'; - str << " storage_id: " << status.storage_id << '\n'; - str << " storage_type: " << status.storage_type << '\n'; + str << "storage:" << '\n' << "{\n"; + str << " component_id: " << storage.component_id << '\n'; + str << " video_on: " << storage.video_on << '\n'; + str << " photo_interval_on: " << storage.photo_interval_on << '\n'; + str << " used_storage_mib: " << storage.used_storage_mib << '\n'; + str << " available_storage_mib: " << storage.available_storage_mib << '\n'; + str << " total_storage_mib: " << storage.total_storage_mib << '\n'; + str << " recording_time_s: " << storage.recording_time_s << '\n'; + str << " media_folder_name: " << storage.media_folder_name << '\n'; + str << " storage_status: " << storage.storage_status << '\n'; + str << " storage_id: " << storage.storage_id << '\n'; + str << " storage_type: " << storage.storage_type << '\n'; str << '}'; return str; } -bool operator==(const Camera::Option& lhs, const Camera::Option& rhs) +bool operator==(const Camera::StorageUpdate& lhs, const Camera::StorageUpdate& rhs) { - return (rhs.option_id == lhs.option_id) && (rhs.option_description == lhs.option_description); + return (rhs.component_id == lhs.component_id) && (rhs.storage == lhs.storage); } -std::ostream& operator<<(std::ostream& str, Camera::Option const& option) +std::ostream& operator<<(std::ostream& str, Camera::StorageUpdate const& storage_update) { str << std::setprecision(15); - str << "option:" << '\n' << "{\n"; - str << " option_id: " << option.option_id << '\n'; - str << " option_description: " << option.option_description << '\n'; + str << "storage_update:" << '\n' << "{\n"; + str << " component_id: " << storage_update.component_id << '\n'; + str << " storage: " << storage_update.storage << '\n'; str << '}'; return str; } -bool operator==(const Camera::Setting& lhs, const Camera::Setting& rhs) +bool operator==(const Camera::CurrentSettingsUpdate& lhs, const Camera::CurrentSettingsUpdate& rhs) { - return (rhs.setting_id == lhs.setting_id) && - (rhs.setting_description == lhs.setting_description) && (rhs.option == lhs.option) && - (rhs.is_range == lhs.is_range); + return (rhs.component_id == lhs.component_id) && (rhs.current_settings == lhs.current_settings); } -std::ostream& operator<<(std::ostream& str, Camera::Setting const& setting) +std::ostream& +operator<<(std::ostream& str, Camera::CurrentSettingsUpdate const& current_settings_update) { 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 << "current_settings_update:" << '\n' << "{\n"; + str << " component_id: " << current_settings_update.component_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::SettingOptions& lhs, const Camera::SettingOptions& rhs) +bool operator==( + const Camera::PossibleSettingOptionsUpdate& lhs, + const Camera::PossibleSettingOptionsUpdate& rhs) { - return (rhs.setting_id == lhs.setting_id) && - (rhs.setting_description == lhs.setting_description) && (rhs.options == lhs.options) && - (rhs.is_range == lhs.is_range); + return (rhs.component_id == lhs.component_id) && (rhs.setting_options == lhs.setting_options); } -std::ostream& operator<<(std::ostream& str, Camera::SettingOptions const& setting_options) +std::ostream& operator<<( + std::ostream& str, Camera::PossibleSettingOptionsUpdate const& possible_setting_options_update) { str << std::setprecision(15); - str << "setting_options:" << '\n' << "{\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 << "possible_setting_options_update:" << '\n' << "{\n"; + str << " component_id: " << possible_setting_options_update.component_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 != setting_options.options.end() ? ", " : "]\n"); + str << (it + 1 != possible_setting_options_update.setting_options.end() ? ", " : "]\n"); } - str << " is_range: " << setting_options.is_range << '\n'; + str << '}'; + return str; +} + +std::ostream& operator<<(std::ostream& str, Camera::Result const& result) +{ + switch (result) { + case Camera::Result::Unknown: + return str << "Unknown"; + case Camera::Result::Success: + return str << "Success"; + case Camera::Result::InProgress: + return str << "In Progress"; + case Camera::Result::Busy: + return str << "Busy"; + case Camera::Result::Denied: + return str << "Denied"; + case Camera::Result::Error: + return str << "Error"; + case Camera::Result::Timeout: + return str << "Timeout"; + case Camera::Result::WrongArgument: + return str << "Wrong Argument"; + case Camera::Result::NoSystem: + return str << "No System"; + case Camera::Result::ProtocolUnsupported: + return str << "Protocol Unsupported"; + case Camera::Result::Unavailable: + return str << "Unavailable"; + case Camera::Result::CameraIdInvalid: + return str << "Camera Id Invalid"; + case Camera::Result::ActionUnsupported: + return str << "Action Unsupported"; + default: + return str << "Unknown"; + } +} + +bool operator==(const Camera::Position& lhs, const Camera::Position& rhs) +{ + return ((std::isnan(rhs.latitude_deg) && std::isnan(lhs.latitude_deg)) || + rhs.latitude_deg == lhs.latitude_deg) && + ((std::isnan(rhs.longitude_deg) && std::isnan(lhs.longitude_deg)) || + rhs.longitude_deg == lhs.longitude_deg) && + ((std::isnan(rhs.absolute_altitude_m) && std::isnan(lhs.absolute_altitude_m)) || + rhs.absolute_altitude_m == lhs.absolute_altitude_m) && + ((std::isnan(rhs.relative_altitude_m) && std::isnan(lhs.relative_altitude_m)) || + rhs.relative_altitude_m == lhs.relative_altitude_m); +} + +std::ostream& operator<<(std::ostream& str, Camera::Position const& position) +{ + str << std::setprecision(15); + str << "position:" << '\n' << "{\n"; + str << " latitude_deg: " << position.latitude_deg << '\n'; + str << " longitude_deg: " << position.longitude_deg << '\n'; + str << " absolute_altitude_m: " << position.absolute_altitude_m << '\n'; + str << " relative_altitude_m: " << position.relative_altitude_m << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::Quaternion& lhs, const Camera::Quaternion& rhs) +{ + return ((std::isnan(rhs.w) && std::isnan(lhs.w)) || rhs.w == lhs.w) && + ((std::isnan(rhs.x) && std::isnan(lhs.x)) || rhs.x == lhs.x) && + ((std::isnan(rhs.y) && std::isnan(lhs.y)) || rhs.y == lhs.y) && + ((std::isnan(rhs.z) && std::isnan(lhs.z)) || rhs.z == lhs.z); +} + +std::ostream& operator<<(std::ostream& str, Camera::Quaternion const& quaternion) +{ + str << std::setprecision(15); + str << "quaternion:" << '\n' << "{\n"; + str << " w: " << quaternion.w << '\n'; + str << " x: " << quaternion.x << '\n'; + str << " y: " << quaternion.y << '\n'; + str << " z: " << quaternion.z << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::EulerAngle& lhs, const Camera::EulerAngle& rhs) +{ + return ((std::isnan(rhs.roll_deg) && std::isnan(lhs.roll_deg)) || + rhs.roll_deg == lhs.roll_deg) && + ((std::isnan(rhs.pitch_deg) && std::isnan(lhs.pitch_deg)) || + rhs.pitch_deg == lhs.pitch_deg) && + ((std::isnan(rhs.yaw_deg) && std::isnan(lhs.yaw_deg)) || rhs.yaw_deg == lhs.yaw_deg); +} + +std::ostream& operator<<(std::ostream& str, Camera::EulerAngle const& euler_angle) +{ + str << std::setprecision(15); + str << "euler_angle:" << '\n' << "{\n"; + str << " roll_deg: " << euler_angle.roll_deg << '\n'; + str << " pitch_deg: " << euler_angle.pitch_deg << '\n'; + str << " yaw_deg: " << euler_angle.yaw_deg << '\n'; + str << '}'; + return str; +} + +bool operator==(const Camera::CaptureInfo& lhs, const Camera::CaptureInfo& rhs) +{ + return (rhs.component_id == lhs.component_id) && (rhs.position == lhs.position) && + (rhs.attitude_quaternion == lhs.attitude_quaternion) && + (rhs.attitude_euler_angle == lhs.attitude_euler_angle) && + (rhs.time_utc_us == lhs.time_utc_us) && (rhs.is_success == lhs.is_success) && + (rhs.index == lhs.index) && (rhs.file_url == lhs.file_url); +} + +std::ostream& operator<<(std::ostream& str, Camera::CaptureInfo const& capture_info) +{ + str << std::setprecision(15); + str << "capture_info:" << '\n' << "{\n"; + str << " component_id: " << capture_info.component_id << '\n'; + str << " position: " << capture_info.position << '\n'; + str << " attitude_quaternion: " << capture_info.attitude_quaternion << '\n'; + str << " attitude_euler_angle: " << capture_info.attitude_euler_angle << '\n'; + str << " time_utc_us: " << capture_info.time_utc_us << '\n'; + str << " is_success: " << capture_info.is_success << '\n'; + str << " index: " << capture_info.index << '\n'; + str << " file_url: " << capture_info.file_url << '\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) && + return (rhs.component_id == lhs.component_id) && (rhs.vendor_name == lhs.vendor_name) && + (rhs.model_name == lhs.model_name) && ((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) && @@ -713,6 +831,7 @@ std::ostream& operator<<(std::ostream& str, Camera::Information const& informati { str << std::setprecision(15); str << "information:" << '\n' << "{\n"; + str << " component_id: " << information.component_id << '\n'; str << " vendor_name: " << information.vendor_name << '\n'; str << " model_name: " << information.model_name << '\n'; str << " focal_length_mm: " << information.focal_length_mm << '\n'; @@ -724,6 +843,24 @@ std::ostream& operator<<(std::ostream& str, Camera::Information const& informati return str; } +bool operator==(const Camera::CameraList& lhs, const Camera::CameraList& rhs) +{ + return (rhs.cameras == lhs.cameras); +} + +std::ostream& operator<<(std::ostream& str, Camera::CameraList const& camera_list) +{ + str << std::setprecision(15); + str << "camera_list:" << '\n' << "{\n"; + str << " cameras: ["; + for (auto it = camera_list.cameras.begin(); it != camera_list.cameras.end(); ++it) { + str << *it; + str << (it + 1 != camera_list.cameras.end() ? ", " : "]\n"); + } + str << '}'; + return str; +} + std::ostream& operator<<(std::ostream& str, Camera::Mode const& mode) { switch (mode) { diff --git a/src/mavsdk/plugins/camera/camera_definition.cpp b/src/mavsdk/plugins/camera/camera_definition.cpp index 01ac614a97..0a237058a4 100644 --- a/src/mavsdk/plugins/camera/camera_definition.cpp +++ b/src/mavsdk/plugins/camera/camera_definition.cpp @@ -3,10 +3,6 @@ namespace mavsdk { -CameraDefinition::CameraDefinition() {} - -CameraDefinition::~CameraDefinition() {} - bool CameraDefinition::load_file(const std::string& filepath) { tinyxml2::XMLError xml_error = _doc.LoadFile(filepath.c_str()); @@ -31,22 +27,16 @@ bool CameraDefinition::load_string(const std::string& content) std::string CameraDefinition::get_model() const { - std::lock_guard lock(_mutex); - return _model; } std::string CameraDefinition::get_vendor() const { - std::lock_guard lock(_mutex); - return _vendor; } bool CameraDefinition::parse_xml() { - std::lock_guard lock(_mutex); - auto e_mavlinkcamera = _doc.FirstChildElement("mavlinkcamera"); if (!e_mavlinkcamera) { LogErr() << "Tag mavlinkcamera not found"; @@ -187,7 +177,7 @@ bool CameraDefinition::parse_xml() for (auto e_update = e_updates->FirstChildElement("update"); e_update != nullptr; e_update = e_update->NextSiblingElement("update")) { // LogDebug() << "Updates: " << e_update->GetText(); - new_parameter->updates.push_back(e_update->GetText()); + new_parameter->updates.emplace_back(e_update->GetText()); } } @@ -242,7 +232,7 @@ bool CameraDefinition::parse_xml() } else { auto maybe_range_options = parse_range_options(e_parameter, param_name, type_map); if (!std::get<0>(maybe_range_options)) { - LogWarn() << "Not found: " << param_name; + LogWarn() << "Range not found for: " << param_name; continue; } @@ -296,7 +286,7 @@ CameraDefinition::parse_options( for (auto e_exclude = e_exclusions->FirstChildElement("exclude"); e_exclude != nullptr; e_exclude = e_exclude->NextSiblingElement("exclude")) { // LogDebug() << "Exclude: " << e_exclude->GetText(); - new_option->exclusions.push_back(e_exclude->GetText()); + new_option->exclusions.emplace_back(e_exclude->GetText()); } } @@ -367,7 +357,7 @@ CameraDefinition::parse_range_options( const char* min_str = param_handle->Attribute("min"); if (!min_str) { - LogErr() << "min range missing for " << param_name; + LogDebug() << "min range missing for " << param_name; return std::make_tuple<>(false, options, default_option); } @@ -376,7 +366,7 @@ CameraDefinition::parse_range_options( const char* max_str = param_handle->Attribute("max"); if (!max_str) { - LogErr() << "max range missing for " << param_name; + LogDebug() << "max range missing for " << param_name; return std::make_tuple<>(false, options, default_option); } @@ -453,59 +443,32 @@ std::pair CameraDefinition::find_default( void CameraDefinition::assume_default_settings() { - std::lock_guard lock(_mutex); + reset_to_default_settings(false); +} +void CameraDefinition::reset_to_default_settings(bool needs_updating) +{ _current_settings.clear(); for (const auto& parameter : _parameter_map) { - // if (parameter.second->is_range) { - InternalCurrentSetting new_setting; new_setting.value = parameter.second->default_option.value; - new_setting.needs_updating = false; + new_setting.needs_updating = needs_updating; _current_settings[parameter.first] = new_setting; - - //} else { - - // for (const auto &option : parameter.second->options) { - // if (!option->is_default) { - // //LogDebug() << option->name << " not default"; - // continue; - // } - // //LogDebug() << option->name << " default value: " << option->value << " (type: " - // << - // // option->value.typestr() << ")"; - - // InternalCurrentSetting new_setting; - // new_setting.value = option->value; - // new_setting.needs_updating = false; - // _current_settings[parameter.first] = new_setting; - // } - //} } } bool CameraDefinition::get_all_settings(std::unordered_map& settings) { - std::lock_guard lock(_mutex); - settings.clear(); for (const auto& current_setting : _current_settings) { settings[current_setting.first] = current_setting.second.value; } - return (settings.size() > 0); + return !settings.empty(); } bool CameraDefinition::get_possible_settings(std::unordered_map& settings) -{ - std::lock_guard lock(_mutex); - - return get_possible_settings_locked(settings); -} - -bool CameraDefinition::get_possible_settings_locked( - std::unordered_map& settings) { settings.clear(); @@ -542,13 +505,11 @@ bool CameraDefinition::get_possible_settings_locked( settings[setting.first] = setting.second.value; } - return (settings.size() > 0); + return !settings.empty(); } bool CameraDefinition::set_setting(const std::string& name, const ParamValue& value) { - std::lock_guard lock(_mutex); - if (_parameter_map.find(name) == _parameter_map.end()) { LogErr() << "Unknown setting to set: " << name; return false; @@ -578,7 +539,7 @@ bool CameraDefinition::set_setting(const std::string& name, const ParamValue& va // needs to happen outside of this class. for (const auto& update : _parameter_map[name]->updates) { if (_current_settings.find(update) == _current_settings.end()) { - // LogDebug() << "Update to '" << update << "' not understood."; + LogDebug() << "Update to '" << update << "' not understood."; continue; } _current_settings[update].needs_updating = true; @@ -589,8 +550,6 @@ bool CameraDefinition::set_setting(const std::string& name, const ParamValue& va bool CameraDefinition::get_setting(const std::string& name, ParamValue& value) { - std::lock_guard lock(_mutex); - if (_current_settings.find(name) == _current_settings.end()) { LogErr() << "Unknown setting to get: " << name; return false; @@ -607,8 +566,6 @@ bool CameraDefinition::get_setting(const std::string& name, ParamValue& value) bool CameraDefinition::get_option_value( const std::string& param_name, const std::string& option_value, ParamValue& value) { - std::lock_guard lock(_mutex); - if (_parameter_map.find(param_name) == _parameter_map.end()) { LogErr() << "Unknown parameter to get option: " << param_name; return false; @@ -626,8 +583,6 @@ bool CameraDefinition::get_option_value( bool CameraDefinition::get_all_options(const std::string& name, std::vector& values) { - std::lock_guard lock(_mutex); - values.clear(); if (_parameter_map.find(name) == _parameter_map.end()) { @@ -645,8 +600,6 @@ bool CameraDefinition::get_all_options(const std::string& name, std::vector& values) { - std::lock_guard lock(_mutex); - values.clear(); if (_parameter_map.find(name) == _parameter_map.end()) { @@ -655,7 +608,7 @@ bool CameraDefinition::get_possible_options( } std::unordered_map settings; - if (!get_possible_settings_locked(settings)) { + if (!get_possible_settings(settings)) { return false; } if (settings.find(name) == settings.end()) { @@ -711,7 +664,7 @@ bool CameraDefinition::get_possible_options( // Only look at current set option. if (_current_settings[parameter.first].value == option->value) { // Go through parameter ranges but only concerning the parameter that - // we're interested in.. + // we're interested in. if (option->parameter_ranges.find(name) != option->parameter_ranges.end()) { for (const auto& range : option->parameter_ranges[name]) { allowed_ranges.push_back(range.second); @@ -729,7 +682,7 @@ bool CameraDefinition::get_possible_options( option_allowed = true; } } - if (option_allowed || allowed_ranges.size() == 0) { + if (option_allowed || allowed_ranges.empty()) { values.push_back(option->value); } } @@ -739,8 +692,6 @@ bool CameraDefinition::get_possible_options( void CameraDefinition::get_unknown_params(std::vector>& params) { - std::lock_guard lock(_mutex); - params.clear(); for (const auto& parameter : _parameter_map) { @@ -752,8 +703,6 @@ void CameraDefinition::get_unknown_params(std::vector lock(_mutex); - for (auto& parameter : _parameter_map) { _current_settings[parameter.first].needs_updating = true; } @@ -761,8 +710,6 @@ void CameraDefinition::set_all_params_unknown() bool CameraDefinition::is_setting_range(const std::string& name) { - std::lock_guard lock(_mutex); - if (_parameter_map.find(name) == _parameter_map.end()) { LogWarn() << "Setting " << name << " not found."; return false; @@ -773,8 +720,6 @@ bool CameraDefinition::is_setting_range(const std::string& name) bool CameraDefinition::get_setting_str(const std::string& name, std::string& description) { - std::lock_guard lock(_mutex); - description.clear(); if (_parameter_map.find(name) == _parameter_map.end()) { @@ -789,8 +734,6 @@ bool CameraDefinition::get_setting_str(const std::string& name, std::string& des bool CameraDefinition::get_option_str( const std::string& setting_name, const std::string& option_name, std::string& description) { - std::lock_guard lock(_mutex); - description.clear(); if (_parameter_map.find(setting_name) == _parameter_map.end()) { @@ -799,8 +742,6 @@ bool CameraDefinition::get_option_str( } for (const auto& option : _parameter_map[setting_name]->options) { - std::stringstream value_ss{}; - value_ss << option->value; if (option->value == option_name) { description = option->name; return true; diff --git a/src/mavsdk/plugins/camera/camera_definition.h b/src/mavsdk/plugins/camera/camera_definition.h index 74d8697f95..a09ef8c223 100644 --- a/src/mavsdk/plugins/camera/camera_definition.h +++ b/src/mavsdk/plugins/camera/camera_definition.h @@ -5,7 +5,6 @@ #include #include #include -#include #include #include #include @@ -14,8 +13,8 @@ namespace mavsdk { class CameraDefinition { public: - CameraDefinition(); - ~CameraDefinition(); + CameraDefinition() = default; + ~CameraDefinition() = default; bool load_file(const std::string& filepath); bool load_string(const std::string& content); @@ -23,8 +22,12 @@ class CameraDefinition { std::string get_vendor() const; std::string get_model() const; + // This is to just assume everything is the default, mostly for testing. void assume_default_settings(); + // This is to start and mark things as requiring an update. + void reset_to_default_settings(bool needs_updating); + struct Setting { std::string name; ParamValue value; @@ -54,8 +57,6 @@ class CameraDefinition { const CameraDefinition& operator=(const CameraDefinition&) = delete; private: - bool get_possible_settings_locked(std::unordered_map& settings); - using ParameterRange = std::unordered_map; struct Option { @@ -92,8 +93,6 @@ class CameraDefinition { std::pair find_default( const std::vector>& options, const std::string& default_str); - mutable std::mutex _mutex{}; - tinyxml2::XMLDocument _doc{}; std::unordered_map> _parameter_map{}; diff --git a/src/mavsdk/plugins/camera/camera_definition_test.cpp b/src/mavsdk/plugins/camera/camera_definition_test.cpp index 83e1225423..370ab8f5dd 100644 --- a/src/mavsdk/plugins/camera/camera_definition_test.cpp +++ b/src/mavsdk/plugins/camera/camera_definition_test.cpp @@ -8,7 +8,7 @@ using namespace mavsdk; -static const std::string e90_unit_test_file = "src/mavsdk/plugins/camera/e90_unit_test.xml"; +static const std::string e90_unit_test_file = "src/mavsdk/plugins/camera/e90_camera.xml"; TEST(CameraDefinition, E90LoadInfoFile) { @@ -44,7 +44,7 @@ TEST(CameraDefinition, E90CheckDefaultSettings) { std::unordered_map settings{}; EXPECT_TRUE(cd.get_all_settings(settings)); - EXPECT_EQ(settings.size(), 17); + EXPECT_EQ(settings.size(), 18); EXPECT_EQ(settings["CAM_MODE"].get(), 1); EXPECT_EQ(settings["CAM_WBMODE"].get(), 0); @@ -55,6 +55,7 @@ TEST(CameraDefinition, E90CheckDefaultSettings) EXPECT_EQ(settings["CAM_VIDRES"].get(), 0); EXPECT_EQ(settings["CAM_VIDFMT"].get(), 1); EXPECT_EQ(settings["CAM_PHOTORATIO"].get(), 1); + EXPECT_EQ(settings["CAM_ASPECTRATIO"].get(), 1.78f); EXPECT_EQ(settings["CAM_METERING"].get(), 0); EXPECT_EQ(settings["CAM_COLORMODE"].get(), 1); EXPECT_EQ(settings["CAM_FLICKER"].get(), 0); @@ -284,7 +285,7 @@ TEST(CameraDefinition, E90SettingsToUpdate) { std::vector> params; cd.get_unknown_params(params); - EXPECT_EQ(params.size(), 17); + EXPECT_EQ(params.size(), 18); } { @@ -297,7 +298,7 @@ TEST(CameraDefinition, E90SettingsToUpdate) { std::vector> params; cd.get_unknown_params(params); - EXPECT_EQ(params.size(), 16); + EXPECT_EQ(params.size(), 17); } cd.set_all_params_unknown(); @@ -305,7 +306,7 @@ TEST(CameraDefinition, E90SettingsToUpdate) { std::vector> params; cd.get_unknown_params(params); - EXPECT_EQ(params.size(), 17); + EXPECT_EQ(params.size(), 18); } } @@ -336,7 +337,7 @@ TEST(CameraDefinition, E90SettingsCauseUpdates) { std::vector> params; cd.get_unknown_params(params); - EXPECT_EQ(params.size(), 4); + EXPECT_EQ(params.size(), 5); // TODO: improve the poor man's vector search. bool found_shutterspd = false; @@ -484,7 +485,7 @@ TEST(CameraDefinition, E90OptionHumanReadable) EXPECT_STREQ(description.c_str(), ""); } -static const std::string uvc_unit_test_file = "src/mavsdk/plugins/camera/uvc_unit_test.xml"; +static const std::string uvc_unit_test_file = "src/mavsdk/plugins/camera/uvc_camera.xml"; TEST(CameraDefinition, UVCLoadInfoFile) { @@ -521,22 +522,22 @@ TEST(CameraDefinition, UVCCheckDefaultSettings) EXPECT_TRUE(cd.get_all_settings(settings)); EXPECT_EQ(settings.size(), 13); - EXPECT_TRUE(cd.is_setting_range("brightness")); - EXPECT_FALSE(cd.is_setting_range("exp-mode")); - - EXPECT_EQ(settings["camera-mode"].get(), 1); - EXPECT_EQ(settings["brightness"].get(), 128); - EXPECT_EQ(settings["contrast"].get(), 32); - EXPECT_EQ(settings["saturation"].get(), 32); - EXPECT_EQ(settings["gain"].get(), 64); - EXPECT_EQ(settings["sharpness"].get(), 24); - EXPECT_EQ(settings["backlight"].get(), 0); - EXPECT_EQ(settings["power-mode"].get(), 0); - EXPECT_EQ(settings["wb-mode"].get(), 1); - EXPECT_EQ(settings["wb-temp"].get(), 4000); - EXPECT_EQ(settings["exp-mode"].get(), 3); - EXPECT_EQ(settings["exp-absolute"].get(), 166); - EXPECT_EQ(settings["exp-priority"].get(), 1); + EXPECT_TRUE(cd.is_setting_range("BRIGHTNESS")); + EXPECT_FALSE(cd.is_setting_range("EXP_MODE")); + + EXPECT_EQ(settings["CAM_MODE"].get(), 1); + EXPECT_EQ(settings["BRIGHTNESS"].get(), 128); + EXPECT_EQ(settings["CONTRAST"].get(), 32); + EXPECT_EQ(settings["SATURATION"].get(), 32); + EXPECT_EQ(settings["GAIN"].get(), 64); + EXPECT_EQ(settings["SHARPNESS"].get(), 24); + EXPECT_EQ(settings["BACKLIGHT"].get(), 0); + EXPECT_EQ(settings["POWER_MODE"].get(), 0); + EXPECT_EQ(settings["WB_MODE"].get(), 1); + EXPECT_EQ(settings["WB_TEMP"].get(), 4000); + EXPECT_EQ(settings["EXP_MODE"].get(), 3); + EXPECT_EQ(settings["EXP_ABSOLUTE"].get(), 166); + EXPECT_EQ(settings["EXP_PRIORITY"].get(), 1); } TEST(CameraDefinition, UVCCheckPossibleSettings) @@ -551,14 +552,14 @@ TEST(CameraDefinition, UVCCheckPossibleSettings) { ParamValue value; value.set(1); - EXPECT_TRUE(cd.set_setting("wb-mode", value)); + EXPECT_TRUE(cd.set_setting("WB_MODE", value)); } // And exposure mode aperture priority. { ParamValue value; value.set(3); - EXPECT_TRUE(cd.set_setting("exp-mode", value)); + EXPECT_TRUE(cd.set_setting("EXP_MODE", value)); } std::unordered_map settings{}; @@ -569,7 +570,7 @@ TEST(CameraDefinition, UVCCheckPossibleSettings) { ParamValue value; value.set(1); - EXPECT_TRUE(cd.set_setting("exp-mode", value)); + EXPECT_TRUE(cd.set_setting("EXP_MODE", value)); } EXPECT_TRUE(cd.get_possible_settings(settings)); @@ -585,21 +586,21 @@ TEST(CameraDefinition, UVCSetRangeSetting) { ParamValue value; value.set(200); - EXPECT_TRUE(cd.set_setting("brightness", value)); + EXPECT_TRUE(cd.set_setting("BRIGHTNESS", value)); } { // Try too big. ParamValue value; value.set(400); - EXPECT_FALSE(cd.set_setting("brightness", value)); + EXPECT_FALSE(cd.set_setting("BRIGHTNESS", value)); } { // Try too small. ParamValue value; value.set(-100); - EXPECT_FALSE(cd.set_setting("brightness", value)); + EXPECT_FALSE(cd.set_setting("BRIGHTNESS", value)); } } @@ -610,13 +611,13 @@ TEST(CameraDefinition, UVCCheckSettingHumanReadable) ASSERT_TRUE(cd.load_file(uvc_unit_test_file)); std::string description{}; - EXPECT_TRUE(cd.get_setting_str("brightness", description)); + EXPECT_TRUE(cd.get_setting_str("BRIGHTNESS", description)); EXPECT_STREQ(description.c_str(), "Brightness"); - EXPECT_TRUE(cd.get_setting_str("wb-mode", description)); + EXPECT_TRUE(cd.get_setting_str("WB_MODE", description)); EXPECT_STREQ(description.c_str(), "White Balance Mode"); - EXPECT_TRUE(cd.get_setting_str("exp-priority", description)); + EXPECT_TRUE(cd.get_setting_str("EXP_PRIORITY", description)); EXPECT_STREQ(description.c_str(), "Exposure Auto Priority"); } @@ -628,16 +629,16 @@ TEST(CameraDefinition, UVCCheckOptionHumanReadable) std::string description{}; // TODO: range params are currently a bit weird like that. - EXPECT_TRUE(cd.get_option_str("brightness", "0", description)); + EXPECT_TRUE(cd.get_option_str("BRIGHTNESS", "0", description)); EXPECT_STREQ(description.c_str(), "min"); - EXPECT_TRUE(cd.get_option_str("brightness", "225", description)); + EXPECT_TRUE(cd.get_option_str("BRIGHTNESS", "225", description)); EXPECT_STREQ(description.c_str(), "max"); - EXPECT_TRUE(cd.get_option_str("brightness", "1", description)); + EXPECT_TRUE(cd.get_option_str("BRIGHTNESS", "1", description)); EXPECT_STREQ(description.c_str(), "step"); - EXPECT_TRUE(cd.get_option_str("wb-mode", "0", description)); + EXPECT_TRUE(cd.get_option_str("WB_MODE", "0", description)); EXPECT_STREQ(description.c_str(), "Manual Mode"); - EXPECT_TRUE(cd.get_option_str("exp-priority", "1", description)); + EXPECT_TRUE(cd.get_option_str("EXP_PRIORITY", "1", description)); EXPECT_STREQ(description.c_str(), "ON"); } diff --git a/src/mavsdk/plugins/camera/camera_impl.cpp b/src/mavsdk/plugins/camera/camera_impl.cpp index 716b739352..e3078f892e 100644 --- a/src/mavsdk/plugins/camera/camera_impl.cpp +++ b/src/mavsdk/plugins/camera/camera_impl.cpp @@ -1,18 +1,20 @@ #include "camera_impl.h" #include "camera_definition.h" #include "system.h" -#include "mavsdk_math.h" #include "http_loader.h" #include "unused.h" #include "callback_list.tpp" #include "fs_utils.h" +#include "string_utils.h" +#include "math_utils.h" #include +#include #include +#include #include -#include #include -#include +#include namespace mavsdk { @@ -21,7 +23,7 @@ template class CallbackList>; template class CallbackList>; template class CallbackList; template class CallbackList; -template class CallbackList; +template class CallbackList; CameraImpl::CameraImpl(System& system) : PluginImplBase(system) { @@ -40,308 +42,184 @@ CameraImpl::~CameraImpl() void CameraImpl::init() { - _system_impl->register_mavlink_message_handler_with_compid( + if (const char* env_p = std::getenv("MAVSDK_CAMERA_DEBUGGING")) { + if (std::string(env_p) == "1") { + LogDebug() << "Camera debugging is on."; + _debugging = true; + } + } + + const auto cache_dir_option = get_cache_directory(); + if (cache_dir_option) { + _file_cache.emplace(cache_dir_option.value() / "camera", 50, true); + } else { + LogErr() << "Failed to get cache directory"; + } + + const auto tmp_option = create_tmp_directory("mavsdk-component-metadata"); + if (tmp_option) { + _tmp_download_path = tmp_option.value(); + } else { + _tmp_download_path = "./mavsdk-component-metadata"; + std::error_code err; + std::filesystem::create_directory(_tmp_download_path, err); + } + + _system_impl->register_mavlink_message_handler( + MAVLINK_MSG_ID_HEARTBEAT, + [this](const mavlink_message_t& message) { process_heartbeat(message); }, + this); + + _system_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, - _camera_id + MAV_COMP_ID_CAMERA, [this](const mavlink_message_t& message) { process_camera_capture_status(message); }, this); - _system_impl->register_mavlink_message_handler_with_compid( + _system_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_STORAGE_INFORMATION, - _camera_id + MAV_COMP_ID_CAMERA, [this](const mavlink_message_t& message) { process_storage_information(message); }, this); - _system_impl->register_mavlink_message_handler_with_compid( + _system_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, - _camera_id + MAV_COMP_ID_CAMERA, [this](const mavlink_message_t& message) { process_camera_image_captured(message); }, this); - _system_impl->register_mavlink_message_handler_with_compid( + _system_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_CAMERA_SETTINGS, - _camera_id + MAV_COMP_ID_CAMERA, [this](const mavlink_message_t& message) { process_camera_settings(message); }, this); - _system_impl->register_mavlink_message_handler_with_compid( + _system_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_CAMERA_INFORMATION, - _camera_id + MAV_COMP_ID_CAMERA, [this](const mavlink_message_t& message) { process_camera_information(message); }, this); - _system_impl->register_mavlink_message_handler_with_compid( + _system_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, - _camera_id + MAV_COMP_ID_CAMERA, [this](const mavlink_message_t& message) { process_video_information(message); }, this); - _system_impl->register_mavlink_message_handler_with_compid( + _system_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, - _camera_id + MAV_COMP_ID_CAMERA, [this](const mavlink_message_t& message) { process_video_stream_status(message); }, this); - _check_connection_status_call_every_cookie = - _system_impl->add_call_every([this]() { check_connection_status(); }, 0.5); - _request_missing_capture_info_cookie = _system_impl->add_call_every([this]() { request_missing_capture_info(); }, 0.5); + + // TODO: use SET_INTERVAL commands instead and fall back to request commands when needed. + _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->remove_call_every(_request_missing_capture_info_cookie); - _system_impl->remove_call_every(_check_connection_status_call_every_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->unregister_all_mavlink_message_handlers(this); - _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(_information.mutex); - _information.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(); - } - - _camera_found = false; -} - -Camera::Result CameraImpl::prepare() -{ - auto prom = std::make_shared>(); - auto ret = prom->get_future(); - - prepare_async([&prom](Camera::Result result) { prom->set_value(result); }); - - return ret.get(); -} - -void CameraImpl::prepare_async(const Camera::ResultCallback& callback) -{ - auto temp_callback = callback; - - std::lock_guard lock(_information.mutex); - - if (_camera_definition) { - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Success); }); - } else { - _camera_definition_callback = [this, temp_callback](Camera::Result result) { - temp_callback(result); - _camera_definition_callback = nullptr; - }; - - if (_has_camera_definition_timed_out) { - // Try to download the camera_definition again - _has_camera_definition_timed_out = false; - request_camera_information(); - } - } -} - -void CameraImpl::check_connection_status() -{ - // FIXME: This is a workaround because we don't want to be tied to the - // discovery of an autopilot which triggers enable() and disable() but - // we are interested if a camera is connected or not. - if (_system_impl->has_camera(_camera_id)) { - if (!_camera_found) { - _camera_found = true; - manual_enable(); - } - } -} - -void CameraImpl::enable() -{ - // FIXME: We check for the connection status manually because - // we're not interested in the connection state of the autopilot - // but only the camera. -} - -void CameraImpl::manual_enable() -{ - refresh_params(); - - request_status(); - request_camera_information(); - - _camera_information_call_every_cookie = - _system_impl->add_call_every([this]() { request_camera_information(); }, 10.0); - - _status.call_every_cookie = _system_impl->add_call_every([this]() { request_status(); }, 5.0); -} - -void CameraImpl::disable() -{ - // FIXME: We check for the connection status manually because - // we're not interested in the connection state of the autopilot - // but only the camera. -} - -void CameraImpl::manual_disable() -{ - invalidate_params(); - _system_impl->remove_call_every(_camera_information_call_every_cookie); - _system_impl->remove_call_every(_status.call_every_cookie); - - _camera_found = false; -} - -void CameraImpl::update_component() -{ - uint8_t cmp_id = _camera_id + MAV_COMP_ID_CAMERA; - _system_impl->update_component_id_messages_handler( - MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, cmp_id, this); - _system_impl->update_component_id_messages_handler( - MAVLINK_MSG_ID_STORAGE_INFORMATION, cmp_id, this); - - _system_impl->update_component_id_messages_handler( - MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, cmp_id, this); - - _system_impl->update_component_id_messages_handler( - MAVLINK_MSG_ID_CAMERA_SETTINGS, cmp_id, this); - - _system_impl->update_component_id_messages_handler( - MAVLINK_MSG_ID_CAMERA_INFORMATION, cmp_id, this); + _system_impl->cancel_all_param(this); - _system_impl->update_component_id_messages_handler( - MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, cmp_id, this); + std::lock_guard lock(_mutex); + _system_impl->remove_call_every(_request_missing_capture_info_cookie); + _system_impl->remove_call_every(_request_slower_call_every_cookie); + _system_impl->remove_call_every(_request_faster_call_every_cookie); - _system_impl->update_component_id_messages_handler( - MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, cmp_id, this); + _storage_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(); } -Camera::Result CameraImpl::select_camera(const size_t id) -{ - static constexpr std::size_t MAX_SUPPORTED_ID = 5; - - if (id > MAX_SUPPORTED_ID) { - return Camera::Result::WrongArgument; - } - - // camera component IDs go from 100 to 105. - _camera_id = id; - - // We should probably reload everything to make sure the - // correct camera is initialized. - update_component(); - manual_disable(); - manual_enable(); - - return Camera::Result::Success; -} +void CameraImpl::enable() {} +void CameraImpl::disable() {} MavlinkCommandSender::CommandLong -CameraImpl::make_command_take_photo(float interval_s, float no_of_photos) +CameraImpl::make_command_take_photo(int32_t component_id, float interval_s, float no_of_photos) { - MavlinkCommandSender::CommandLong cmd_take_photo{}; + MavlinkCommandSender::CommandLong cmd{}; - cmd_take_photo.command = MAV_CMD_IMAGE_START_CAPTURE; - cmd_take_photo.params.maybe_param1 = 0.0f; // Reserved, set to 0 - cmd_take_photo.params.maybe_param2 = interval_s; - cmd_take_photo.params.maybe_param3 = no_of_photos; - cmd_take_photo.params.maybe_param4 = static_cast(_capture.sequence++); - cmd_take_photo.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.command = MAV_CMD_IMAGE_START_CAPTURE; + cmd.params.maybe_param1 = 0.0f; // Reserved, set to 0 + cmd.params.maybe_param2 = interval_s; + cmd.params.maybe_param3 = no_of_photos; + cmd.params.maybe_param4 = get_and_increment_capture_sequence(component_id); + cmd.target_component_id = fixup_component_target(component_id); - return cmd_take_photo; + return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_zoom_out() +MavlinkCommandSender::CommandLong CameraImpl::make_command_zoom_out(int32_t component_id) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_SET_CAMERA_ZOOM; - cmd.params.maybe_param1 = (float)ZOOM_TYPE_CONTINUOUS; + cmd.params.maybe_param1 = static_cast(ZOOM_TYPE_CONTINUOUS); cmd.params.maybe_param2 = -1.f; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_zoom_in() +MavlinkCommandSender::CommandLong CameraImpl::make_command_zoom_in(int32_t component_id) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_SET_CAMERA_ZOOM; - cmd.params.maybe_param1 = (float)ZOOM_TYPE_CONTINUOUS; + cmd.params.maybe_param1 = static_cast(ZOOM_TYPE_CONTINUOUS); cmd.params.maybe_param2 = 1.f; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_zoom_stop() +MavlinkCommandSender::CommandLong CameraImpl::make_command_zoom_stop(int32_t component_id) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_SET_CAMERA_ZOOM; - cmd.params.maybe_param1 = (float)ZOOM_TYPE_CONTINUOUS; + cmd.params.maybe_param1 = static_cast(ZOOM_TYPE_CONTINUOUS); cmd.params.maybe_param2 = 0.f; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_zoom_range(float range) +MavlinkCommandSender::CommandLong +CameraImpl::make_command_zoom_range(int32_t component_id, float range) { // Clip to safe range. range = std::max(0.f, std::min(range, 100.f)); MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_SET_CAMERA_ZOOM; - cmd.params.maybe_param1 = (float)ZOOM_TYPE_RANGE; + cmd.params.maybe_param1 = static_cast(ZOOM_TYPE_RANGE); cmd.params.maybe_param2 = range; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong -CameraImpl::make_command_track_point(float point_x, float point_y, float radius) +MavlinkCommandSender::CommandLong CameraImpl::make_command_track_point( + int32_t component_id, float point_x, float point_y, float radius) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_CAMERA_TRACK_POINT; - cmd.params.maybe_param1 = (float)point_x; - cmd.params.maybe_param2 = (float)point_y; - cmd.params.maybe_param3 = (float)radius; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.params.maybe_param1 = point_x; + cmd.params.maybe_param2 = point_y; + cmd.params.maybe_param3 = radius; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } MavlinkCommandSender::CommandLong CameraImpl::make_command_track_rectangle( - float top_left_x, float top_left_y, float bottom_right_x, float bottom_right_y) + int32_t component_id, + float top_left_x, + float top_left_y, + float bottom_right_x, + float bottom_right_y) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_CAMERA_TRACK_RECTANGLE; @@ -349,291 +227,263 @@ MavlinkCommandSender::CommandLong CameraImpl::make_command_track_rectangle( cmd.params.maybe_param2 = top_left_y; cmd.params.maybe_param3 = bottom_right_x; cmd.params.maybe_param4 = bottom_right_y; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_track_stop() + +MavlinkCommandSender::CommandLong CameraImpl::make_command_track_stop(int32_t component_id) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_CAMERA_STOP_TRACKING; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_focus_in() + +MavlinkCommandSender::CommandLong CameraImpl::make_command_focus_in(int32_t component_id) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_SET_CAMERA_FOCUS; - cmd.params.maybe_param1 = (float)FOCUS_TYPE_CONTINUOUS; + cmd.params.maybe_param1 = static_cast(FOCUS_TYPE_CONTINUOUS); cmd.params.maybe_param2 = -1.f; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_focus_out() + +MavlinkCommandSender::CommandLong CameraImpl::make_command_focus_out(int32_t component_id) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_SET_CAMERA_FOCUS; - cmd.params.maybe_param1 = (float)FOCUS_TYPE_CONTINUOUS; + cmd.params.maybe_param1 = static_cast(FOCUS_TYPE_CONTINUOUS); cmd.params.maybe_param2 = 1.f; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_focus_stop() + +MavlinkCommandSender::CommandLong CameraImpl::make_command_focus_stop(int32_t component_id) { MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_SET_CAMERA_FOCUS; - cmd.params.maybe_param1 = (float)FOCUS_TYPE_CONTINUOUS; + cmd.params.maybe_param1 = static_cast(FOCUS_TYPE_CONTINUOUS); cmd.params.maybe_param2 = 0.f; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_focus_range(float range) + +MavlinkCommandSender::CommandLong +CameraImpl::make_command_focus_range(int32_t component_id, float range) { // Clip to safe range. range = std::max(0.f, std::min(range, 100.f)); MavlinkCommandSender::CommandLong cmd{}; cmd.command = MAV_CMD_SET_CAMERA_FOCUS; - cmd.params.maybe_param1 = (float)FOCUS_TYPE_RANGE; + cmd.params.maybe_param1 = static_cast(FOCUS_TYPE_RANGE); cmd.params.maybe_param2 = range; - cmd.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd.target_component_id = fixup_component_target(component_id); return cmd; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_photo() +MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_photo(int32_t component_id) { MavlinkCommandSender::CommandLong cmd_stop_photo{}; cmd_stop_photo.command = MAV_CMD_IMAGE_STOP_CAPTURE; - cmd_stop_photo.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd_stop_photo.target_component_id = fixup_component_target(component_id); return cmd_stop_photo; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_start_video(float capture_status_rate_hz) +MavlinkCommandSender::CommandLong +CameraImpl::make_command_start_video(int32_t component_id, float capture_status_rate_hz) { MavlinkCommandSender::CommandLong cmd_start_video{}; cmd_start_video.command = MAV_CMD_VIDEO_START_CAPTURE; cmd_start_video.params.maybe_param1 = 0.f; // Reserved, set to 0 cmd_start_video.params.maybe_param2 = capture_status_rate_hz; - cmd_start_video.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd_start_video.target_component_id = fixup_component_target(component_id); return cmd_start_video; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_video() +MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_video(int32_t component_id) { MavlinkCommandSender::CommandLong cmd_stop_video{}; cmd_stop_video.command = MAV_CMD_VIDEO_STOP_CAPTURE; cmd_stop_video.params.maybe_param1 = 0.f; // Reserved, set to 0 - cmd_stop_video.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd_stop_video.target_component_id = fixup_component_target(component_id); return cmd_stop_video; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_set_camera_mode(float mavlink_mode) +MavlinkCommandSender::CommandLong +CameraImpl::make_command_set_camera_mode(int32_t component_id, float mavlink_mode) { MavlinkCommandSender::CommandLong cmd_set_camera_mode{}; cmd_set_camera_mode.command = MAV_CMD_SET_CAMERA_MODE; cmd_set_camera_mode.params.maybe_param1 = 0.0f; // Reserved, set to 0 cmd_set_camera_mode.params.maybe_param2 = mavlink_mode; - cmd_set_camera_mode.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd_set_camera_mode.target_component_id = fixup_component_target(component_id); return cmd_set_camera_mode; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_start_video_streaming(int32_t stream_id) +MavlinkCommandSender::CommandLong +CameraImpl::make_command_start_video_streaming(int32_t component_id, int32_t stream_id) { MavlinkCommandSender::CommandLong cmd_start_video_streaming{}; cmd_start_video_streaming.command = MAV_CMD_VIDEO_START_STREAMING; cmd_start_video_streaming.params.maybe_param1 = static_cast(stream_id); - cmd_start_video_streaming.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd_start_video_streaming.target_component_id = fixup_component_target(component_id); return cmd_start_video_streaming; } -MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_video_streaming(int32_t stream_id) +MavlinkCommandSender::CommandLong +CameraImpl::make_command_stop_video_streaming(int32_t component_id, int32_t stream_id) { MavlinkCommandSender::CommandLong cmd_stop_video_streaming{}; cmd_stop_video_streaming.command = MAV_CMD_VIDEO_STOP_STREAMING; cmd_stop_video_streaming.params.maybe_param1 = static_cast(stream_id); - cmd_stop_video_streaming.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd_stop_video_streaming.target_component_id = fixup_component_target(component_id); return cmd_stop_video_streaming; } -Camera::Result CameraImpl::take_photo() +Camera::Result CameraImpl::take_photo(int32_t component_id) { - // TODO: check whether we are in photo mode. - - std::lock_guard lock(_capture.mutex); - // Take 1 photo only with no interval - auto cmd_take_photo = make_command_take_photo(0.f, 1.0f); + auto cmd_take_photo = make_command_take_photo(component_id, 0.f, 1.0f); return camera_result_from_command_result(_system_impl->send_command(cmd_take_photo)); } -Camera::Result CameraImpl::zoom_out_start() +Camera::Result CameraImpl::zoom_out_start(int32_t component_id) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_zoom_out(); + auto cmd = make_command_zoom_out(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::zoom_in_start() +Camera::Result CameraImpl::zoom_in_start(int32_t component_id) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_zoom_in(); + auto cmd = make_command_zoom_in(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::zoom_stop() +Camera::Result CameraImpl::zoom_stop(int32_t component_id) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_zoom_stop(); + auto cmd = make_command_zoom_stop(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::zoom_range(float range) +Camera::Result CameraImpl::zoom_range(int32_t component_id, float range) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_zoom_range(range); + auto cmd = make_command_zoom_range(component_id, range); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::track_point(float point_x, float point_y, float radius) +Camera::Result +CameraImpl::track_point(int32_t component_id, float point_x, float point_y, float radius) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_track_point(point_x, point_y, radius); + auto cmd = make_command_track_point(component_id, point_x, point_y, radius); return camera_result_from_command_result(_system_impl->send_command(cmd)); } Camera::Result CameraImpl::track_rectangle( - float top_left_x, float top_left_y, float bottom_right_x, float bottom_right_y) + int32_t component_id, + float top_left_x, + float top_left_y, + float bottom_right_x, + float bottom_right_y) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_track_rectangle(top_left_x, top_left_y, bottom_right_x, bottom_right_y); + auto cmd = make_command_track_rectangle( + component_id, top_left_x, top_left_y, bottom_right_x, bottom_right_y); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::track_stop() +Camera::Result CameraImpl::track_stop(int32_t component_id) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_track_stop(); + auto cmd = make_command_track_stop(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::focus_in_start() +Camera::Result CameraImpl::focus_in_start(int32_t component_id) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_focus_in(); + auto cmd = make_command_focus_in(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::focus_out_start() +Camera::Result CameraImpl::focus_out_start(int32_t component_id) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_focus_out(); + auto cmd = make_command_focus_out(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::focus_stop() +Camera::Result CameraImpl::focus_stop(int32_t component_id) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_focus_stop(); + auto cmd = make_command_focus_stop(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::focus_range(float range) +Camera::Result CameraImpl::focus_range(int32_t component_id, float range) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_focus_range(range); + auto cmd = make_command_focus_range(component_id, range); return camera_result_from_command_result(_system_impl->send_command(cmd)); } -Camera::Result CameraImpl::start_photo_interval(float interval_s) +Camera::Result CameraImpl::start_photo_interval(int32_t component_id, float interval_s) { - if (!interval_valid(interval_s)) { - return Camera::Result::WrongArgument; - } - - // TODO: check whether we are in photo mode. - - std::lock_guard lock(_capture.mutex); - - auto cmd_take_photo_time_lapse = make_command_take_photo(interval_s, 0.f); + auto cmd_take_photo_time_lapse = make_command_take_photo(component_id, interval_s, 0.f); return camera_result_from_command_result(_system_impl->send_command(cmd_take_photo_time_lapse)); } -Camera::Result CameraImpl::stop_photo_interval() +Camera::Result CameraImpl::stop_photo_interval(int32_t component_id) { - auto cmd_stop_photo_interval = make_command_stop_photo(); + auto cmd_stop_photo_interval = make_command_stop_photo(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd_stop_photo_interval)); } -Camera::Result CameraImpl::start_video() +Camera::Result CameraImpl::start_video(int32_t component_id) { - // TODO: check whether video capture is already in progress. - // TODO: check whether we are in video mode. - // Capture status rate is not set - auto cmd_start_video = make_command_start_video(0.f); + auto cmd_start_video = make_command_start_video(component_id, 0.f); return camera_result_from_command_result(_system_impl->send_command(cmd_start_video)); } -Camera::Result CameraImpl::stop_video() +Camera::Result CameraImpl::stop_video(int32_t component_id) { - auto cmd_stop_video = make_command_stop_video(); - - { - std::lock_guard lock(_video_stream_info.mutex); - _video_stream_info.data.status = Camera::VideoStreamInfo::VideoStreamStatus::NotRunning; - } + auto cmd_stop_video = make_command_stop_video(component_id); return camera_result_from_command_result(_system_impl->send_command(cmd_stop_video)); } -void CameraImpl::zoom_in_start_async(const Camera::ResultCallback& callback) +void CameraImpl::zoom_in_start_async(int32_t component_id, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_zoom_in(); + auto cmd = make_command_zoom_in(component_id); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -641,11 +491,9 @@ void CameraImpl::zoom_in_start_async(const Camera::ResultCallback& callback) }); } -void CameraImpl::zoom_out_start_async(const Camera::ResultCallback& callback) +void CameraImpl::zoom_out_start_async(int32_t component_id, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_zoom_out(); + auto cmd = make_command_zoom_out(component_id); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -653,11 +501,9 @@ void CameraImpl::zoom_out_start_async(const Camera::ResultCallback& callback) }); } -void CameraImpl::zoom_stop_async(const Camera::ResultCallback& callback) +void CameraImpl::zoom_stop_async(int32_t component_id, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_zoom_stop(); + auto cmd = make_command_zoom_stop(component_id); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -665,11 +511,10 @@ void CameraImpl::zoom_stop_async(const Camera::ResultCallback& callback) }); } -void CameraImpl::zoom_range_async(float range, const Camera::ResultCallback& callback) +void CameraImpl::zoom_range_async( + int32_t component_id, float range, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_zoom_range(range); + auto cmd = make_command_zoom_range(component_id, range); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -678,11 +523,13 @@ void CameraImpl::zoom_range_async(float range, const Camera::ResultCallback& cal } void CameraImpl::track_point_async( - float point_x, float point_y, float radius, const Camera::ResultCallback& callback) + int32_t component_id, + float point_x, + float point_y, + float radius, + const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_track_point(point_x, point_y, radius); + auto cmd = make_command_track_point(component_id, point_x, point_y, radius); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -691,15 +538,15 @@ void CameraImpl::track_point_async( } void CameraImpl::track_rectangle_async( + int32_t component_id, float top_left_x, float top_left_y, float bottom_right_x, float bottom_right_y, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_track_rectangle(top_left_x, top_left_y, bottom_right_x, bottom_right_y); + auto cmd = make_command_track_rectangle( + component_id, top_left_x, top_left_y, bottom_right_x, bottom_right_y); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -707,11 +554,9 @@ void CameraImpl::track_rectangle_async( }); } -void CameraImpl::track_stop_async(const Camera::ResultCallback& callback) +void CameraImpl::track_stop_async(int32_t component_id, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_track_stop(); + auto cmd = make_command_track_stop(component_id); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -719,11 +564,9 @@ void CameraImpl::track_stop_async(const Camera::ResultCallback& callback) }); } -void CameraImpl::focus_in_start_async(const Camera::ResultCallback& callback) +void CameraImpl::focus_in_start_async(int32_t component_id, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_focus_in(); + auto cmd = make_command_focus_in(component_id); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -731,11 +574,9 @@ void CameraImpl::focus_in_start_async(const Camera::ResultCallback& callback) }); } -void CameraImpl::focus_out_start_async(const Camera::ResultCallback& callback) +void CameraImpl::focus_out_start_async(int32_t component_id, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_focus_out(); + auto cmd = make_command_focus_out(component_id); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -743,11 +584,9 @@ void CameraImpl::focus_out_start_async(const Camera::ResultCallback& callback) }); } -void CameraImpl::focus_stop_async(const Camera::ResultCallback& callback) +void CameraImpl::focus_stop_async(int32_t component_id, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_focus_stop(); + auto cmd = make_command_focus_stop(component_id); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -755,11 +594,10 @@ void CameraImpl::focus_stop_async(const Camera::ResultCallback& callback) }); } -void CameraImpl::focus_range_async(float range, const Camera::ResultCallback& callback) +void CameraImpl::focus_range_async( + int32_t component_id, float range, const Camera::ResultCallback& callback) { - std::lock_guard lock(_capture.mutex); - - auto cmd = make_command_focus_range(range); + auto cmd = make_command_focus_range(component_id, range); _system_impl->send_command_async( cmd, [this, callback](MavlinkCommandSender::Result result, float) { @@ -767,14 +605,10 @@ void CameraImpl::focus_range_async(float range, const Camera::ResultCallback& ca }); } -void CameraImpl::take_photo_async(const Camera::ResultCallback& callback) +void CameraImpl::take_photo_async(int32_t component_id, const Camera::ResultCallback& callback) { - // TODO: check whether we are in photo mode. - - std::lock_guard lock(_capture.mutex); - // Take 1 photo only with no interval - auto cmd_take_photo = make_command_take_photo(0.f, 1.0f); + auto cmd_take_photo = make_command_take_photo(component_id, 0.f, 1.0f); _system_impl->send_command_async( cmd_take_photo, [this, callback](MavlinkCommandSender::Result result, float) { @@ -783,20 +617,9 @@ void CameraImpl::take_photo_async(const Camera::ResultCallback& callback) } void CameraImpl::start_photo_interval_async( - float interval_s, const Camera::ResultCallback& callback) + int32_t component_id, float interval_s, const Camera::ResultCallback& callback) { - if (!interval_valid(interval_s)) { - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::WrongArgument); }); - return; - } - - // TODO: check whether we are in photo mode. - - std::lock_guard lock(_capture.mutex); - - auto cmd_take_photo_time_lapse = make_command_take_photo(interval_s, 0.f); + auto cmd_take_photo_time_lapse = make_command_take_photo(component_id, interval_s, 0.f); _system_impl->send_command_async( cmd_take_photo_time_lapse, [this, callback](MavlinkCommandSender::Result result, float) { @@ -804,9 +627,10 @@ void CameraImpl::start_photo_interval_async( }); } -void CameraImpl::stop_photo_interval_async(const Camera::ResultCallback& callback) +void CameraImpl::stop_photo_interval_async( + int32_t component_id, const Camera::ResultCallback& callback) { - auto cmd_stop_photo_interval = make_command_stop_photo(); + auto cmd_stop_photo_interval = make_command_stop_photo(component_id); _system_impl->send_command_async( cmd_stop_photo_interval, [this, callback](MavlinkCommandSender::Result result, float) { @@ -814,13 +638,10 @@ void CameraImpl::stop_photo_interval_async(const Camera::ResultCallback& callbac }); } -void CameraImpl::start_video_async(const Camera::ResultCallback& callback) +void CameraImpl::start_video_async(int32_t component_id, const Camera::ResultCallback& callback) { - // TODO: check whether video capture is already in progress. - // TODO: check whether we are in video mode. - // Capture status rate is not set - auto cmd_start_video = make_command_start_video(0.f); + auto cmd_start_video = make_command_start_video(component_id, 0.f); _system_impl->send_command_async( cmd_start_video, [this, callback](MavlinkCommandSender::Result result, float) { @@ -828,9 +649,9 @@ void CameraImpl::start_video_async(const Camera::ResultCallback& callback) }); } -void CameraImpl::stop_video_async(const Camera::ResultCallback& callback) +void CameraImpl::stop_video_async(int32_t component_id, const Camera::ResultCallback& callback) { - auto cmd_stop_video = make_command_stop_video(); + auto cmd_stop_video = make_command_stop_video(component_id); _system_impl->send_command_async( cmd_stop_video, [this, callback](MavlinkCommandSender::Result result, float) { @@ -838,115 +659,119 @@ void CameraImpl::stop_video_async(const Camera::ResultCallback& callback) }); } -Camera::Information CameraImpl::information() const +Camera::CameraList CameraImpl::camera_list() { - std::lock_guard lock(_information.mutex); + std::lock_guard lock(_mutex); - return _information.data; + return camera_list_with_lock(); } -Camera::InformationHandle -CameraImpl::subscribe_information(const Camera::InformationCallback& callback) +Camera::CameraList CameraImpl::camera_list_with_lock() { - std::lock_guard lock(_information.mutex); - auto handle = _information.subscription_callbacks.subscribe(callback); + Camera::CameraList result{}; - // If there was already a subscription, cancel the call - if (_status.call_every_cookie) { - _system_impl->remove_call_every(_status.call_every_cookie); + for (auto& potential_camera : potential_cameras_with_lock()) { + if (!potential_camera->maybe_information) { + continue; + } + result.cameras.push_back(potential_camera->maybe_information.value()); } - if (callback) { - _system_impl->remove_call_every(_status.call_every_cookie); - _status.call_every_cookie = - _system_impl->add_call_every([this]() { request_status(); }, 1.0); - } else { - _system_impl->remove_call_every(_status.call_every_cookie); - } + return result; +} + +Camera::CameraListHandle +CameraImpl::subscribe_camera_list(const Camera::CameraListCallback& callback) +{ + std::lock_guard lock(_mutex); + auto handle = _camera_list_subscription_callbacks.subscribe(callback); + + notify_camera_list_with_lock(); return handle; } -void CameraImpl::unsubscribe_information(Camera::InformationHandle handle) +void CameraImpl::unsubscribe_camera_list(Camera::CameraListHandle handle) { - std::lock_guard lock(_information.mutex); - _information.subscription_callbacks.unsubscribe(handle); + std::lock_guard lock(_mutex); + _camera_list_subscription_callbacks.unsubscribe(handle); } -Camera::Result CameraImpl::start_video_streaming(int32_t stream_id) +void CameraImpl::notify_camera_list_with_lock() { - 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; - } + _system_impl->call_user_callback([&]() { + _camera_list_subscription_callbacks.queue( + camera_list_with_lock(), [this](const auto& func) { func(); }); + }); +} - // TODO Check whether we're in video mode - auto command = make_command_start_video_streaming(stream_id); +Camera::Result CameraImpl::start_video_streaming(int32_t component_id, int32_t stream_id) +{ + auto command = make_command_start_video_streaming(component_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 stream_id) +Camera::Result CameraImpl::stop_video_streaming(int32_t component_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(stream_id); + auto command = make_command_stop_video_streaming(component_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() +void CameraImpl::request_slower() { - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, _camera_id + MAV_COMP_ID_CAMERA, nullptr); - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, _camera_id + MAV_COMP_ID_CAMERA, nullptr); + std::lock_guard lock(_mutex); + + for (auto& camera : _potential_cameras) { + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, + fixup_component_target(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, + fixup_component_target(camera.component_id), + nullptr); + + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, + fixup_component_target(camera.component_id), + nullptr); + + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_STORAGE_INFORMATION, + fixup_component_target(camera.component_id), + nullptr); - return _video_stream_info.data; + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_CAMERA_SETTINGS, fixup_component_target(camera.component_id), nullptr); + } } Camera::VideoStreamInfoHandle CameraImpl::subscribe_video_stream_info(const Camera::VideoStreamInfoCallback& callback) { - std::lock_guard lock(_video_stream_info.mutex); + std::lock_guard lock(_mutex); - auto handle = _video_stream_info.subscription_callbacks.subscribe(callback); + auto handle = _video_stream_info_subscription_callbacks.subscribe(callback); - 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(); }, 1.0); - } else { - _system_impl->remove_call_every(_video_stream_info.call_every_cookie); - } + notify_video_stream_info_for_all_with_lock(); 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 @@ -960,7 +785,7 @@ CameraImpl::camera_result_from_command_result(const MavlinkCommandSender::Result case MavlinkCommandSender::Result::ConnectionError: // FALLTHROUGH case MavlinkCommandSender::Result::Busy: - return Camera::Result::Error; + // FALLTHROUGH case MavlinkCommandSender::Result::Failed: return Camera::Result::Error; case MavlinkCommandSender::Result::Denied: @@ -969,6 +794,9 @@ CameraImpl::camera_result_from_command_result(const MavlinkCommandSender::Result return Camera::Result::Denied; case MavlinkCommandSender::Result::Timeout: return Camera::Result::Timeout; + case MavlinkCommandSender::Result::Unsupported: + return Camera::Result::ActionUnsupported; + case MavlinkCommandSender::Result::Cancelled: default: return Camera::Result::Unknown; } @@ -982,18 +810,18 @@ Camera::Result CameraImpl::camera_result_from_parameter_result( return Camera::Result::Success; case MavlinkParameterClient::Result::Timeout: return Camera::Result::Timeout; - case MavlinkParameterClient::Result::ConnectionError: - return Camera::Result::Error; case MavlinkParameterClient::Result::WrongType: - return Camera::Result::WrongArgument; + // FALLTHROUGH case MavlinkParameterClient::Result::ParamNameTooLong: - return Camera::Result::WrongArgument; + // FALLTHROUGH case MavlinkParameterClient::Result::NotFound: - return Camera::Result::WrongArgument; + // FALLTHROUGH case MavlinkParameterClient::Result::ValueUnsupported: return Camera::Result::WrongArgument; + case MavlinkParameterClient::Result::ConnectionError: + // FALLTHROUGH case MavlinkParameterClient::Result::Failed: - return Camera::Result::Error; + // FALLTHROUGH case MavlinkParameterClient::Result::UnknownError: return Camera::Result::Error; default: @@ -1001,76 +829,74 @@ Camera::Result CameraImpl::camera_result_from_parameter_result( } } -Camera::Result CameraImpl::set_mode(const Camera::Mode mode) +Camera::Result CameraImpl::set_mode(int32_t component_id, const Camera::Mode mode) { const float mavlink_mode = to_mavlink_camera_mode(mode); - auto cmd_set_camera_mode = make_command_set_camera_mode(mavlink_mode); + auto cmd_set_camera_mode = make_command_set_camera_mode(component_id, mavlink_mode); const auto command_result = _system_impl->send_command(cmd_set_camera_mode); const auto camera_result = camera_result_from_command_result(command_result); if (camera_result == Camera::Result::Success) { - { - std::lock_guard lock(_mode.mutex); - _mode.data = mode; - } - notify_mode(); - if (_camera_definition != nullptr) { - save_camera_mode(mavlink_mode); + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera != nullptr) { + save_camera_mode_with_lock(*maybe_potential_camera, mode); } } return camera_result; } -void CameraImpl::save_camera_mode(const float mavlink_camera_mode) +void CameraImpl::save_camera_mode_with_lock(PotentialCamera& potential_camera, Camera::Mode mode) { - if (!std::isfinite(mavlink_camera_mode)) { - LogWarn() << "Can't save NAN as camera mode"; + potential_camera.mode = mode; + + // If there is a camera definition which is the case if we are in this + // function, and if CAM_MODE is defined there, then we reuse that type. + // Otherwise, we hardcode it to `uint32_t`. + + // Note that it could be that the camera definition defines options + // different from {PHOTO, VIDEO}, in which case the mode received from + // CAMERA_SETTINGS will be wrong. + + if (!potential_camera.camera_definition) { return; } - // If there is a camera definition (which is the case if we are - // in this function, and if CAM_MODE is defined there, then - // we reuse that type. Otherwise, we hardcode it to `uint32_t`. - // Note that it could be that the camera definition defines options - // different than {PHOTO, VIDEO}, in which case the mode received - // from CAMERA_SETTINGS will be wrong. Not sure if it means that - // it should be ignored in that case, but that may be tricky to - // maintain (what if the MAVLink CAMERA_MODE enum evolves?), so - // I am assuming here that in such a case, CAMERA_SETTINGS is - // never sent by the camera. ParamValue value; - if (_camera_definition->get_setting("CAM_MODE", value)) { + if (potential_camera.camera_definition->get_setting("CAM_MODE", value)) { if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } else if (value.is()) { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } } else { - value.set(static_cast(mavlink_camera_mode)); + value.set(static_cast(mode)); } - _camera_definition->set_setting("CAM_MODE", value); - refresh_params(); + potential_camera.camera_definition->set_setting("CAM_MODE", value); + refresh_params_with_lock(potential_camera, false); + notify_mode_with_lock(potential_camera); } -float CameraImpl::to_mavlink_camera_mode(const Camera::Mode mode) const +float CameraImpl::to_mavlink_camera_mode(const Camera::Mode mode) { switch (mode) { case Camera::Mode::Photo: @@ -1083,100 +909,89 @@ float CameraImpl::to_mavlink_camera_mode(const Camera::Mode mode) const } } -void CameraImpl::set_mode_async(const Camera::Mode mode, const Camera::ResultCallback& callback) +void CameraImpl::set_mode_async( + int32_t component_id, const Camera::Mode mode, const Camera::ResultCallback& callback) { const auto mavlink_mode = to_mavlink_camera_mode(mode); - auto cmd_set_camera_mode = make_command_set_camera_mode(mavlink_mode); + auto cmd_set_camera_mode = make_command_set_camera_mode(component_id, mavlink_mode); _system_impl->send_command_async( cmd_set_camera_mode, - [this, callback, mode](MavlinkCommandSender::Result result, float progress) { + [this, callback, mode, component_id](MavlinkCommandSender::Result result, float progress) { UNUSED(progress); - receive_set_mode_command_result(result, callback, mode); + receive_set_mode_command_result(result, callback, mode, component_id); }); } -Camera::Mode CameraImpl::mode() -{ - std::lock_guard lock(_mode.mutex); - return _mode.data; -} - Camera::ModeHandle CameraImpl::subscribe_mode(const Camera::ModeCallback& callback) { - std::unique_lock lock(_mode.mutex); - auto handle = _mode.subscription_callbacks.subscribe(callback); - lock.unlock(); - - notify_mode(); + std::lock_guard lock(_mutex); + auto handle = _mode_subscription_callbacks.subscribe(callback); - if (callback) { - _system_impl->remove_call_every(_mode.call_every_cookie); - _mode.call_every_cookie = - _system_impl->add_call_every([this]() { request_camera_settings(); }, 5.0); - } else { - _system_impl->remove_call_every(_mode.call_every_cookie); - } + notify_mode_for_all_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); } -bool CameraImpl::interval_valid(float interval_s) +Camera::StorageHandle CameraImpl::subscribe_storage(const Camera::StorageCallback& callback) { - // Reject everything faster than 1000 Hz, as well as negative inputs. - if (interval_s < 0.001f) { - LogWarn() << "Invalid interval input"; - return false; - } else { - return true; - } -} + std::lock_guard lock(_mutex); + auto handle = _storage_subscription_callbacks.subscribe(callback); -void CameraImpl::request_status() -{ - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, _camera_id + MAV_COMP_ID_CAMERA, nullptr); - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_STORAGE_INFORMATION, _camera_id + MAV_COMP_ID_CAMERA, nullptr); + notify_storage_for_all_with_lock(); + + return handle; } -Camera::StatusHandle CameraImpl::subscribe_status(const Camera::StatusCallback& callback) +void CameraImpl::unsubscribe_storage(Camera::StorageHandle handle) { - std::lock_guard lock(_status.mutex); - auto handle = _status.subscription_callbacks.subscribe(callback); - - return handle; + std::lock_guard lock(_mutex); + _storage_subscription_callbacks.unsubscribe(handle); } -void CameraImpl::unsubscribe_status(Camera::StatusHandle handle) +Camera::CaptureInfoHandle +CameraImpl::subscribe_capture_info(const Camera::CaptureInfoCallback& callback) { - std::lock_guard lock(_status.mutex); - _status.subscription_callbacks.unsubscribe(handle); + std::lock_guard lock(_mutex); + return _capture_info_callbacks.subscribe(callback); } -Camera::Status CameraImpl::status() +void CameraImpl::unsubscribe_capture_info(Camera::CaptureInfoHandle handle) { - std::lock_guard lock(_status.mutex); - return _status.data; + std::lock_guard lock(_mutex); + _capture_info_callbacks.unsubscribe(handle); } -Camera::CaptureInfoHandle -CameraImpl::subscribe_capture_info(const Camera::CaptureInfoCallback& callback) +void CameraImpl::process_heartbeat(const mavlink_message_t& message) { - std::lock_guard lock(_capture_info.mutex); - return _capture_info.callbacks.subscribe(callback); + // Check for potential camera + std::lock_guard lock(_mutex); + auto found = + std::any_of(_potential_cameras.begin(), _potential_cameras.end(), [&](const auto& item) { + return item.component_id == message.compid; + }); + + if (!found) { + _potential_cameras.emplace_back(message.compid); + check_potential_cameras_with_lock(); + } } -void CameraImpl::unsubscribe_capture_info(Camera::CaptureInfoHandle handle) +void CameraImpl::check_potential_cameras_with_lock() { - std::lock_guard lock(_capture_info.mutex); - _capture_info.callbacks.unsubscribe(handle); + for (auto& potential_camera : _potential_cameras) { + // First step, get information if we don't already have it. + if (!potential_camera.maybe_information) { + request_camera_information(potential_camera.component_id); + potential_camera.information_requested = true; + } + } } void CameraImpl::process_camera_capture_status(const mavlink_message_t& message) @@ -1184,29 +999,34 @@ 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_component_id_with_lock( + message.compid, camera_capture_status.camera_device_id); + + if (maybe_potential_camera == nullptr) { + return; } - { - std::lock_guard lock(_status.mutex); + auto& camera = *maybe_potential_camera; + + // If image_count got smaller, consider that the storage was formatted. + if (camera_capture_status.image_count < camera.capture_status.image_count) { + LogInfo() << "Seems like storage was formatted, setting state accordingly"; + reset_following_format_storage_with_lock(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 = float(camera_capture_status.recording_time_ms) / 1e3f; + camera.storage.video_on = (camera_capture_status.video_status == 1); + camera.storage.photo_interval_on = + (camera_capture_status.image_status == 2 || camera_capture_status.image_status == 3); + camera.capture_status.received_camera_capture_status = true; + camera.storage.recording_time_s = + static_cast(camera_capture_status.recording_time_ms) / 1e3f; - _status.image_count = camera_capture_status.image_count; + camera.capture_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; - } + if (camera.capture_status.image_count_at_connection == -1) { + camera.capture_status.image_count_at_connection = camera_capture_status.image_count; } - - check_status(); } void CameraImpl::process_storage_information(const mavlink_message_t& message) @@ -1214,69 +1034,64 @@ void CameraImpl::process_storage_information(const mavlink_message_t& message) mavlink_storage_information_t storage_information; mavlink_msg_storage_information_decode(&message, &storage_information); - if (storage_information.total_capacity == 0.0f) { - // Some MAVLink systems happen to send the STORAGE_INFORMATION message - // to indicate that the camera has a slot for a storage even if there - // is no way to know anything about that storage (e.g. whether or not - // there is an sdcard in the slot). - // - // We consider that a total capacity of 0 means that this is such a - // message, and we don't expect MAVSDK users to leverage it, which is - // why it is ignored. + std::lock_guard lock(_mutex); + // TODO: should STORAGE_INFORMATION have a camera_device_id? + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(message.compid, 0); + + if (maybe_potential_camera == nullptr) { 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; - } + auto& camera = *maybe_potential_camera; + + camera.storage.storage_status = storage_status_from_mavlink(storage_information.status); + camera.storage.available_storage_mib = storage_information.available_capacity; + camera.storage.used_storage_mib = storage_information.used_capacity; + camera.storage.total_storage_mib = storage_information.total_capacity; + camera.storage.storage_id = storage_information.storage_id; + camera.storage.storage_type = storage_type_from_mavlink(storage_information.type); + camera.received_storage = true; - check_status(); + notify_storage_with_lock(camera); } -Camera::Status::StorageStatus -CameraImpl::storage_status_from_mavlink(const int storage_status) const +Camera::Storage::StorageStatus CameraImpl::storage_status_from_mavlink(const int storage_status) { switch (storage_status) { case STORAGE_STATUS_EMPTY: - return Camera::Status::StorageStatus::NotAvailable; + return Camera::Storage::StorageStatus::NotAvailable; case STORAGE_STATUS_UNFORMATTED: - return Camera::Status::StorageStatus::Unformatted; + return Camera::Storage::StorageStatus::Unformatted; case STORAGE_STATUS_READY: - return Camera::Status::StorageStatus::Formatted; + return Camera::Storage::StorageStatus::Formatted; break; case STORAGE_STATUS_NOT_SUPPORTED: - return Camera::Status::StorageStatus::NotSupported; + return Camera::Storage::StorageStatus::NotSupported; default: LogErr() << "Unknown storage status received."; - return Camera::Status::StorageStatus::NotSupported; + return Camera::Storage::StorageStatus::NotSupported; } } -Camera::Status::StorageType CameraImpl::storage_type_from_mavlink(const int storage_type) const +Camera::Storage::StorageType CameraImpl::storage_type_from_mavlink(const int storage_type) { switch (storage_type) { default: LogErr() << "Unknown storage_type enum value: " << storage_type; // FALLTHROUGH case STORAGE_TYPE_UNKNOWN: - return mavsdk::Camera::Status::StorageType::Unknown; + return mavsdk::Camera::Storage::StorageType::Unknown; case STORAGE_TYPE_USB_STICK: - return mavsdk::Camera::Status::StorageType::UsbStick; + return mavsdk::Camera::Storage::StorageType::UsbStick; case STORAGE_TYPE_SD: - return mavsdk::Camera::Status::StorageType::Sd; + return mavsdk::Camera::Storage::StorageType::Sd; case STORAGE_TYPE_MICROSD: - return mavsdk::Camera::Status::StorageType::Microsd; + return mavsdk::Camera::Storage::StorageType::Microsd; case STORAGE_TYPE_HD: - return mavsdk::Camera::Status::StorageType::Hd; + return mavsdk::Camera::Storage::StorageType::Hd; case STORAGE_TYPE_OTHER: - return mavsdk::Camera::Status::StorageType::Other; + return mavsdk::Camera::Storage::StorageType::Other; } } @@ -1285,97 +1100,105 @@ 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); - { - Camera::CaptureInfo capture_info = {}; - capture_info.position.latitude_deg = image_captured.lat / 1e7; - capture_info.position.longitude_deg = image_captured.lon / 1e7; - capture_info.position.absolute_altitude_m = image_captured.alt / 1e3f; - capture_info.position.relative_altitude_m = image_captured.relative_alt / 1e3f; - capture_info.time_utc_us = image_captured.time_utc; - capture_info.attitude_quaternion.w = image_captured.q[0]; - capture_info.attitude_quaternion.x = image_captured.q[1]; - capture_info.attitude_quaternion.y = image_captured.q[2]; - capture_info.attitude_quaternion.z = image_captured.q[3]; - capture_info.attitude_euler_angle = - to_euler_angle_from_quaternion(capture_info.attitude_quaternion); - capture_info.file_url = std::string(image_captured.file_url); - capture_info.is_success = (image_captured.capture_result == 1); - capture_info.index = image_captured.image_index; - - _status.photo_list.insert(std::make_pair(image_captured.image_index, capture_info)); - - _captured_request_cv.notify_all(); - - 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( - capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); }); - - if (_capture_info.last_advertised_image_index != -1) { - // Save captured indices that have been dropped to request later, however, don't - // do it from the very beginning as there might be many photos from a previous - // time that we don't want to request. - for (int i = _capture_info.last_advertised_image_index + 1; i < capture_info.index; - ++i) { - if (_capture_info.missing_image_retries.find(i) == - _capture_info.missing_image_retries.end()) { - _capture_info.missing_image_retries[i] = 0; - } - } - } + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(message.compid, image_captured.camera_id); - _capture_info.last_advertised_image_index = capture_info.index; - } - - else if (auto it = _capture_info.missing_image_retries.find(capture_info.index); - it != _capture_info.missing_image_retries.end()) { - _capture_info.callbacks.queue( - capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); }); - _capture_info.missing_image_retries.erase(it); - } + if (maybe_potential_camera == nullptr) { + return; } -} - -void CameraImpl::request_missing_capture_info() -{ - std::lock_guard lock(_capture_info.mutex); - for (auto it = _capture_info.missing_image_retries.begin(); - it != _capture_info.missing_image_retries.end(); - /* ++it */) { - if (it->second > 3) { - it = _capture_info.missing_image_retries.erase(it); - } else { - ++it; + auto& camera = *maybe_potential_camera; + + mavsdk::Quaternion quaternion{}; + quaternion.w = image_captured.q[0]; + quaternion.x = image_captured.q[1]; + quaternion.y = image_captured.q[2]; + quaternion.z = image_captured.q[3]; + auto euler = to_euler_angle_from_quaternion(quaternion); + + Camera::CaptureInfo capture_info = {}; + capture_info.component_id = camera.component_id; + capture_info.position.latitude_deg = image_captured.lat / 1e7; + capture_info.position.longitude_deg = image_captured.lon / 1e7; + capture_info.position.absolute_altitude_m = static_cast(image_captured.alt) / 1e3f; + capture_info.position.relative_altitude_m = + static_cast(image_captured.relative_alt) / 1e3f; + capture_info.time_utc_us = image_captured.time_utc; + capture_info.attitude_quaternion.w = image_captured.q[0]; + capture_info.attitude_quaternion.x = image_captured.q[1]; + capture_info.attitude_quaternion.y = image_captured.q[2]; + capture_info.attitude_quaternion.z = image_captured.q[3]; + capture_info.attitude_euler_angle.roll_deg = euler.roll_deg; + capture_info.attitude_euler_angle.pitch_deg = euler.pitch_deg; + capture_info.attitude_euler_angle.yaw_deg = euler.yaw_deg; + capture_info.file_url = std::string(image_captured.file_url); + capture_info.is_success = (image_captured.capture_result == 1); + capture_info.index = image_captured.image_index; + + camera.capture_status.photo_list.insert( + std::make_pair(image_captured.image_index, capture_info)); + + // Notify user if a new image has been captured. + if (camera.capture_info.last_advertised_image_index < capture_info.index) { + _capture_info_callbacks.queue( + capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); }); + + if (camera.capture_info.last_advertised_image_index != -1) { + // Save captured indices that have been dropped to request later, however, don't + // do it from the very beginning as there might be many photos from a previous + // time that we don't want to request. + for (int i = camera.capture_info.last_advertised_image_index + 1; + i < capture_info.index; + ++i) { + if (camera.capture_info.missing_image_retries.find(i) == + camera.capture_info.missing_image_retries.end()) { + camera.capture_info.missing_image_retries[i] = 0; + } + } } + + camera.capture_info.last_advertised_image_index = capture_info.index; } - if (!_capture_info.missing_image_retries.empty()) { - auto it_lowest_retries = std::min_element( - _capture_info.missing_image_retries.begin(), _capture_info.missing_image_retries.end()); - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, - _camera_id + MAV_COMP_ID_CAMERA, - nullptr, - it_lowest_retries->first); - it_lowest_retries->second += 1; + else if (auto it = camera.capture_info.missing_image_retries.find(capture_info.index); + it != camera.capture_info.missing_image_retries.end()) { + _capture_info_callbacks.queue( + capture_info, [this](const auto& func) { _system_impl->call_user_callback(func); }); + camera.capture_info.missing_image_retries.erase(it); } } -Camera::EulerAngle CameraImpl::to_euler_angle_from_quaternion(Camera::Quaternion quaternion) +void CameraImpl::request_missing_capture_info() { - auto& q = quaternion; - - // FIXME: This is duplicated from telemetry/math_conversions.cpp. - Camera::EulerAngle euler_angle; - euler_angle.roll_deg = to_deg_from_rad( - atan2f(2.0f * (q.w * q.x + q.y * q.z), 1.0f - 2.0f * (q.x * q.x + q.y * q.y))); + std::lock_guard lock(_mutex); + for (auto& potential_camera : _potential_cameras) { + // Clean out entries once we have tried 3 times. + for (auto it = potential_camera.capture_info.missing_image_retries.begin(); + it != potential_camera.capture_info.missing_image_retries.end(); + /* ++it */) { + if (it->second > 3) { + it = potential_camera.capture_info.missing_image_retries.erase(it); + } else { + ++it; + } + } - euler_angle.pitch_deg = to_deg_from_rad(asinf(2.0f * (q.w * q.y - q.z * q.x))); - euler_angle.yaw_deg = to_deg_from_rad( - atan2f(2.0f * (q.w * q.z + q.x * q.y), 1.0f - 2.0f * (q.y * q.y + q.z * q.z))); - return euler_angle; + // Request a few entries, 3 each time. + for (unsigned i = 0; i < 3; ++i) { + if (!potential_camera.capture_info.missing_image_retries.empty()) { + auto it_lowest_retries = std::min_element( + potential_camera.capture_info.missing_image_retries.begin(), + potential_camera.capture_info.missing_image_retries.end()); + _system_impl->mavlink_request_message().request( + MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, + potential_camera.component_id, + nullptr, + it_lowest_retries->first); + it_lowest_retries->second += 1; + } + } + } } void CameraImpl::process_camera_settings(const mavlink_message_t& message) @@ -1383,19 +1206,18 @@ void CameraImpl::process_camera_settings(const mavlink_message_t& message) mavlink_camera_settings_t camera_settings; mavlink_msg_camera_settings_decode(&message, &camera_settings); - { - std::lock_guard lock(_mode.mutex); - _mode.data = to_camera_mode(camera_settings.mode_id); - } - notify_mode(); + std::lock_guard lock(_mutex); + auto maybe_potential_camera = maybe_potential_camera_for_component_id_with_lock( + message.compid, camera_settings.camera_device_id); - if (_camera_definition) { - // This "parameter" needs to be manually set. - save_camera_mode(camera_settings.mode_id); + if (maybe_potential_camera == nullptr) { + return; } + + save_camera_mode_with_lock(*maybe_potential_camera, to_camera_mode(camera_settings.mode_id)); } -Camera::Mode CameraImpl::to_camera_mode(const uint8_t mavlink_camera_mode) const +Camera::Mode CameraImpl::to_camera_mode(const uint8_t mavlink_camera_mode) { switch (mavlink_camera_mode) { case CAMERA_MODE_IMAGE: @@ -1417,274 +1239,322 @@ void CameraImpl::process_camera_information(const mavlink_message_t& message) camera_information.model_name[sizeof(camera_information.model_name) - 1] = '\0'; camera_information.cam_definition_uri[sizeof(camera_information.cam_definition_uri) - 1] = '\0'; - std::lock_guard lock(_information.mutex); - - _information.data.vendor_name = (char*)(camera_information.vendor_name); - _information.data.model_name = (char*)(camera_information.model_name); - _information.data.focal_length_mm = camera_information.focal_length; - _information.data.horizontal_sensor_size_mm = camera_information.sensor_size_h; - _information.data.vertical_sensor_size_mm = camera_information.sensor_size_v; - _information.data.horizontal_resolution_px = camera_information.resolution_h; - _information.data.vertical_resolution_px = camera_information.resolution_v; - - _information.subscription_callbacks.queue( - _information.data, [this](const auto& func) { _system_impl->call_user_callback(func); }); + Camera::Information new_information{}; + // TODO: Check the case for 1-6. + new_information.component_id = message.compid; + new_information.vendor_name = reinterpret_cast(camera_information.vendor_name); + new_information.model_name = reinterpret_cast(camera_information.model_name); + new_information.focal_length_mm = camera_information.focal_length; + new_information.horizontal_sensor_size_mm = camera_information.sensor_size_h; + new_information.vertical_sensor_size_mm = camera_information.sensor_size_v; + new_information.horizontal_resolution_px = camera_information.resolution_h; + new_information.vertical_resolution_px = camera_information.resolution_v; + + std::lock_guard lock(_mutex); + + auto potential_camera = + std::find_if(_potential_cameras.begin(), _potential_cameras.end(), [&](auto& item) { + return item.component_id == message.compid; + }); - if (should_fetch_camera_definition(camera_information.cam_definition_uri)) { - _is_fetching_camera_definition = true; + if (potential_camera == _potential_cameras.end()) { + _potential_cameras.emplace_back(message.compid); + potential_camera = std::prev(_potential_cameras.end()); + } - std::thread([this, camera_information]() { - std::string content{}; - const auto result = fetch_camera_definition(camera_information, content); + // We need a copy of the component ID inside the information. + potential_camera->component_id = new_information.component_id; + potential_camera->maybe_information = new_information; + potential_camera->camera_definition_url = camera_information.cam_definition_uri; + potential_camera->camera_definition_version = camera_information.cam_definition_version; + check_camera_definition_with_lock(*potential_camera); +} - if (result == Camera::Result::Success) { - LogDebug() << "Successfully loaded camera definition"; +void CameraImpl::check_camera_definition_with_lock(PotentialCamera& potential_camera) +{ + const std::string url = potential_camera.camera_definition_url; - if (_camera_definition_callback) { - _system_impl->call_user_callback( - [this, result]() { _camera_definition_callback(result); }); - } + if (potential_camera.camera_definition_url.empty()) { + potential_camera.camera_definition_result = Camera::Result::Unavailable; + notify_camera_list_with_lock(); + } - _camera_definition.reset(new CameraDefinition()); - _camera_definition->load_string(content); - refresh_params(); + const auto& info = potential_camera.maybe_information.value(); + auto file_cache_tag = replace_non_ascii_and_whitespace( + std::string("camera_definition-") + info.model_name + "_" + info.vendor_name + "-" + + std::to_string(potential_camera.camera_definition_version) + ".xml"); - } else if (result == Camera::Result::ProtocolUnsupported) { - LogWarn() << "Protocol for " << camera_information.cam_definition_uri - << " not supported"; - if (_camera_definition_callback) { - _system_impl->call_user_callback( - [this, result]() { _camera_definition_callback(result); }); - } + std::optional cached_file_option{}; + if (_file_cache) { + cached_file_option = _file_cache->access(file_cache_tag); + } - } else { - LogDebug() << "Failed to fetch camera definition!"; + if (cached_file_option) { + 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_with_lock(); - if (++_camera_definition_fetch_count >= 3) { - LogWarn() << "Giving up fetching the camera definition"; + } else { + potential_camera.is_fetching_camera_definition = true; - std::lock_guard thread_lock(_information.mutex); - _has_camera_definition_timed_out = true; + if (url.empty()) { + LogInfo() << "No camera definition URL available"; + potential_camera.camera_definition_result = Camera::Result::ProtocolUnsupported; + notify_camera_list_with_lock(); - if (_camera_definition_callback) { - _system_impl->call_user_callback( - [this, result]() { _camera_definition_callback(result); }); - } - } + } else if (starts_with(url, "http://") || starts_with(url, "https://")) { +#if BUILD_WITHOUT_CURL == 1 + potential_camera.camera_definition_result = Camera::Result::ProtocolUnsupported; + notify_camera_list(); +#else + if (_http_loader == nullptr) { + _http_loader = std::make_unique(); } + LogInfo() << "Downloading camera definition from: " << url; + + auto component_id = potential_camera.component_id; + + auto download_path = _tmp_download_path / file_cache_tag; + + _http_loader->download_async( + url, + download_path.string(), + [download_path, file_cache_tag, component_id, this]( + int progress, HttpStatus status, CURLcode curl_code) mutable { + // TODO: check if we still exist + LogDebug() << "Download progress: " << progress + << ", status: " << static_cast(status) + << ", curl_code: " << std::to_string(curl_code); + + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + LogErr() << "Failed to find camera."; + } - std::lock_guard thread_lock(_information.mutex); - _is_fetching_camera_definition = false; - }).detach(); - } -} + if (status == HttpStatus::Error) { + LogErr() << "File download failed with result " + << 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_with_lock(); + + } else if (status == HttpStatus::Finished) { + LogDebug() << "File download finished " << download_path; + if (_file_cache) { + // Cache the file (this will move/remove the temp file as well) + download_path = _file_cache->insert(file_cache_tag, download_path) + .value_or(download_path); + LogDebug() << "Cached path: " << download_path; + } + 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_with_lock(); + } + }); +#endif + } else if (starts_with(url, "mftp://") || starts_with(url, "mavlinkftp://")) { + LogInfo() << "Download file: " << url << " using MAVLink FTP..."; + + auto component_id = potential_camera.component_id; + + auto downloaded_filename = strip_prefix(strip_prefix(url, "mavlinkftp://"), "mftp://"); + + _system_impl->mavlink_ftp_client().download_async( + downloaded_filename, + _tmp_download_path.string(), + false, + [file_cache_tag, downloaded_filename, component_id, this]( + MavlinkFtpClient::ClientResult client_result, + MavlinkFtpClient::ProgressData progress_data) mutable { + // TODO: check if we still exist + if (client_result == MavlinkFtpClient::ClientResult::Next) { + LogDebug() + << "Mavlink FTP download progress: " + << 100 * progress_data.bytes_transferred / progress_data.total_bytes + << " %"; + return; + } -bool CameraImpl::should_fetch_camera_definition(const std::string& uri) const -{ - return !uri.empty() && !_camera_definition && !_is_fetching_camera_definition && - !_has_camera_definition_timed_out; -} + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + LogErr() << "Failed to find camera with ID " << component_id; + return; + } -bool CameraImpl::starts_with(const std::string& str, const std::string& prefix) -{ - return str.compare(0, prefix.size(), prefix) == 0; -} + if (client_result != MavlinkFtpClient::ClientResult::Success) { + 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_with_lock(); + return; + } -Camera::Result CameraImpl::fetch_camera_definition( - const mavlink_camera_information_t& camera_information, std::string& camera_definition_out) -{ - auto& uri = camera_information.cam_definition_uri; + auto downloaded_filepath = _tmp_download_path / downloaded_filename; - if (starts_with(uri, "http://") || starts_with(uri, "https://")) { - LogInfo() << "Download file: " << uri << " using cURL..."; - auto result = download_definition_file_curl(uri, camera_definition_out); - LogInfo() << "Downloaded file, result " << result; - return result; - } else if (starts_with(uri, "mftp://") || starts_with(uri, "mavlinkftp://")) { - LogInfo() << "Download file: " << uri << " using MAVLink FTP..."; - auto result = download_definition_file_mftp(uri, camera_definition_out); - LogInfo() << "Downloaded file, result " << result; - return result; - } else { - LogErr() << "Unknown protocol for URL: " << uri; - return Camera::Result::ProtocolUnsupported; + LogDebug() << "File download finished to " << downloaded_filepath; + if (_file_cache) { + // Cache the file (this will move/remove the temp file as well) + downloaded_filepath = + _file_cache->insert(file_cache_tag, downloaded_filepath) + .value_or(downloaded_filepath); + LogDebug() << "Cached path: " << downloaded_filepath; + } + 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_with_lock(); + }); + } else { + LogErr() << "Unknown protocol for URL: " << url; + potential_camera.camera_definition_result = Camera::Result::ProtocolUnsupported; + notify_camera_list_with_lock(); + } } } -Camera::Result CameraImpl::download_definition_file_curl( - const std::string& uri, std::string& camera_definition_out) +void CameraImpl::load_camera_definition_with_lock( + PotentialCamera& potential_camera, const std::filesystem::path& path) { -#if BUILD_WITHOUT_CURL == 1 - UNUSED(uri); - UNUSED(camera_definition_out); - return Camera::Result::ProtocolUnsupported; -#else - HttpLoader http_loader; - LogInfo() << "Downloading camera definition from: " << uri; - if (!http_loader.download_text_sync(uri, camera_definition_out)) { - LogErr() << "Failed to download camera definition."; - return Camera::Result::Error; + if (potential_camera.camera_definition == nullptr) { + potential_camera.camera_definition = std::make_unique(); } -#endif - - return Camera::Result::Success; -} -std::string CameraImpl::get_filename_from_path(const std::string& path) -{ - size_t pos = path.find_last_of("/"); - if (pos != std::string::npos) { - return path.substr(pos + 1); + if (!potential_camera.camera_definition->load_file(path.string())) { + LogErr() << "Failed to load camera definition: " << path; + // We can't keep something around that's not loaded correctly. + potential_camera.camera_definition = nullptr; + return; } - return path; // If no directory separator is found, return the whole path as the filename -} -std::string CameraImpl::strip_mavlinkftp_prefix(const std::string& str) -{ - const std::string prefix1 = "mftp://"; - const std::string prefix2 = "mavlinkftp://"; + // We assume default settings initially and then load the params one by one. + // This way we can start using it straightaway. + potential_camera.camera_definition->reset_to_default_settings(true); - if (str.compare(0, prefix1.size(), prefix1) == 0) { - return str.substr(prefix1.size()); - } - if (str.compare(0, prefix2.size(), prefix2) == 0) { - return str.substr(prefix2.size()); - } - return str; // If no known prefix is found, return the original string + refresh_params_with_lock(potential_camera, true); } -Camera::Result CameraImpl::download_definition_file_mftp( - const std::string& uri, std::string& camera_definition_out) +void CameraImpl::process_video_information(const mavlink_message_t& message) { - std::string tmp_download_path; - const auto tmp_option = create_tmp_directory("mavsdk-camera-definition-download"); - if (tmp_option) { - tmp_download_path = tmp_option.value().string(); - } else { - tmp_download_path = "./mavsdk-camera-definition-download"; - std::error_code err; - std::filesystem::create_directory(tmp_download_path, err); - } - - auto prom = std::promise(); - auto fut = prom.get_future(); - - _system_impl->mavlink_ftp_client().download_async( - strip_mavlinkftp_prefix(uri), - tmp_download_path, - false, - [&prom](MavlinkFtpClient::ClientResult client_result, MavlinkFtpClient::ProgressData) { - if (client_result != MavlinkFtpClient::ClientResult::Next) { - prom.set_value(client_result); - } - }, - static_cast(_camera_id + MAV_COMP_ID_CAMERA)); + mavlink_video_stream_information_t received_video_info; + mavlink_msg_video_stream_information_decode(&message, &received_video_info); - auto result = fut.get(); + std::lock_guard lock(_mutex); + auto maybe_potential_camera = maybe_potential_camera_for_component_id_with_lock( + message.compid, received_video_info.camera_device_id); - if (result != MavlinkFtpClient::ClientResult::Success) { - LogErr() << "MAVLink FTP download failed: " << result; - return Camera::Result::Error; + if (maybe_potential_camera == nullptr) { + return; } - std::filesystem::path tmp_file = std::filesystem::path(tmp_download_path) / - std::filesystem::path(get_filename_from_path(uri)); + auto& camera = *maybe_potential_camera; - std::ifstream file_stream(tmp_file.string()); - if (!file_stream.is_open()) { - LogErr() << "Could not open: " << tmp_file.string(); - return Camera::Result::Error; - } - std::stringstream buffer; - buffer << file_stream.rdbuf(); - camera_definition_out = buffer.str(); - return Camera::Result::Success; -} + // 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); -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); + 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; + camera.video_stream_info.stream_id = received_video_info.stream_id; + camera.received_video_stream_info = true; - { - 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(); + notify_video_stream_info_with_lock(camera); } 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_component_id_with_lock( + message.compid, received_video_stream_status.camera_device_id); + + 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); + + // We only set this when we get the static information rather than just the status. + // camera.received_video_stream_info = true; + + notify_video_stream_info_with_lock(camera); } -void CameraImpl::notify_video_stream_info() +void CameraImpl::notify_video_stream_info_for_all_with_lock() { - std::lock_guard lock(_video_stream_info.mutex); + for (auto& camera : _potential_cameras) { + notify_video_stream_info_with_lock(camera); + } +} + +void CameraImpl::notify_video_stream_info_with_lock(PotentialCamera& camera) +{ + if (!camera.received_video_stream_info) { + return; + } - _video_stream_info.subscription_callbacks.queue( - _video_stream_info.data, + _video_stream_info_subscription_callbacks.queue( + Camera::VideoStreamUpdate{camera.component_id, camera.video_stream_info}, [this](const auto& func) { _system_impl->call_user_callback(func); }); } -void CameraImpl::check_status() +void CameraImpl::notify_storage_for_all_with_lock() { - std::lock_guard lock(_status.mutex); - - if (_status.received_camera_capture_status && _status.received_storage_information) { - _status.subscription_callbacks.queue( - _status.data, [this](const auto& func) { _system_impl->call_user_callback(func); }); + for (auto& potential_camera : potential_cameras_with_lock()) { + notify_storage_with_lock(*potential_camera); + } +} - _status.received_camera_capture_status = false; - _status.received_storage_information = false; +void CameraImpl::notify_storage_with_lock(PotentialCamera& camera) +{ + if (!camera.received_storage) { + return; } + + _storage_subscription_callbacks.queue( + Camera::StorageUpdate{camera.component_id, camera.storage}, + [this](const auto& func) { _system_impl->call_user_callback(func); }); } void CameraImpl::receive_command_result( - MavlinkCommandSender::Result command_result, const Camera::ResultCallback& callback) + MavlinkCommandSender::Result command_result, const Camera::ResultCallback& callback) const { Camera::Result camera_result = camera_result_from_command_result(command_result); @@ -1695,128 +1565,109 @@ void CameraImpl::receive_command_result( void CameraImpl::receive_set_mode_command_result( const MavlinkCommandSender::Result command_result, - const Camera::ResultCallback callback, - const Camera::Mode mode) + const Camera::ResultCallback& callback, + const Camera::Mode mode, + int32_t component_id) { Camera::Result camera_result = camera_result_from_command_result(command_result); if (callback) { - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback, camera_result]() { temp_callback(camera_result); }); + _system_impl->call_user_callback([callback, camera_result]() { callback(camera_result); }); } - if (command_result == MavlinkCommandSender::Result::Success && _camera_definition) { - // This "parameter" needs to be manually set. - { - std::lock_guard lock(_mode.mutex); - _mode.data = mode; - } - - const auto mavlink_mode = to_mavlink_camera_mode(mode); - - if (std::isnan(mavlink_mode)) { - LogWarn() << "Unknown camera mode"; - return; + if (camera_result == Camera::Result::Success) { + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera != nullptr) { + save_camera_mode_with_lock(*maybe_potential_camera, mode); } - - notify_mode(); - save_camera_mode(mavlink_mode); } } -void CameraImpl::notify_mode() +void CameraImpl::notify_mode_for_all_with_lock() { - std::lock_guard lock(_mode.mutex); - - _mode.subscription_callbacks.queue( - _mode.data, [this](const auto& func) { _system_impl->call_user_callback(func); }); + for (auto& camera : potential_cameras_with_lock()) { + notify_mode_with_lock(*camera); + } } -bool CameraImpl::get_possible_setting_options(std::vector& settings) +void CameraImpl::notify_mode_with_lock(PotentialCamera& camera) { - settings.clear(); - - if (!_camera_definition) { - LogWarn() << "Error: no camera definition available yet"; - return false; - } - - std::unordered_map cd_settings{}; - _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; - } - - settings.push_back(cd_setting.first); - } - - return settings.size() > 0; + _mode_subscription_callbacks.queue( + Camera::ModeUpdate{camera.component_id, camera.mode}, + [this](const auto& func) { _system_impl->call_user_callback(func); }); } -bool CameraImpl::get_possible_options( - const std::string& setting_id, std::vector& options) +bool CameraImpl::get_possible_options_with_lock( + PotentialCamera& camera, const std::string& setting_id, std::vector& options) { options.clear(); - if (!_camera_definition) { + if (!camera.camera_definition) { LogWarn() << "Error: no camera definition available yet"; return false; } std::vector values; - if (!_camera_definition->get_possible_options(setting_id, values)) { + if (!camera.camera_definition->get_possible_options(setting_id, values)) { return false; } for (const auto& value : values) { - std::stringstream ss{}; - ss << value; Camera::Option option{}; - option.option_id = ss.str(); - if (!is_setting_range(setting_id)) { - get_option_str(setting_id, option.option_id, option.option_description); + option.option_id = value.get_string(); + if (!camera.camera_definition->is_setting_range(setting_id)) { + get_option_str_with_lock( + camera, setting_id, option.option_id, option.option_description); } options.push_back(option); } - return options.size() > 0; + return !options.empty(); } -bool CameraImpl::is_setting_range(const std::string& setting_id) -{ - return _camera_definition->is_setting_range(setting_id); -} +Camera::Result CameraImpl::set_setting(int32_t component_id, const Camera::Setting& setting) -Camera::Result CameraImpl::set_setting(Camera::Setting setting) { auto prom = std::make_shared>(); auto ret = prom->get_future(); - set_setting_async(setting, [&prom](Camera::Result result) { prom->set_value(result); }); + set_setting_async( + component_id, setting, [&prom](Camera::Result result) { prom->set_value(result); }); return ret.get(); } -void CameraImpl::set_setting_async(Camera::Setting setting, const Camera::ResultCallback callback) +void CameraImpl::set_setting_async( + int32_t component_id, const Camera::Setting& setting, const Camera::ResultCallback& callback) { - set_option_async(setting.setting_id, setting.option, callback); + set_option_async(component_id, setting.setting_id, setting.option, callback); } void CameraImpl::set_option_async( + int32_t component_id, const std::string& setting_id, const Camera::Option& option, const Camera::ResultCallback& callback) { - if (!_camera_definition) { - LogWarn() << "Error: no camera defnition available yet."; - if (callback) { - const auto temp_callback = callback; + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + if (callback != nullptr) { + _system_impl->call_user_callback( + [callback]() { callback(Camera::Result::CameraIdInvalid); }); + } + return; + } + + auto& camera = *maybe_potential_camera; + + if (camera.camera_definition == nullptr) { + if (callback != nullptr) { _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Error); }); + [callback]() { callback(Camera::Result::Unavailable); }); } return; } @@ -1824,54 +1675,47 @@ void CameraImpl::set_option_async( // We get it first so that we have the type of the param value. ParamValue value; - if (_camera_definition->is_setting_range(setting_id)) { + if (camera.camera_definition->is_setting_range(setting_id)) { // TODO: Get type from minimum. std::vector all_values; - if (!_camera_definition->get_all_options(setting_id, all_values)) { + if (!camera.camera_definition->get_all_options(setting_id, all_values)) { if (callback) { LogErr() << "Could not get all options to get type for range param."; - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Error); }); + _system_impl->call_user_callback([callback]() { callback(Camera::Result::Error); }); } return; } - if (all_values.size() == 0) { + if (all_values.empty()) { if (callback) { LogErr() << "Could not get any options to get type for range param."; - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Error); }); + _system_impl->call_user_callback([callback]() { callback(Camera::Result::Error); }); } return; } value = all_values[0]; - // Now re-use that type. - // FIXME: this is quite ugly, we should do better than that. + + // Now re-use that type. This is quite ugly, but I don't know how we could do better than + // that. if (!value.set_as_same_type(option.option_id)) { if (callback) { LogErr() << "Could not set option value to given type."; - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Error); }); + _system_impl->call_user_callback([callback]() { callback(Camera::Result::Error); }); } return; } } else { - if (!_camera_definition->get_option_value(setting_id, option.option_id, value)) { + if (!camera.camera_definition->get_option_value(setting_id, option.option_id, value)) { if (callback) { LogErr() << "Could not get option value."; - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Error); }); + _system_impl->call_user_callback([callback]() { callback(Camera::Result::Error); }); } return; } std::vector possible_values; - _camera_definition->get_possible_options(setting_id, possible_values); + camera.camera_definition->get_possible_options(setting_id, possible_values); bool allowed = false; for (const auto& possible_value : possible_values) { if (value == possible_value) { @@ -1881,9 +1725,7 @@ void CameraImpl::set_option_async( if (!allowed) { LogErr() << "Setting " << setting_id << "(" << option.option_id << ") not allowed"; if (callback) { - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Error); }); + _system_impl->call_user_callback([callback]() { callback(Camera::Result::Error); }); } return; } @@ -1892,85 +1734,109 @@ void CameraImpl::set_option_async( _system_impl->set_param_async( setting_id, value, - [this, callback, setting_id, value](MavlinkParameterClient::Result result) { - if (result == MavlinkParameterClient::Result::Success) { - if (!this->_camera_definition) { - if (callback) { - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Error); }); - } - return; - } + [this, component_id, callback, setting_id, value](MavlinkParameterClient::Result result) { + std::lock_guard lock_later(_mutex); + auto maybe_potential_camera_later = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + // We already checked these fields earlier, so we don't check again. + assert(maybe_potential_camera_later != nullptr); + assert(maybe_potential_camera_later->camera_definition != nullptr); - if (!_camera_definition->set_setting(setting_id, value)) { - if (callback) { - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Error); }); - } - return; - } + auto& camera_later = *maybe_potential_camera_later; + if (result != MavlinkParameterClient::Result::Success) { if (callback) { - const auto temp_callback = callback; - _system_impl->call_user_callback( - [temp_callback]() { temp_callback(Camera::Result::Success); }); + _system_impl->call_user_callback([callback, result]() { + callback(camera_result_from_parameter_result(result)); + }); } + return; + } - // FIXME: We are already holding the lock when this lambda is run and need to - // schedule the refresh_params() for later. - // We (ab)use the thread pool for the user callbacks for this. - _system_impl->call_user_callback([this]() { refresh_params(); }); - } else { + if (!camera_later.camera_definition->set_setting(setting_id, value)) { if (callback) { - const auto temp_callback = callback; - _system_impl->call_user_callback([temp_callback, result]() { - temp_callback(camera_result_from_parameter_result(result)); - }); + _system_impl->call_user_callback( + [callback]() { callback(Camera::Result::Error); }); } + return; } + + if (callback) { + _system_impl->call_user_callback( + [callback]() { callback(Camera::Result::Success); }); + } + refresh_params_with_lock(camera_later, false); }, this, - static_cast(_camera_id + MAV_COMP_ID_CAMERA), + camera.component_id, true); } void CameraImpl::get_setting_async( - Camera::Setting setting, const Camera::GetSettingCallback callback) + int32_t component_id, + const Camera::Setting& setting, + const Camera::GetSettingCallback& callback) { + { + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + if (callback != nullptr) { + _system_impl->call_user_callback( + [callback]() { callback(Camera::Result::CameraIdInvalid, {}); }); + } + return; + } + + auto& camera = *maybe_potential_camera; + + if (camera.camera_definition == nullptr) { + if (callback != nullptr) { + _system_impl->call_user_callback( + [callback]() { callback(Camera::Result::Unavailable, {}); }); + } + return; + } + } + get_option_async( + component_id, setting.setting_id, - [this, setting, callback](Camera::Result result, const Camera::Option& option) { + [this, callback](Camera::Result result, const Camera::Option& option) { Camera::Setting new_setting{}; new_setting.option = option; if (callback) { - const auto temp_callback = callback; _system_impl->call_user_callback( - [temp_callback, result, new_setting]() { temp_callback(result, new_setting); }); + [callback, result, new_setting]() { callback(result, new_setting); }); } }); } -std::pair CameraImpl::get_setting(Camera::Setting setting) +std::pair +CameraImpl::get_setting(int32_t component_id, const Camera::Setting& setting) { auto prom = std::make_shared>>(); auto ret = prom->get_future(); - get_setting_async(setting, [&prom](Camera::Result result, const Camera::Setting& new_setting) { - prom->set_value(std::make_pair<>(result, new_setting)); - }); + get_setting_async( + component_id, setting, [&prom](Camera::Result result, const Camera::Setting& new_setting) { + prom->set_value(std::make_pair<>(result, new_setting)); + }); return ret.get(); } -Camera::Result CameraImpl::get_option(const std::string& setting_id, Camera::Option& option) +Camera::Result +CameraImpl::get_option(int32_t component_id, const std::string& setting_id, Camera::Option& option) { auto prom = std::make_shared>(); auto ret = prom->get_future(); get_option_async( - setting_id, [prom, &option](Camera::Result result, const Camera::Option& option_gotten) { + component_id, + setting_id, + [prom, &option](Camera::Result result, const Camera::Option& option_gotten) { prom->set_value(result); if (result == Camera::Result::Success) { option = option_gotten; @@ -1987,29 +1853,40 @@ Camera::Result CameraImpl::get_option(const std::string& setting_id, Camera::Opt } void CameraImpl::get_option_async( + int32_t component_id, const std::string& setting_id, const std::function& callback) { - if (!_camera_definition) { - LogWarn() << "Error: no camera defnition available yet."; - if (callback) { - Camera::Option empty_option{}; - const auto temp_callback = callback; - _system_impl->call_user_callback([temp_callback, empty_option]() { - temp_callback(Camera::Result::Error, empty_option); - }); + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + if (callback != nullptr) { + _system_impl->call_user_callback( + [callback]() { callback(Camera::Result::CameraIdInvalid, {}); }); + } + return; + } + + auto& camera = *maybe_potential_camera; + + if (camera.camera_definition == nullptr) { + if (callback != nullptr) { + _system_impl->call_user_callback( + [callback]() { callback(Camera::Result::Unavailable, {}); }); } return; } ParamValue value; // We should have this cached and don't need to get the param. - if (_camera_definition->get_setting(setting_id, value)) { + if (camera.camera_definition->get_setting(setting_id, value)) { if (callback) { Camera::Option new_option{}; new_option.option_id = value.get_string(); - if (!is_setting_range(setting_id)) { - get_option_str(setting_id, new_option.option_id, new_option.option_description); + if (!camera.camera_definition->is_setting_range(setting_id)) { + get_option_str_with_lock( + camera, setting_id, new_option.option_id, new_option.option_description); } const auto temp_callback = callback; _system_impl->call_user_callback([temp_callback, new_option]() { @@ -2020,10 +1897,9 @@ void CameraImpl::get_option_async( // If this still happens, we request the param, but also complain. LogWarn() << "Setting '" << setting_id << "' not found."; if (callback) { - Camera::Option no_option{}; const auto temp_callback = callback; _system_impl->call_user_callback( - [temp_callback, no_option]() { temp_callback(Camera::Result::Error, no_option); }); + [temp_callback]() { temp_callback(Camera::Result::Error, {}); }); } } } @@ -2031,225 +1907,336 @@ 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(); + 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(); + 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() +void CameraImpl::notify_current_settings_for_all_with_lock() { - std::lock_guard lock(_subscribe_current_settings.mutex); + for (auto& potential_camera : potential_cameras_with_lock()) { + if (potential_camera->camera_definition != nullptr) { + notify_current_settings_with_lock(*potential_camera); + } + } +} - if (_subscribe_current_settings.callbacks.empty()) { - return; +void CameraImpl::notify_possible_setting_options_for_all_with_lock() +{ + for (auto& potential_camera : potential_cameras_with_lock()) { + if (potential_camera->camera_definition != nullptr) { + notify_possible_setting_options_with_lock(*potential_camera); + } } +} + +void CameraImpl::notify_current_settings_with_lock(PotentialCamera& potential_camera) +{ + assert(potential_camera.camera_definition != nullptr); - if (!_camera_definition) { - LogErr() << "notify_current_settings has no camera definition"; + if (_subscribe_current_settings_callbacks.empty()) { return; } - std::vector current_settings{}; - std::vector possible_setting_options{}; - if (!get_possible_setting_options(possible_setting_options)) { + auto possible_setting_options = get_possible_setting_options_with_lock(potential_camera); + if (possible_setting_options.first != Camera::Result::Success) { LogErr() << "Could not get possible settings in current options subscription."; return; } - for (auto& possible_setting : possible_setting_options) { + auto& camera = potential_camera; + + Camera::CurrentSettingsUpdate update{}; + update.component_id = potential_camera.component_id; + + for (auto& possible_setting : possible_setting_options.second) { // use the cache for this, presumably we updated it right before. ParamValue value; - if (_camera_definition->get_setting(possible_setting, value)) { + if (camera.camera_definition->get_setting(possible_setting.setting_id, value)) { Camera::Setting setting{}; - setting.setting_id = possible_setting; - setting.is_range = is_setting_range(possible_setting); - get_setting_str(setting.setting_id, setting.setting_description); + setting.setting_id = possible_setting.setting_id; + setting.is_range = + camera.camera_definition->is_setting_range(possible_setting.setting_id); + get_setting_str_with_lock(camera, setting.setting_id, setting.setting_description); setting.option.option_id = value.get_string(); - if (!is_setting_range(possible_setting)) { - get_option_str( + if (!camera.camera_definition->is_setting_range(possible_setting.setting_id)) { + get_option_str_with_lock( + camera, setting.setting_id, 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() +void CameraImpl::notify_possible_setting_options_with_lock(PotentialCamera& potential_camera) { - std::lock_guard lock(_subscribe_possible_setting_options.mutex); + assert(potential_camera.camera_definition != nullptr); - if (_subscribe_possible_setting_options.callbacks.empty()) { + if (_subscribe_possible_setting_options_callbacks.empty()) { return; } - if (!_camera_definition) { - LogErr() << "notify_possible_setting_options has no camera definition"; - return; - } + Camera::PossibleSettingOptionsUpdate update{}; + update.component_id = potential_camera.component_id; - auto setting_options = possible_setting_options(); - if (setting_options.size() == 0) { + 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, [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::vector CameraImpl::possible_setting_options() +std::pair> +CameraImpl::get_possible_setting_options(int32_t component_id) { - std::vector results{}; + std::pair> result; + + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + result.first = Camera::Result::CameraIdInvalid; + return result; + } - std::vector possible_settings{}; - if (!get_possible_setting_options(possible_settings)) { - LogErr() << "Could not get possible settings."; - return results; + auto& camera = *maybe_potential_camera; + if (camera.is_fetching_camera_definition || camera.camera_definition == nullptr) { + result.first = Camera::Result::Unavailable; + return result; } + return get_possible_setting_options_with_lock(*maybe_potential_camera); +} + +std::pair> +CameraImpl::get_possible_setting_options_with_lock(PotentialCamera& potential_camera) +{ + std::pair> result; + + std::unordered_map possible_settings; + assert(potential_camera.camera_definition != nullptr); + + auto& camera = potential_camera; + + camera.camera_definition->get_possible_settings(possible_settings); + for (auto& possible_setting : possible_settings) { Camera::SettingOptions setting_options{}; - setting_options.setting_id = possible_setting; - setting_options.is_range = _camera_definition->is_setting_range(possible_setting); - get_setting_str(setting_options.setting_id, setting_options.setting_description); - get_possible_options(possible_setting, setting_options.options); - results.push_back(setting_options); + setting_options.setting_id = possible_setting.first; + setting_options.is_range = + camera.camera_definition->is_setting_range(possible_setting.first); + get_setting_str_with_lock( + camera, setting_options.setting_id, setting_options.setting_description); + get_possible_options_with_lock(camera, possible_setting.first, setting_options.options); + result.second.push_back(setting_options); } - return results; + result.first = Camera::Result::Success; + return result; } -void CameraImpl::refresh_params() +void CameraImpl::refresh_params_with_lock(PotentialCamera& potential_camera, bool initial_load) { - if (!_camera_definition) { - return; - } + assert(potential_camera.camera_definition != nullptr); std::vector> params; - _camera_definition->get_unknown_params(params); - if (params.size() == 0) { + potential_camera.camera_definition->get_unknown_params(params); + + if (params.empty()) { // We're assuming that we changed one option and this did not cause // any other possible settings to change. However, we still would // like to notify the current settings with this one change. - notify_current_settings(); - notify_possible_setting_options(); + notify_current_settings_with_lock(potential_camera); + notify_possible_setting_options_with_lock(potential_camera); return; } + auto component_id = potential_camera.component_id; + unsigned count = 0; for (const auto& param : params) { const std::string& param_name = param.first; const ParamValue& param_value_type = param.second; const bool is_last = (count == params.size() - 1); - _system_impl->get_param_async( - param_name, - param_value_type, - [param_name, is_last, this](MavlinkParameterClient::Result result, ParamValue value) { - if (result != MavlinkParameterClient::Result::Success) { - return; - } - // We need to check again by the time this callback runs - if (!this->_camera_definition) { - return; - } + if (_debugging) { + LogDebug() << "Trying to get param: " << param_name; + } + _system_impl->param_sender(potential_camera.component_id, true) + ->get_param_async( + param_name, + param_value_type, + [component_id, param_name, is_last, this]( + MavlinkParameterClient::Result result, const ParamValue& value) { + if (result != MavlinkParameterClient::Result::Success) { + return; + } - if (!this->_camera_definition->set_setting(param_name, value)) { - return; - } + std::lock_guard lock_later(_mutex); + auto maybe_potential_camera_later = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + // We already checked these fields earlier, so we don't check again. + assert(maybe_potential_camera_later != nullptr); + assert(maybe_potential_camera_later->camera_definition != nullptr); - if (is_last) { - notify_current_settings(); - notify_possible_setting_options(); - } - }, - this, - static_cast(_camera_id + MAV_COMP_ID_CAMERA), - true); + auto& camera_later = *maybe_potential_camera_later; + + if (camera_later.camera_definition->set_setting(param_name, value)) { + if (_debugging) { + LogDebug() << "Got setting for " << param_name << ": " << value; + } + return; + } + + if (is_last) { + notify_current_settings_with_lock(camera_later); + notify_possible_setting_options_with_lock(camera_later); + } + }, + this); + + if (initial_load) { + subscribe_to_param_changes_with_lock(potential_camera, param_name, param_value_type); + } ++count; } } -void CameraImpl::invalidate_params() +void CameraImpl::subscribe_to_param_changes_with_lock( + PotentialCamera& camera, std::string param_name, const ParamValue& param_value_type) { - if (!_camera_definition) { - return; - } + auto component_id = camera.component_id; + auto changed = [this, component_id, param_name](auto new_param) { + if (_debugging) { + LogDebug() << "Got changing param: " << param_name << " -> " << new_param; + } - _camera_definition->set_all_params_unknown(); + std::lock_guard lock_later(_mutex); + + auto maybe_potential_camera_later = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + // We already checked these fields earlier, so we don't check again. + assert(maybe_potential_camera_later != nullptr); + assert(maybe_potential_camera_later->camera_definition != nullptr); + auto& camera_later = *maybe_potential_camera_later; + + ParamValue param_value; + param_value.set(new_param); + camera_later.camera_definition->set_setting(param_name, param_value); + }; + + if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else if (param_value_type.is()) { + _system_impl->param_sender(camera.component_id, true) + ->subscribe_param_changed(param_name, changed, this); + } else { + LogErr() << "Unknown type for param " << param_name; + } } -bool CameraImpl::get_setting_str(const std::string& setting_id, std::string& description) +bool CameraImpl::get_setting_str_with_lock( + PotentialCamera& potential_camera, const std::string& setting_id, std::string& description) { - if (!_camera_definition) { + if (potential_camera.camera_definition == nullptr) { return false; } - return _camera_definition->get_setting_str(setting_id, description); + return potential_camera.camera_definition->get_setting_str(setting_id, description); } -bool CameraImpl::get_option_str( - const std::string& setting_id, const std::string& option_id, std::string& description) +bool CameraImpl::get_option_str_with_lock( + PotentialCamera& potential_camera, + const std::string& setting_id, + const std::string& option_id, + std::string& description) { - if (!_camera_definition) { + if (potential_camera.camera_definition == nullptr) { return false; } - return _camera_definition->get_option_str(setting_id, option_id, description); + return potential_camera.camera_definition->get_option_str(setting_id, option_id, description); } -void CameraImpl::request_camera_settings() +void CameraImpl::request_camera_information(uint8_t component_id) { _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_CAMERA_SETTINGS, _camera_id + MAV_COMP_ID_CAMERA, nullptr); + MAVLINK_MSG_ID_CAMERA_INFORMATION, fixup_component_target(component_id), nullptr); } -void CameraImpl::request_camera_information() -{ - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_CAMERA_INFORMATION, _camera_id + MAV_COMP_ID_CAMERA, nullptr); -} - -Camera::Result CameraImpl::format_storage(int32_t storage_id) +Camera::Result CameraImpl::format_storage(int32_t component_id, int32_t storage_id) { auto prom = std::make_shared>(); auto ret = prom->get_future(); - format_storage_async(storage_id, [prom](Camera::Result result) { prom->set_value(result); }); + format_storage_async( + component_id, storage_id, [prom](Camera::Result result) { prom->set_value(result); }); return ret.get(); } -void CameraImpl::format_storage_async(int32_t storage_id, Camera::ResultCallback callback) +void CameraImpl::format_storage_async( + int32_t component_id, int32_t storage_id, const Camera::ResultCallback& callback) { MavlinkCommandSender::CommandLong cmd_format{}; @@ -2257,38 +2244,35 @@ void CameraImpl::format_storage_async(int32_t storage_id, Camera::ResultCallback cmd_format.params.maybe_param1 = static_cast(storage_id); // storage ID cmd_format.params.maybe_param2 = 1.0f; // format cmd_format.params.maybe_param3 = 1.0f; // clear - cmd_format.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd_format.target_component_id = component_id; _system_impl->send_command_async( cmd_format, [this, callback](MavlinkCommandSender::Result result, float progress) { UNUSED(progress); receive_command_result(result, [this, callback](Camera::Result camera_result) { - if (camera_result == Camera::Result::Success) { - reset_following_format_storage(); - } - callback(camera_result); }); }); } -Camera::Result CameraImpl::reset_settings() +Camera::Result CameraImpl::reset_settings(int32_t component_id) { auto prom = std::make_shared>(); auto ret = prom->get_future(); - reset_settings_async([prom](Camera::Result result) { prom->set_value(result); }); + reset_settings_async(component_id, [prom](Camera::Result result) { prom->set_value(result); }); return ret.get(); } -void CameraImpl::reset_settings_async(const Camera::ResultCallback callback) + +void CameraImpl::reset_settings_async(int32_t component_id, const Camera::ResultCallback& callback) { MavlinkCommandSender::CommandLong cmd_format{}; cmd_format.command = MAV_CMD_RESET_CAMERA_SETTINGS; cmd_format.params.maybe_param1 = 1.0f; // reset - cmd_format.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; + cmd_format.target_component_id = component_id; _system_impl->send_command_async( cmd_format, [this, callback](MavlinkCommandSender::Result result, float progress) { @@ -2300,29 +2284,25 @@ void CameraImpl::reset_settings_async(const Camera::ResultCallback callback) }); } -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(); - } + 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> -CameraImpl::list_photos(Camera::PhotosRange photos_range) +CameraImpl::list_photos(int32_t component_id, Camera::PhotosRange photos_range) { std::promise>> prom; auto ret = prom.get_future(); list_photos_async( - photos_range, [&prom](Camera::Result result, std::vector photo_list) { + component_id, + photos_range, + [&prom](Camera::Result result, const std::vector& photo_list) { prom.set_value(std::make_pair(result, photo_list)); }); @@ -2330,39 +2310,58 @@ CameraImpl::list_photos(Camera::PhotosRange photos_range) } void CameraImpl::list_photos_async( - Camera::PhotosRange photos_range, const Camera::ListPhotosCallback callback) + int32_t component_id, + Camera::PhotosRange photos_range, + const Camera::ListPhotosCallback& callback) { if (!callback) { LogWarn() << "Trying to get a photo list with a null callback, ignoring..."; return; } - { - std::lock_guard status_lock(_status.mutex); + std::lock_guard lock(_mutex); - if (_status.is_fetching_photos) { - _system_impl->call_user_callback([callback]() { - callback(Camera::Result::Busy, std::vector{}); - }); - return; - } else { - _status.is_fetching_photos = true; - } + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + LogWarn() << "Invalid camera ID: " << component_id; + return; + } + auto& camera = *maybe_potential_camera; - if (_status.image_count == -1) { - LogErr() << "Cannot list photos: camera status has not been received yet!"; - _status.is_fetching_photos = false; - _system_impl->call_user_callback([callback]() { - callback(Camera::Result::Error, std::vector{}); - }); - return; + if (camera.capture_status.image_count == -1) { + LogErr() << "Cannot list photos: camera status has not been received yet!"; + _system_impl->call_user_callback( + [callback]() { callback(Camera::Result::Error, std::vector{}); }); + return; + } + + // By default we just try to keep track of photos captured while we were connected. + // If the API user asks for all, we will start requesting all previous ones. + + if (camera.capture_status.photos_range == Camera::PhotosRange::SinceConnection && + photos_range == Camera::PhotosRange::All) { + camera.capture_status.photos_range = photos_range; + + auto oldest_photo_it = std::min_element( + camera.capture_status.photo_list.begin(), + camera.capture_status.photo_list.end(), + [](auto& a, auto& b) { return a.first < b.first; }); + + if (oldest_photo_it != camera.capture_status.photo_list.end()) { + for (int i = 0; i < oldest_photo_it->first; ++i) { + if (camera.capture_info.missing_image_retries.find(i) == + camera.capture_info.missing_image_retries.end()) { + camera.capture_info.missing_image_retries[i] = 0; + } + } } } - const int start_index = [this, photos_range]() { + const int start_index = [&, this]() { switch (photos_range) { case Camera::PhotosRange::SinceConnection: - return _status.image_count_at_connection; + return camera.capture_status.image_count_at_connection; case Camera::PhotosRange::All: // FALLTHROUGH default: @@ -2370,73 +2369,189 @@ void CameraImpl::list_photos_async( } }(); - std::thread([this, start_index, callback]() { - std::unique_lock capture_request_lock(_captured_request_mutex); - - for (int i = start_index; i < _status.image_count; i++) { - // In case the vehicle sends capture info, but not those we are asking, we do not - // want to loop infinitely. The safety_count is here to abort if this happens. - auto safety_count = 0; - const auto safety_count_boundary = 10; - - while (_status.photo_list.find(i) == _status.photo_list.end() && - safety_count < safety_count_boundary) { - safety_count++; - - auto request_try_number = 0; - const auto request_try_limit = - 10; // Timeout if the request times out that many times - auto cv_status = std::cv_status::timeout; - - while (cv_status == std::cv_status::timeout) { - request_try_number++; - if (request_try_number >= request_try_limit) { - std::lock_guard status_lock(_status.mutex); - _status.is_fetching_photos = false; - _system_impl->call_user_callback([callback]() { - callback(Camera::Result::Timeout, std::vector{}); - }); - return; - } + std::vector photo_list; - _system_impl->mavlink_request_message().request( - MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, - _camera_id + MAV_COMP_ID_CAMERA, - nullptr, - i); - cv_status = _captured_request_cv.wait_for( - capture_request_lock, std::chrono::seconds(1)); - } - } + for (const auto& capture_info : camera.capture_status.photo_list) { + if (capture_info.first >= start_index) { + photo_list.push_back(capture_info.second); + } + } - if (safety_count == safety_count_boundary) { - std::lock_guard status_lock(_status.mutex); - _status.is_fetching_photos = false; - _system_impl->call_user_callback([callback]() { - callback(Camera::Result::Error, std::vector{}); - }); - return; - } + _system_impl->call_user_callback( + [callback, photo_list]() { callback(Camera::Result::Success, photo_list); }); +} + +std::pair CameraImpl::get_mode(int32_t component_id) +{ + std::pair result{}; + + std::lock_guard lock(_mutex); + + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + result.first = Camera::Result::CameraIdInvalid; + } else { + result.first = Camera::Result::Success; + result.second = maybe_potential_camera->mode; + } + + return result; +} + +std::pair CameraImpl::get_storage(int32_t component_id) +{ + std::pair result{}; + + std::lock_guard lock(_mutex); + + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + result.first = Camera::Result::CameraIdInvalid; + } else { + if (maybe_potential_camera->received_storage) { + result.first = Camera::Result::Success; + result.second = maybe_potential_camera->storage; + } else { + result.first = Camera::Result::Unavailable; } + } + + return result; +} - std::vector photo_list; - { - std::lock_guard status_lock(_status.mutex); +std::pair +CameraImpl::get_video_stream_info(int32_t component_id) +{ + std::pair result{}; - for (auto capture_info : _status.photo_list) { - if (capture_info.first >= start_index) { - photo_list.push_back(capture_info.second); - } + std::lock_guard lock(_mutex); + + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + 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 component_id) +{ + std::pair> result; + + std::lock_guard lock(_mutex); + auto maybe_potential_camera = + maybe_potential_camera_for_component_id_with_lock(component_id, 0); + if (maybe_potential_camera == nullptr) { + result.first = Camera::Result::CameraIdInvalid; + return result; + } + + auto& camera = *maybe_potential_camera; + if (camera.is_fetching_camera_definition || camera.camera_definition == nullptr) { + result.first = Camera::Result::Unavailable; + return result; + } + + auto possible_setting_options = get_possible_setting_options_with_lock(camera); + if (possible_setting_options.first != Camera::Result::Success) { + result.first = possible_setting_options.first; + return result; + } + + for (auto& possible_setting : possible_setting_options.second) { + // use the cache for this, presumably we updated it right before. + ParamValue value; + if (camera.camera_definition->get_setting(possible_setting.setting_id, value)) { + Camera::Setting setting{}; + setting.setting_id = possible_setting.setting_id; + setting.is_range = + camera.camera_definition->is_setting_range(possible_setting.setting_id); + get_setting_str_with_lock(camera, setting.setting_id, setting.setting_description); + setting.option.option_id = value.get_string(); + if (!camera.camera_definition->is_setting_range(possible_setting.setting_id)) { + get_option_str_with_lock( + camera, + setting.setting_id, + setting.option.option_id, + setting.option.option_description); } + result.second.push_back(setting); + } + } - _status.is_fetching_photos = false; + result.first = Camera::Result::Success; + return result; +} - const auto temp_callback = callback; - _system_impl->call_user_callback([temp_callback, photo_list]() { - temp_callback(Camera::Result::Success, photo_list); - }); +uint16_t CameraImpl::get_and_increment_capture_sequence(int32_t component_id) +{ + if (component_id == 0) { + // All cameras + return 0; + } + + std::lock_guard lock(_mutex); + + for (auto& potential_camera : _potential_cameras) { + if (potential_camera.component_id == component_id) { + return potential_camera.capture_sequence++; + } + } + + return 0; +} + +std::vector CameraImpl::potential_cameras_with_lock() +{ + std::vector result; + + for (auto& potential_camera : _potential_cameras) { + if (!potential_camera.maybe_information) { + continue; } - }).detach(); + result.push_back(&potential_camera); + } + + return result; +} + +CameraImpl::PotentialCamera* CameraImpl::maybe_potential_camera_for_component_id_with_lock( + uint8_t component_id, uint8_t camera_device_id) +{ + // Component Ids 1-6 means the camera is connected to the autopilot and the + // ID is set by the camera_device_id instead. + if (component_id == 1 && camera_device_id != 0) { + component_id = camera_device_id; + } + + const auto it = std::find_if( + _potential_cameras.begin(), _potential_cameras.end(), [&](auto& potential_camera) { + return potential_camera.component_id = component_id; + }); + + if (it == _potential_cameras.end()) { + return nullptr; + } + + // How to get pointer from iterator? + return &(*it); +} + +uint8_t CameraImpl::fixup_component_target(uint8_t component_id) +{ + // Component Ids 1-6 means the camera is connected to the autopilot. + if (component_id >= 1 && component_id <= 6) { + return 1; + } + + return component_id; } } // namespace mavsdk diff --git a/src/mavsdk/plugins/camera/camera_impl.h b/src/mavsdk/plugins/camera/camera_impl.h index ed435b215c..a284265d5d 100644 --- a/src/mavsdk/plugins/camera/camera_impl.h +++ b/src/mavsdk/plugins/camera/camera_impl.h @@ -1,8 +1,13 @@ #pragma once +#include +#include #include +#include +#include #include "camera_definition.h" +#include "file_cache.h" #include "mavlink_include.h" #include "plugins/camera/camera.h" #include "plugin_impl_base.h" @@ -23,152 +28,238 @@ class CameraImpl : public PluginImplBase { void enable() override; void disable() override; - Camera::Result prepare(); - Camera::Result select_camera(size_t id); + Camera::Result take_photo(int32_t camera_id); - Camera::Result take_photo(); + Camera::Result start_photo_interval(int32_t camera_id, float interval_s); + Camera::Result stop_photo_interval(int32_t camera_id); - Camera::Result start_photo_interval(float interval_s); - Camera::Result stop_photo_interval(); + Camera::Result start_video(int32_t camera_id); + Camera::Result stop_video(int32_t camera_id); - Camera::Result start_video(); - Camera::Result stop_video(); + Camera::Result zoom_in_start(int32_t camera_id); + Camera::Result zoom_out_start(int32_t camera_id); + Camera::Result zoom_stop(int32_t camera_id); + Camera::Result zoom_range(int32_t camera_id, float range); + + Camera::Result track_point(int32_t camera_id, float point_x, float point_y, float radius); + Camera::Result track_rectangle( + int32_t camera_id, + float top_left_x, + float top_left_y, + float bottom_right_x, + float bottom_right_y); + Camera::Result track_stop(int32_t camera_id); + Camera::Result focus_in_start(int32_t camera_id); + Camera::Result focus_out_start(int32_t camera_id); + Camera::Result focus_stop(int32_t camera_id); + Camera::Result focus_range(int32_t camera_id, float range); + + void zoom_in_start_async(int32_t camera_id, const Camera::ResultCallback& callback); + void zoom_out_start_async(int32_t camera_id, const Camera::ResultCallback& callback); + void zoom_stop_async(int32_t camera_id, const Camera::ResultCallback& callback); + void zoom_range_async(int32_t camera_id, float range, const Camera::ResultCallback& callback); - Camera::Result zoom_in_start(); - Camera::Result zoom_out_start(); - Camera::Result zoom_stop(); - Camera::Result zoom_range(float range); - Camera::Result track_point(float point_x, float point_y, float radius); - Camera::Result - track_rectangle(float top_left_x, float top_left_y, float bottom_right_x, float bottom_right_y); - Camera::Result track_stop(); - Camera::Result focus_in_start(); - Camera::Result focus_out_start(); - Camera::Result focus_stop(); - Camera::Result focus_range(float range); - - void zoom_in_start_async(const Camera::ResultCallback& callback); - void zoom_out_start_async(const Camera::ResultCallback& callback); - void zoom_stop_async(const Camera::ResultCallback& callback); - void zoom_range_async(float range, const Camera::ResultCallback& callback); void track_point_async( - float point_x, float point_y, float radius, const Camera::ResultCallback& callback); + int32_t camera_id, + float point_x, + float point_y, + float radius, + const Camera::ResultCallback& callback); void track_rectangle_async( + int32_t camera_id, float top_left_x, float top_left_y, float bottom_right_x, float bottom_right_y, const Camera::ResultCallback& callback); - void track_stop_async(const Camera::ResultCallback& callback); - void focus_in_start_async(const Camera::ResultCallback& callback); - void focus_out_start_async(const Camera::ResultCallback& callback); - void focus_stop_async(const Camera::ResultCallback& callback); - void focus_range_async(float range, const Camera::ResultCallback& callback); + void track_stop_async(int32_t camera_id, const Camera::ResultCallback& callback); - void prepare_async(const Camera::ResultCallback& callback); - void take_photo_async(const Camera::ResultCallback& callback); - void start_photo_interval_async(float interval_s, const Camera::ResultCallback& callback); - void stop_photo_interval_async(const Camera::ResultCallback& callback); - void start_video_async(const Camera::ResultCallback& callback); - void stop_video_async(const Camera::ResultCallback& callback); + void focus_in_start_async(int32_t camera_id, const Camera::ResultCallback& callback); + void focus_out_start_async(int32_t camera_id, const Camera::ResultCallback& callback); + void focus_stop_async(int32_t camera_id, const Camera::ResultCallback& callback); + void focus_range_async(int32_t camera_id, float range, const Camera::ResultCallback& callback); - Camera::Information information() const; - Camera::InformationHandle subscribe_information(const Camera::InformationCallback& callback); - void unsubscribe_information(Camera::InformationHandle handle); + void take_photo_async(int32_t camera_id, const Camera::ResultCallback& callback); + void start_photo_interval_async( + int32_t camera_id, float interval_s, const Camera::ResultCallback& callback); + void stop_photo_interval_async(int32_t camera_id, const Camera::ResultCallback& callback); + void start_video_async(int32_t camera_id, const Camera::ResultCallback& callback); + void stop_video_async(int32_t camera_id, const Camera::ResultCallback& callback); - std::pair get_video_stream_info(); + Camera::CameraList camera_list(); + Camera::CameraListHandle subscribe_camera_list(const Camera::CameraListCallback& callback); + void unsubscribe_camera_list(Camera::CameraListHandle handle); Camera::VideoStreamInfo video_stream_info(); Camera::VideoStreamInfoHandle subscribe_video_stream_info(const Camera::VideoStreamInfoCallback& callback); void unsubscribe_video_stream_info(Camera::VideoStreamInfoHandle handle); - Camera::Result start_video_streaming(int32_t stream_id); - Camera::Result stop_video_streaming(int32_t stream_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); + + Camera::Result set_mode(int32_t camera_id, Camera::Mode mode); + void + set_mode_async(int32_t camera_id, Camera::Mode mode, const Camera::ResultCallback& callback); - Camera::Result set_mode(const Camera::Mode mode); - void set_mode_async(const Camera::Mode mode, const Camera::ResultCallback& callback); + std::pair get_mode(int32_t camera_id); - Camera::Mode mode(); Camera::ModeHandle subscribe_mode(const Camera::ModeCallback& callback); void unsubscribe_mode(Camera::ModeHandle handle); Camera::CaptureInfoHandle subscribe_capture_info(const Camera::CaptureInfoCallback& callback); void unsubscribe_capture_info(Camera::CaptureInfoHandle handle); - Camera::Status status(); - Camera::StatusHandle subscribe_status(const Camera::StatusCallback& callback); - void unsubscribe_status(Camera::StatusHandle handle); + Camera::StorageHandle subscribe_storage(const Camera::StorageCallback& callback); + void unsubscribe_storage(Camera::StorageHandle handle); - Camera::Result set_setting(Camera::Setting setting); - void set_setting_async(Camera::Setting setting, const Camera::ResultCallback callback); + std::pair get_storage(int32_t camera_id); - void get_setting_async(Camera::Setting setting, const Camera::GetSettingCallback callback); - std::pair get_setting(Camera::Setting setting); + void get_setting_async( + int32_t camera_id, + const Camera::Setting& setting, + const Camera::GetSettingCallback& callback); + Camera::Result set_setting(int32_t camera_id, const Camera::Setting& setting); + void set_setting_async( + int32_t camera_id, const Camera::Setting& setting, const Camera::ResultCallback& callback); - std::vector possible_setting_options(); + std::pair + get_setting(int32_t camera_id, const Camera::Setting& setting); bool is_setting_range(const std::string& setting_id); Camera::CurrentSettingsHandle subscribe_current_settings(const Camera::CurrentSettingsCallback& callback); void unsubscribe_current_settings(Camera::CurrentSettingsHandle handle); + + std::pair> get_current_settings(int32_t camera_id); + Camera::PossibleSettingOptionsHandle subscribe_possible_setting_options(const Camera::PossibleSettingOptionsCallback& callback); void unsubscribe_possible_setting_options(Camera::PossibleSettingOptionsHandle handle); - Camera::Result format_storage(int32_t storage_id); - void format_storage_async(int32_t storage_id, const Camera::ResultCallback callback); + std::pair> + get_possible_setting_options(int32_t camera_id); - Camera::Result reset_settings(); - void reset_settings_async(const Camera::ResultCallback callback); + Camera::Result format_storage(int32_t camera_id, int32_t storage_id); + void format_storage_async( + int32_t camera_id, int32_t storage_id, const Camera::ResultCallback& callback); + + Camera::Result reset_settings(int32_t camera_id); + void reset_settings_async(int32_t camera_id, const Camera::ResultCallback& callback); std::pair> - list_photos(Camera::PhotosRange photos_range); - void - list_photos_async(Camera::PhotosRange photos_range, const Camera::ListPhotosCallback callback); + list_photos(int32_t camera_id, Camera::PhotosRange photos_range); + void list_photos_async( + int32_t camera_id, + Camera::PhotosRange photos_range, + const Camera::ListPhotosCallback& callback); CameraImpl(const CameraImpl&) = delete; CameraImpl& operator=(const CameraImpl&) = delete; private: - bool get_possible_setting_options(std::vector& settings); - bool get_possible_options(const std::string& setting_id, std::vector& options); + struct CaptureStatus { + 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; + Camera::PhotosRange photos_range{Camera::PhotosRange::SinceConnection}; + }; + + 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{}; + unsigned camera_definition_version{}; + Camera::Result camera_definition_result{Camera::Result::Unknown}; + bool is_fetching_camera_definition{false}; + size_t camera_definition_fetch_count{0}; + + bool information_requested{false}; + + CaptureStatus capture_status{}; + CaptureInfo capture_info{}; + + Camera::Storage storage{}; + Camera::VideoStreamInfo video_stream_info{}; + Camera::Mode mode{Camera::Mode::Unknown}; + + uint16_t capture_sequence{0}; + + bool received_storage{false}; + bool received_video_stream_info{false}; + + // This is 1-6 for autopilot based cameras. Commands need to be sent out to 1 (autopilot). + uint8_t component_id; + + bool operator==(const PotentialCamera& other) const + { + return this->component_id == other.component_id; + } + }; + + Camera::CameraList camera_list_with_lock(); + + std::pair> + get_possible_setting_options_with_lock(PotentialCamera& potential_camera); + + void get_setting_async_with_lock( + PotentialCamera& potential_camera, + Camera::Setting setting, + const Camera::GetSettingCallback& callback); + + bool get_possible_options_with_lock( + PotentialCamera& camera, + const std::string& setting_id, + std::vector& options); void set_option_async( + int32_t camera_id, const std::string& setting_id, const Camera::Option& option, const Camera::ResultCallback& callback); - Camera::Result get_option(const std::string& setting_id, Camera::Option& option); + Camera::Result + get_option(int32_t camera_id, const std::string& setting_id, Camera::Option& option); void get_option_async( + int32_t camera_id, const std::string& setting_id, const std::function& callback); - bool get_setting_str(const std::string& setting_id, std::string& description); - bool get_option_str( - const std::string& setting_id, const std::string& option_id, std::string& description); - - void check_connection_status(); - void manual_enable(); - void manual_disable(); - void update_component(); + bool get_setting_str_with_lock( + PotentialCamera& potential_camera, const std::string& setting_id, std::string& description); + bool get_option_str_with_lock( + PotentialCamera& potential_camera, + const std::string& setting_id, + const std::string& option_id, + std::string& description); void receive_set_mode_command_result( - const MavlinkCommandSender::Result command_result, - const Camera::ResultCallback callback, - const Camera::Mode mode); + MavlinkCommandSender::Result command_result, + const Camera::ResultCallback& callback, + Camera::Mode mode, + int32_t camera_id); static Camera::Result - camera_result_from_command_result(const MavlinkCommandSender::Result command_result); + camera_result_from_command_result(MavlinkCommandSender::Result command_result); static Camera::Result - camera_result_from_parameter_result(const MavlinkParameterClient::Result parameter_result); + camera_result_from_parameter_result(MavlinkParameterClient::Result parameter_result); void receive_command_result( - MavlinkCommandSender::Result command_result, const Camera::ResultCallback& callback); - - static bool interval_valid(float interval_s); + MavlinkCommandSender::Result command_result, const Camera::ResultCallback& callback) const; + void process_heartbeat(const mavlink_message_t& message); void process_camera_image_captured(const mavlink_message_t& message); void process_storage_information(const mavlink_message_t& message); void process_camera_capture_status(const mavlink_message_t& message); @@ -176,146 +267,128 @@ 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(); - Camera::Status::StorageStatus storage_status_from_mavlink(const int storage_status) const; - Camera::Status::StorageType storage_type_from_mavlink(const int storage_type) const; + void reset_following_format_storage_with_lock(PotentialCamera& potential_camera); - Camera::EulerAngle to_euler_angle_from_quaternion(Camera::Quaternion quaternion); + 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_current_settings(); - void notify_possible_setting_options(); + void notify_mode_for_all_with_lock(); + void notify_mode_with_lock(PotentialCamera& camera); - void check_status(); + void notify_video_stream_info_for_all_with_lock(); + void notify_video_stream_info_with_lock(PotentialCamera& camera); - bool should_fetch_camera_definition(const std::string& uri) const; - Camera::Result fetch_camera_definition( - const mavlink_camera_information_t& camera_information, std::string& camera_definition_out); - Camera::Result - download_definition_file_curl(const std::string& uri, std::string& camera_definition_out); - Camera::Result - download_definition_file_mftp(const std::string& uri, std::string& camera_definition_out); + void notify_current_settings_for_all_with_lock(); + void notify_current_settings_with_lock(PotentialCamera& potential_camera); + + void notify_possible_setting_options_for_all_with_lock(); + void notify_possible_setting_options_with_lock(PotentialCamera& potential_camera); - void refresh_params(); - void invalidate_params(); + void notify_camera_list_with_lock(); - void save_camera_mode(const float mavlink_camera_mode); - float to_mavlink_camera_mode(const Camera::Mode mode) const; - Camera::Mode to_camera_mode(const uint8_t mavlink_camera_mode) const; + void notify_storage_for_all_with_lock(); + void notify_storage_with_lock(PotentialCamera& camera); + + void refresh_params_with_lock(PotentialCamera& camera, bool initial_load); + + void save_camera_mode_with_lock(PotentialCamera& potential_camera, Camera::Mode mode); + + static Camera::Storage::StorageStatus storage_status_from_mavlink(int storage_status); + static Camera::Storage::StorageType storage_type_from_mavlink(int storage_type); + static float to_mavlink_camera_mode(Camera::Mode mode); + static Camera::Mode to_camera_mode(uint8_t mavlink_camera_mode); CallEveryHandler::Cookie _camera_information_call_every_cookie{}; - CallEveryHandler::Cookie _check_connection_status_call_every_cookie{}; CallEveryHandler::Cookie _request_missing_capture_info_cookie{}; - void request_camera_settings(); - void request_camera_information(); - void request_video_stream_info(); - void request_video_stream_status(); - void request_status(); + // 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); - MavlinkCommandSender::CommandLong make_command_take_photo(float interval_s, float no_of_photos); - MavlinkCommandSender::CommandLong make_command_stop_photo(); + MavlinkCommandSender::CommandLong + make_command_take_photo(int32_t camera_id, float interval_s, float no_of_photos); + MavlinkCommandSender::CommandLong make_command_stop_photo(int32_t camera_id); - MavlinkCommandSender::CommandLong make_command_set_camera_mode(float mavlink_mode); - MavlinkCommandSender::CommandLong make_command_start_video(float capture_status_rate_hz); - MavlinkCommandSender::CommandLong make_command_stop_video(); + MavlinkCommandSender::CommandLong + make_command_set_camera_mode(int32_t camera_id, float mavlink_mode); - MavlinkCommandSender::CommandLong make_command_start_video_streaming(int32_t stream_id); - MavlinkCommandSender::CommandLong make_command_stop_video_streaming(int32_t stream_id); + MavlinkCommandSender::CommandLong + make_command_start_video(int32_t camera_id, float capture_status_rate_hz); + MavlinkCommandSender::CommandLong make_command_stop_video(int32_t camera_id); - MavlinkCommandSender::CommandLong make_command_zoom_in(); - MavlinkCommandSender::CommandLong make_command_zoom_out(); - MavlinkCommandSender::CommandLong make_command_zoom_stop(); - MavlinkCommandSender::CommandLong make_command_zoom_range(float range); MavlinkCommandSender::CommandLong - make_command_track_point(float point_x, float point_y, float radius); + make_command_start_video_streaming(int32_t camera_id, int32_t stream_id); + MavlinkCommandSender::CommandLong + make_command_stop_video_streaming(int32_t camera_id, int32_t stream_id); + + MavlinkCommandSender::CommandLong make_command_zoom_in(int32_t camera_id); + MavlinkCommandSender::CommandLong make_command_zoom_out(int32_t camera_id); + MavlinkCommandSender::CommandLong make_command_zoom_stop(int32_t camera_id); + MavlinkCommandSender::CommandLong make_command_zoom_range(int32_t camera_id, float range); + MavlinkCommandSender::CommandLong + make_command_track_point(int32_t camera_id, float point_x, float point_y, float radius); MavlinkCommandSender::CommandLong make_command_track_rectangle( - float top_left_x, float top_left_y, float bottom_right_x, float bottom_right_y); - MavlinkCommandSender::CommandLong make_command_track_stop(); - MavlinkCommandSender::CommandLong make_command_focus_in(); - MavlinkCommandSender::CommandLong make_command_focus_out(); - MavlinkCommandSender::CommandLong make_command_focus_stop(); - MavlinkCommandSender::CommandLong make_command_focus_range(float range); + int32_t camera_id, + float top_left_x, + float top_left_y, + float bottom_right_x, + float bottom_right_y); + MavlinkCommandSender::CommandLong make_command_track_stop(int32_t camera_id); + MavlinkCommandSender::CommandLong make_command_focus_in(int32_t camera_id); + MavlinkCommandSender::CommandLong make_command_focus_out(int32_t camera_id); + 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(); - static bool starts_with(const std::string& str, const std::string& prefix); + void subscribe_to_param_changes_with_lock( + PotentialCamera& camera, std::string param_name, const ParamValue& param_value_type); + + uint16_t get_and_increment_capture_sequence(int32_t camera_id); + + std::vector potential_cameras_with_lock(); + 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); + + PotentialCamera* maybe_potential_camera_for_component_id_with_lock( + uint8_t component_id, uint8_t camera_device_id); + static std::string get_filename_from_path(const std::string& path); - static std::string strip_mavlinkftp_prefix(const std::string& str); - std::unique_ptr _camera_definition{}; - 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{}; + static uint8_t fixup_component_target(uint8_t component_id); - std::atomic _camera_id{0}; - std::atomic _camera_found{false}; + std::mutex _mutex; + std::vector _potential_cameras; + 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{}; - 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}; + CallEveryHandler::Cookie _check_potential_cameras_call_every_cookie{}; - CallbackList subscription_callbacks{}; - CallEveryHandler::Cookie call_every_cookie{}; - } _status{}; + std::optional _file_cache{}; + std::filesystem::path _tmp_download_path{}; - static constexpr double DEFAULT_TIMEOUT_S = 3.0; + CallbackList _storage_subscription_callbacks{}; - struct { - std::mutex mutex{}; - Camera::Mode data{}; - CallbackList subscription_callbacks{}; - CallEveryHandler::Cookie call_every_cookie{}; - } _mode{}; + CallEveryHandler::Cookie _request_slower_call_every_cookie{}; + CallEveryHandler::Cookie _request_faster_call_every_cookie{}; - struct { - std::mutex mutex{}; - int sequence = 1; // The MAVLink spec says the sequence starts at 1. - } _capture{}; + std::unique_ptr _http_loader{nullptr}; - 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 { - mutable std::mutex mutex{}; - Camera::Information data{}; - CallbackList subscription_callbacks{}; - } _information{}; - - 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; + bool _debugging{false}; }; } // namespace mavsdk diff --git a/src/mavsdk/plugins/camera/e90_unit_test.xml b/src/mavsdk/plugins/camera/e90_camera.xml similarity index 97% rename from src/mavsdk/plugins/camera/e90_unit_test.xml rename to src/mavsdk/plugins/camera/e90_camera.xml index d388142557..e7d3e4e7b1 100644 --- a/src/mavsdk/plugins/camera/e90_unit_test.xml +++ b/src/mavsdk/plugins/camera/e90_camera.xml @@ -1,6 +1,6 @@ - + E90 Yuneec @@ -61,7 +61,7 @@