From 2ccc4ae9e254508a555bb3d7df3dc67094f09dc8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 24 Mar 2018 15:54:51 +0100 Subject: [PATCH] camera: status, mode, capture, status, tests This adds more camera features and some integration tests for it. --- integration_tests/CMakeLists.txt | 5 + integration_tests/camera_mode.cpp | 66 +++ integration_tests/camera_take_photo.cpp | 106 +++++ .../camera_take_photo_interval.cpp | 85 ++++ integration_tests/camera_test_helpers.cpp | 65 +++ integration_tests/camera_test_helpers.h | 7 + plugins/camera/camera.cpp | 10 + plugins/camera/camera.h | 79 ++++ plugins/camera/camera_impl.cpp | 377 +++++++++++++++++- plugins/camera/camera_impl.h | 41 +- 10 files changed, 839 insertions(+), 2 deletions(-) create mode 100644 integration_tests/camera_mode.cpp create mode 100644 integration_tests/camera_take_photo.cpp create mode 100644 integration_tests/camera_take_photo_interval.cpp create mode 100644 integration_tests/camera_test_helpers.cpp create mode 100644 integration_tests/camera_test_helpers.h diff --git a/integration_tests/CMakeLists.txt b/integration_tests/CMakeLists.txt index f9787a888d..fbb75021ec 100644 --- a/integration_tests/CMakeLists.txt +++ b/integration_tests/CMakeLists.txt @@ -33,6 +33,10 @@ add_executable(integration_tests_runner gimbal.cpp transition_multicopter_fixedwing.cpp follow_me.cpp + camera_take_photo.cpp + camera_take_photo_interval.cpp + camera_mode.cpp + camera_test_helpers.cpp ) include_directories( @@ -54,6 +58,7 @@ target_link_libraries(integration_tests_runner dronecore_info dronecore_gimbal dronecore_follow_me + dronecore_camera gtest gtest_main gmock diff --git a/integration_tests/camera_mode.cpp b/integration_tests/camera_mode.cpp new file mode 100644 index 0000000000..1b9f0e7d40 --- /dev/null +++ b/integration_tests/camera_mode.cpp @@ -0,0 +1,66 @@ +#include +#include +#include "integration_test_helper.h" +#include "global_include.h" +#include "dronecore.h" +#include "camera_test_helpers.h" + + +using namespace dronecore; + + +TEST(CameraTest, ChangeMode) +{ + DroneCore dc; + + ConnectionResult ret = dc.add_udp_connection(); + ASSERT_EQ(ret, ConnectionResult::SUCCESS); + + // Wait for device to connect via heartbeat. + std::this_thread::sleep_for(std::chrono::seconds(2)); + + Device &device = dc.device(); + auto camera = std::make_shared(device); + + set_mode(camera, Camera::Mode::PHOTO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + Camera::Mode new_mode = get_mode(camera); + EXPECT_EQ(new_mode, Camera::Mode::PHOTO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + set_mode(camera, Camera::Mode::PHOTO_SURVEY); + std::this_thread::sleep_for(std::chrono::seconds(2)); + new_mode = get_mode(camera); + EXPECT_EQ(new_mode, Camera::Mode::PHOTO_SURVEY); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + set_mode(camera, Camera::Mode::VIDEO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + new_mode = get_mode(camera); + EXPECT_EQ(new_mode, Camera::Mode::VIDEO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + set_mode(camera, Camera::Mode::PHOTO_SURVEY); + std::this_thread::sleep_for(std::chrono::seconds(2)); + new_mode = get_mode(camera); + EXPECT_EQ(new_mode, Camera::Mode::PHOTO_SURVEY); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + set_mode(camera, Camera::Mode::PHOTO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + new_mode = get_mode(camera); + EXPECT_EQ(new_mode, Camera::Mode::PHOTO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + set_mode(camera, Camera::Mode::VIDEO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + new_mode = get_mode(camera); + EXPECT_EQ(new_mode, Camera::Mode::VIDEO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + set_mode(camera, Camera::Mode::PHOTO); + std::this_thread::sleep_for(std::chrono::seconds(2)); + new_mode = get_mode(camera); + EXPECT_EQ(new_mode, Camera::Mode::PHOTO); + std::this_thread::sleep_for(std::chrono::seconds(2)); +} diff --git a/integration_tests/camera_take_photo.cpp b/integration_tests/camera_take_photo.cpp new file mode 100644 index 0000000000..617141fc09 --- /dev/null +++ b/integration_tests/camera_take_photo.cpp @@ -0,0 +1,106 @@ +#include +#include +#include +#include +#include +#include "integration_test_helper.h" +#include "global_include.h" +#include "dronecore.h" +#include "camera_test_helpers.h" + +using namespace dronecore; +using namespace std::placeholders; // for `_1` + +static void receive_camera_result(Camera::Result result); + +static void receive_capture_info(Camera::CaptureInfo capture_info); + +static std::atomic _received_result {false}; +static std::atomic _received_capture_info {false}; + + +TEST(CameraTest, TakePhoto) +{ + DroneCore dc; + + ConnectionResult ret = dc.add_udp_connection(); + ASSERT_EQ(ret, ConnectionResult::SUCCESS); + + // Wait for device to connect via heartbeat. + std::this_thread::sleep_for(std::chrono::seconds(2)); + + Device &device = dc.device(); + auto camera = std::make_shared(device); + + // We want to take the picture in photo mode. + set_mode(camera, Camera::Mode::PHOTO); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + camera->capture_info_async(std::bind(&receive_capture_info, _1)); + + camera->take_photo_async(std::bind(&receive_camera_result, _1)); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + EXPECT_TRUE(_received_capture_info); +} + +TEST(CameraTest, TakeMultiplePhotos) +{ + DroneCore dc; + + const int num_photos_to_take = 3; + + ConnectionResult ret = dc.add_udp_connection(); + ASSERT_EQ(ret, ConnectionResult::SUCCESS); + + // Wait for device to connect via heartbeat. + std::this_thread::sleep_for(std::chrono::seconds(2)); + + Device &device = dc.device(); + auto camera = std::make_shared(device); + + // We want to take the picture in photo mode. + set_mode(camera, Camera::Mode::PHOTO); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + camera->capture_info_async(std::bind(&receive_capture_info, _1)); + + for (unsigned i = 0; i < num_photos_to_take; ++i) { + camera->take_photo_async(std::bind(&receive_camera_result, _1)); + LogDebug() << "taking picture: " << i; + std::this_thread::sleep_for(std::chrono::milliseconds(1500)); + EXPECT_TRUE(_received_capture_info); + _received_capture_info = false; + } +} + +void receive_camera_result(Camera::Result result) +{ + _received_result = true; + EXPECT_EQ(result, Camera::Result::SUCCESS); +} + +void receive_capture_info(Camera::CaptureInfo 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.quaternion.w << ", " + << capture_info.quaternion.x << ", " + << capture_info.quaternion.y << ", " + << capture_info.quaternion.z << "."; + LogInfo() << "Result: " + << (capture_info.success ? "success" : "fail") << "."; + LogInfo() << "Saved to " + << capture_info.file_url << " (" + << capture_info.index << ")."; + + _received_capture_info = true; +} diff --git a/integration_tests/camera_take_photo_interval.cpp b/integration_tests/camera_take_photo_interval.cpp new file mode 100644 index 0000000000..ab77d51f14 --- /dev/null +++ b/integration_tests/camera_take_photo_interval.cpp @@ -0,0 +1,85 @@ +#include +#include +#include +#include +#include +#include "integration_test_helper.h" +#include "global_include.h" +#include "dronecore.h" +#include "camera_test_helpers.h" + +using namespace dronecore; +using namespace std::placeholders; // for `_1` + +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) +{ + DroneCore dc; + + ConnectionResult ret = dc.add_udp_connection(); + ASSERT_EQ(ret, ConnectionResult::SUCCESS); + + // Wait for device to connect via heartbeat. + std::this_thread::sleep_for(std::chrono::seconds(2)); + + Device &device = dc.device(); + auto camera = std::make_shared(device); + + // We want to take the pictures in photo mode. + set_mode(camera, Camera::Mode::PHOTO); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + check_interval_on(camera, false); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + _received_result = false; + camera->start_photo_interval_async(2.0f, std::bind(&receive_camera_result, _1)); + + // 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(std::bind(&receive_camera_result, _1)); + + 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->get_status_async([prom, on](Camera::Result result, Camera::Status status) { + EXPECT_EQ(result, Camera::Result::SUCCESS); + 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(1)); + + EXPECT_EQ(status, std::future_status::ready); +} diff --git a/integration_tests/camera_test_helpers.cpp b/integration_tests/camera_test_helpers.cpp new file mode 100644 index 0000000000..1b38400156 --- /dev/null +++ b/integration_tests/camera_test_helpers.cpp @@ -0,0 +1,65 @@ +#include +#include +#include "integration_test_helper.h" +#include "camera_test_helpers.h" + +using namespace dronecore; + +Camera::Mode get_mode(std::shared_ptr camera) +{ + struct PromiseResult { + Camera::Result result; + Camera::Mode mode; + }; + + auto prom = std::make_shared>(); + auto ret = prom->get_future(); + + // Let's get the mode first + camera->get_mode_async([prom](Camera::Result result, Camera::Mode mode) { + PromiseResult pr {}; + pr.result = result; + pr.mode = mode; + prom->set_value(pr); + }); + + // Block now for a while to wait for result. + auto status = ret.wait_for(std::chrono::seconds(7)); + + EXPECT_EQ(status, std::future_status::ready); + + if (status == std::future_status::ready) { + PromiseResult new_ret = ret.get(); + EXPECT_EQ(new_ret.result, Camera::Result::SUCCESS); + EXPECT_NE(new_ret.mode, Camera::Mode::UNKNOWN); + return new_ret.mode; + } else { + return Camera::Mode::UNKNOWN; + } +} + +void set_mode(std::shared_ptr camera, Camera::Mode mode) +{ + // FIXME: this should not be required. + std::this_thread::sleep_for(std::chrono::seconds(1)); + + auto prom = std::make_shared>(); + auto ret = prom->get_future(); + + // Let's get the mode first + camera->set_mode_async(mode, [mode, prom](Camera::Result result, + Camera::Mode mode_got) { + EXPECT_EQ(result, Camera::Result::SUCCESS); + EXPECT_EQ(mode, mode_got); + prom->set_value(); + }); + + // Block now for a while to wait for result. + auto status = ret.wait_for(std::chrono::seconds(10)); + + EXPECT_EQ(status, std::future_status::ready); + + if (status == std::future_status::ready) { + ret.get(); + } +} diff --git a/integration_tests/camera_test_helpers.h b/integration_tests/camera_test_helpers.h new file mode 100644 index 0000000000..2c67ed6bc2 --- /dev/null +++ b/integration_tests/camera_test_helpers.h @@ -0,0 +1,7 @@ +#pragma once + +#include "dronecore.h" +#include "plugins/camera/camera.h" + +dronecore::Camera::Mode get_mode(std::shared_ptr camera); +void set_mode(std::shared_ptr camera, dronecore::Camera::Mode mode); diff --git a/plugins/camera/camera.cpp b/plugins/camera/camera.cpp index 648eec6534..b0eadb5c40 100644 --- a/plugins/camera/camera.cpp +++ b/plugins/camera/camera.cpp @@ -73,6 +73,16 @@ void Camera::get_mode_async(const mode_callback_t &callback) _impl->get_mode_async(callback); } +void Camera::get_status_async(get_status_callback_t callback) +{ + _impl->get_status_async(callback); +} + +void Camera::capture_info_async(capture_info_callback_t callback) +{ + _impl->capture_info_async(callback); +} + const char *Camera::result_str(Result result) { switch (result) { diff --git a/plugins/camera/camera.h b/plugins/camera/camera.h index 1fa37b1a40..f501a0c4a9 100644 --- a/plugins/camera/camera.h +++ b/plugins/camera/camera.h @@ -185,6 +185,85 @@ class Camera : public PluginBase */ void get_mode_async(const mode_callback_t &callback); + /** + * @brief Information about a picture just captured. + */ + struct CaptureInfo { + /** + * @brief Position type. + */ + struct Position { + double latitude_deg; /**< @brief Latitude in degrees (range: -90 to +90) */ + double longitude_deg; /**< @brief Longitude in degrees (range: -180 to 180) */ + float absolute_altitude_m; /**< @brief Altitude AMSL (above mean sea level) in metres */ + float relative_altitude_m; /**< @brief Altitude relative to takeoff altitude in metres */ + } position; /**< @brief Position of drone/camera when image was captured. */ + + /** + * @brief Quaternion type. + * + * All rotations and axis systems follow the right-hand rule. + * The Hamilton quaternion product definition is used. + * A zero-rotation quaternion is represented by (1,0,0,0). + * The quaternion could also be written as w + xi + yj + zk. + * + * For more info see: https://en.wikipedia.org/wiki/Quaternion + */ + struct Quaternion { + float w; /**< @brief Quaternion entry 0 also denoted as a. */ + float x; /**< @brief Quaternion entry 1 also denoted as b. */ + float y; /**< @brief Quaternion entry 2 also denoted as c. */ + float z; /**< @brief Quaternion entry 3 also denoted as d. */ + } quaternion; /**< @brief Quaternion of camera orientation. */ + + uint64_t time_utc_us; /**< @brief timestamp in UTC (since UNIX epoch) in microseconds. */ + bool success; /**< @brief True if capture was successful. */ + int index; /**< @brief Zero-based index of this image since armed. */ + std::string file_url; /**< URL of image taken to download. */ + }; + + /** + * @brief Callback type for capture info updates. + */ + typedef std::function capture_info_callback_t; + + /** + * @brief Subscribe to capture info updates (asynchronous). + * + * @param callback Function to call with updates. + */ + void capture_info_async(capture_info_callback_t callback); + + /** + * @brief Information about camera status. + */ + struct Status { + bool video_on; + bool photo_interval_on; + + enum class StorageStatus { + NOT_AVAILABLE, + UNFORMATTED, + FORMATTED + } storage_status; + float used_storage_mib; + float available_storage_mib; + float total_storage_mib; + }; + + /** + * @brief Callback type to get status. + */ + typedef std::function get_status_callback_t; + + + /** + * @brief Get camera status. + * + * @param callback Function to call with camera status. + */ + void get_status_async(get_status_callback_t callback); + /** * @brief Copy constructor (object is not copyable). */ diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index a938af9588..f480ec79b7 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -6,6 +6,8 @@ namespace dronecore { +using namespace std::placeholders; // for `_1` + CameraImpl::CameraImpl(Device &device) : PluginImplBase(device) { @@ -17,7 +19,28 @@ CameraImpl::~CameraImpl() _parent.unregister_plugin(this); } -void CameraImpl::init() {} +void CameraImpl::init() +{ + _parent.register_mavlink_message_handler( + MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, + std::bind(&CameraImpl::process_camera_capture_status, this, _1), + this); + + _parent.register_mavlink_message_handler( + MAVLINK_MSG_ID_STORAGE_INFORMATION, + std::bind(&CameraImpl::process_storage_information, this, _1), + this); + + _parent.register_mavlink_message_handler( + MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, + std::bind(&CameraImpl::process_camera_image_captured, this, _1), + this); + + _parent.register_mavlink_message_handler( + MAVLINK_MSG_ID_CAMERA_SETTINGS, + std::bind(&CameraImpl::process_camera_settings, this, _1), + this); +} void CameraImpl::deinit() {} @@ -77,6 +100,87 @@ Camera::Result CameraImpl::start_video() MAV_COMP_ID_CAMERA)); } +Camera::Result CameraImpl::stop_video() +{ + // TODO: add camera ID + return camera_result_from_command_result( + _parent.send_command_with_ack( + MAV_CMD_VIDEO_STOP_CAPTURE, + MavlinkCommands::Params {0.0f, // all camera IDs + NAN, NAN, NAN, NAN, NAN, NAN}, + MAV_COMP_ID_CAMERA)); +} + +void CameraImpl::take_photo_async(const Camera::result_callback_t &callback) +{ + // TODO: add camera ID + _parent.send_command_with_ack_async( + MAV_CMD_IMAGE_START_CAPTURE, + MavlinkCommands::Params {0.0f, // all camera IDs + 0.0f, // no interval for one picture + 1.0f, // take only one picture + float(_capture_sequence++), + NAN, NAN, NAN}, + std::bind(&CameraImpl::receive_command_result, std::placeholders::_1, callback), + MAV_COMP_ID_CAMERA); + +} + +void CameraImpl::start_photo_interval_async(float interval_s, + const Camera::result_callback_t &callback) +{ + if (!interval_valid(interval_s)) { + if (callback) { + callback(Camera::Result::WRONG_ARGUMENT); + } + return; + } + + // TODO: add camera ID + _parent.send_command_with_ack_async( + MAV_CMD_IMAGE_START_CAPTURE, + MavlinkCommands::Params {0.0f, // all camera IDs + interval_s, + 0.0f, // unlimited photos + float(_capture_sequence++), + NAN, NAN, NAN}, + std::bind(&CameraImpl::receive_command_result, std::placeholders::_1, callback), + MAV_COMP_ID_CAMERA); +} + +void CameraImpl::stop_photo_interval_async(const Camera::result_callback_t &callback) +{ + // TODO: add camera ID + _parent.send_command_with_ack_async( + MAV_CMD_IMAGE_STOP_CAPTURE, + MavlinkCommands::Params {0.0f, // all camera IDs + NAN, NAN, NAN, NAN, NAN, NAN}, + std::bind(&CameraImpl::receive_command_result, std::placeholders::_1, callback), + MAV_COMP_ID_CAMERA); +} + +void CameraImpl::start_video_async(const Camera::result_callback_t &callback) +{ + _parent.send_command_with_ack_async( + MAV_CMD_VIDEO_START_CAPTURE, + MavlinkCommands::Params {0.0f, // all camera IDs + NAN, // fps not set yet + NAN, // resolution not set yet + NAN, NAN, NAN, NAN}, + std::bind(&CameraImpl::receive_command_result, std::placeholders::_1, callback), + MAV_COMP_ID_CAMERA); +} + +void CameraImpl::stop_video_async(const Camera::result_callback_t &callback) +{ + _parent.send_command_with_ack_async( + MAV_CMD_VIDEO_STOP_CAPTURE, + MavlinkCommands::Params {0.0f, // all camera IDs + NAN, NAN, NAN, NAN, NAN, NAN}, + std::bind(&CameraImpl::receive_command_result, std::placeholders::_1, callback), + MAV_COMP_ID_CAMERA); +} + Camera::Result CameraImpl::camera_result_from_command_result(MavlinkCommands::Result command_result) { @@ -129,6 +233,30 @@ void CameraImpl::set_mode_async(Camera::Mode mode, const Camera::mode_callback_t MAV_COMP_ID_CAMERA); } +void CameraImpl::get_mode_async(Camera::mode_callback_t callback) +{ + std::lock_guard lock(_get_mode.mutex); + if (_get_mode.callback != nullptr) { + if (callback) { + callback(Camera::Result::BUSY, Camera::Mode::UNKNOWN); + } + } + _get_mode.callback = callback; + + // TODO: we should probably listen for the command result instead of + // just the camera_mode. + _parent.send_command_with_ack_async( + MAV_CMD_REQUEST_CAMERA_SETTINGS, + MavlinkCommands::Params {1.0, // Request it + NAN, NAN, NAN, NAN, NAN, NAN // Reserved + }, + nullptr, + MAV_COMP_ID_CAMERA); + + _parent.register_timeout_handler( + std::bind(&CameraImpl::receive_get_mode_timeout, this), 1.0, &_timeout_cookie); +} + bool CameraImpl::interval_valid(float interval_s) { // Reject everything faster than 1000 Hz, as well as negative inputs. @@ -140,6 +268,243 @@ bool CameraImpl::interval_valid(float interval_s) } } +void CameraImpl::get_status_async(Camera::get_status_callback_t callback) +{ + if (callback == nullptr) { + LogDebug() << "get_status_async called with empty callback"; + return; + } + + { + std::lock_guard lock(_last_status.mutex); + // If we're already trying to get the status, we need to wait. + if (_last_status.callback != nullptr) { + if (callback) { + Camera::Status empty_status = {}; + callback(Camera::Result::IN_PROGRESS, empty_status); + return; + } + } + // Let's create a subscription for the (hopefully) incoming messages. + _last_status.callback = callback; + } + + _parent.send_command_with_ack_async( + MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS, + MavlinkCommands::Params {1.0f, // request it + NAN, NAN, NAN, NAN, NAN, NAN}, + std::bind(&CameraImpl::receive_camera_capture_status_result, this, _1), + MAV_COMP_ID_CAMERA); + + _parent.send_command_with_ack_async( + MAV_CMD_REQUEST_STORAGE_INFORMATION, + MavlinkCommands::Params {0.0f, // Reserved + 1.0f, // request it + NAN, NAN, NAN, NAN, NAN}, + std::bind(&CameraImpl::receive_storage_information_result, this, _1), + MAV_COMP_ID_CAMERA); + + // We would like to complete this status within 3s, TODO: remove this long timeout. + _parent.register_timeout_handler(std::bind(&CameraImpl::status_timeout_happened, this), + DEFAULT_TIMEOUT_S * 3.0, &_timeout_cookie); +} + +void CameraImpl::receive_camera_capture_status_result(MavlinkCommands::Result result) +{ + std::lock_guard lock(_last_status.mutex); + if (_last_status.callback == nullptr) { + // We're not expecting this message, let's ignore it. + return; + } + + if (result != MavlinkCommands::Result::SUCCESS) { + if (_last_status.callback) { + Camera::Status empty_status = {}; + _last_status.callback(camera_result_from_command_result(result), empty_status); + } + // Something went wrong, we give up. + _last_status.callback = nullptr; + } + + // FIXME: doesn't this interfere with other timeout handlers? + _parent.refresh_timeout_handler(this); +} + +void CameraImpl::capture_info_async(Camera::capture_info_callback_t callback) +{ + _capture_info_callback = callback; +} + +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); + + { + std::lock_guard lock(_last_status.mutex); + _last_status.data.video_on = (camera_capture_status.video_status == 1); + _last_status.data.photo_interval_on = (camera_capture_status.image_status == 2 || + camera_capture_status.image_status == 3); + _last_status.received_camera_capture_status = true; + } + + check_last_status(); +} + +void CameraImpl::process_storage_information(const mavlink_message_t &message) +{ + mavlink_storage_information_t storage_information; + mavlink_msg_storage_information_decode(&message, &storage_information); + + { + std::lock_guard lock(_last_status.mutex); + if (storage_information.status == 0) { + _last_status.data.storage_status = Camera::Status::StorageStatus::NOT_AVAILABLE; + } else if (storage_information.status == 1) { + _last_status.data.storage_status = Camera::Status::StorageStatus::UNFORMATTED; + } else if (storage_information.status == 2) { + _last_status.data.storage_status = Camera::Status::StorageStatus::FORMATTED; + } + _last_status.data.available_storage_mib = storage_information.available_capacity; + _last_status.data.used_storage_mib = storage_information.used_capacity; + _last_status.data.total_storage_mib = storage_information.total_capacity; + _last_status.received_storage_information = true; + } + + check_last_status(); +} + +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); + + { + if (_capture_info_callback) { + 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.quaternion.w = image_captured.q[0]; + capture_info.quaternion.x = image_captured.q[1]; + capture_info.quaternion.y = image_captured.q[2]; + capture_info.quaternion.z = image_captured.q[3]; + capture_info.file_url = std::string(image_captured.file_url); + capture_info.success = (image_captured.capture_result == 1); + capture_info.index = image_captured.image_index; + _capture_info_callback(capture_info); + } + } +} + +void CameraImpl::process_camera_settings(const mavlink_message_t &message) +{ + std::lock_guard lock(_get_mode.mutex); + if (_get_mode.callback == nullptr) { + // It seems that we are not interested. + return; + } + + mavlink_camera_settings_t camera_settings; + mavlink_msg_camera_settings_decode(&message, &camera_settings); + + Camera::Mode mode; + switch (camera_settings.mode_id) { + case CAMERA_MODE_IMAGE: + mode = Camera::Mode::PHOTO; + break; + case CAMERA_MODE_VIDEO: + mode = Camera::Mode::VIDEO; + break; + case CAMERA_MODE_IMAGE_SURVEY: + mode = Camera::Mode::PHOTO_SURVEY; + break; + default: + mode = Camera::Mode::UNKNOWN; + break; + } + + + //if (_camera_definition) { + // // This "parameter" needs to be manually set. + + // CameraDefinition::ParameterValue value; + // value.value.as_uint32 = camera_settings.mode_id; + // _camera_definition->update_setting("CAM_MODE", value); + //} + + _get_mode.callback(Camera::Result::SUCCESS, mode); + _get_mode.callback = nullptr; + + // FIXME: this will interfere with others. + _parent.unregister_timeout_handler(_timeout_cookie); +} + +void CameraImpl::check_last_status() +{ + std::lock_guard lock(_last_status.mutex); + if (_last_status.received_camera_capture_status && + _last_status.received_storage_information) { + + if (_last_status.callback) { + _last_status.callback(Camera::Result::SUCCESS, _last_status.data); + _last_status.callback = nullptr; + _last_status.received_camera_capture_status = false; + _last_status.received_storage_information = false; + // TODO: we need to make sure not to interfere with other timeouts + _parent.unregister_timeout_handler(_timeout_cookie); + } + } +} + +void CameraImpl::status_timeout_happened() +{ + std::lock_guard lock(_last_status.mutex); + + if (_last_status.callback) { + // Send empty settings with timeout error. + Camera::Status empty_status = {}; + _last_status.callback(Camera::Result::TIMEOUT, empty_status); + // Then give up. + } + // Discard the subscription + _last_status.callback = nullptr; +} + +void CameraImpl::receive_command_result(MavlinkCommands::Result command_result, + const Camera::result_callback_t &callback) +{ + Camera::Result camera_result = camera_result_from_command_result(command_result); + + if (callback) { + callback(camera_result); + } +} + + +void CameraImpl::receive_storage_information_result(MavlinkCommands::Result result) +{ + std::lock_guard lock(_last_status.mutex); + if (_last_status.callback == nullptr) { + // We're not expecting this message, let's ignore it. + return; + } + + if (result != MavlinkCommands::Result::SUCCESS) { + if (_last_status.callback) { + Camera::Status empty_status = {}; + _last_status.callback(camera_result_from_command_result(result), empty_status); + } + // Something went wrong, we give up. + _last_status.callback = nullptr; + } + + // TODO: decode and save values + _parent.refresh_timeout_handler(this); +} + void CameraImpl::receive_set_mode_command_result(MavlinkCommands::Result command_result, const Camera::mode_callback_t &callback, Camera::Mode mode) @@ -151,4 +516,14 @@ void CameraImpl::receive_set_mode_command_result(MavlinkCommands::Result command } } +void CameraImpl::receive_get_mode_timeout() +{ + std::lock_guard lock(_get_mode.mutex); + if (_get_mode.callback) { + _get_mode.callback(Camera::Result::TIMEOUT, Camera::Mode::UNKNOWN); + _get_mode.callback = nullptr; + } +} + + } // namespace dronecore diff --git a/plugins/camera/camera_impl.h b/plugins/camera/camera_impl.h index a813f60171..862593a788 100644 --- a/plugins/camera/camera_impl.h +++ b/plugins/camera/camera_impl.h @@ -35,24 +35,63 @@ class CameraImpl : public PluginImplBase void stop_video_async(const Camera::result_callback_t &callback); void set_mode_async(Camera::Mode mode, const Camera::mode_callback_t &callback); - void get_mode_async(const Camera::mode_callback_t &callback); + void get_mode_async(Camera::mode_callback_t callback); + + void capture_info_async(Camera::capture_info_callback_t callback); + + void get_status_async(Camera::get_status_callback_t callback); // Non-copyable CameraImpl(const CameraImpl &) = delete; const CameraImpl &operator=(const CameraImpl &) = delete; private: + struct { + std::mutex mutex {}; + Camera::get_status_callback_t callback {nullptr}; + Camera::Status data {}; + bool received_camera_capture_status {false}; + bool received_storage_information {false}; + } _last_status; + + struct { + std::mutex mutex {}; + Camera::mode_callback_t callback {nullptr}; + } _get_mode; + void receive_set_mode_command_result(MavlinkCommands::Result command_result, const Camera::mode_callback_t &callback, Camera::Mode mode); + void receive_get_mode_timeout(); + static Camera::Result camera_result_from_command_result( MavlinkCommands::Result command_result); + static void receive_command_result(MavlinkCommands::Result command_result, + const Camera::result_callback_t &callback); + static bool interval_valid(float interval_s); + 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); + void process_camera_settings(const mavlink_message_t &message); + void receive_storage_information_result(MavlinkCommands::Result result); + + void receive_camera_capture_status_result(MavlinkCommands::Result result); + + void check_last_status(); + + void status_timeout_happened(); + + static constexpr double DEFAULT_TIMEOUT_S = 3.0; + int _capture_sequence = 0; + Camera::capture_info_callback_t _capture_info_callback {nullptr}; + + void *_timeout_cookie = nullptr; };