diff --git a/src/mavsdk/plugins/camera/camera_impl.cpp b/src/mavsdk/plugins/camera/camera_impl.cpp index 4581bada39..83e314b58a 100644 --- a/src/mavsdk/plugins/camera/camera_impl.cpp +++ b/src/mavsdk/plugins/camera/camera_impl.cpp @@ -956,37 +956,58 @@ void CameraImpl::process_storage_information(const mavlink_message_t& message) { std::lock_guard lock(_status.mutex); - switch (storage_information.status) { - case STORAGE_STATUS_EMPTY: - _status.data.storage_status = Camera::Status::StorageStatus::NotAvailable; - break; - case STORAGE_STATUS_UNFORMATTED: - _status.data.storage_status = Camera::Status::StorageStatus::Unformatted; - break; - case STORAGE_STATUS_READY: - _status.data.storage_status = Camera::Status::StorageStatus::Formatted; - break; - case STORAGE_STATUS_NOT_SUPPORTED: - _status.data.storage_status = Camera::Status::StorageStatus::NotSupported; - break; - default: - _status.data.storage_status = Camera::Status::StorageStatus::NotSupported; - LogErr() << "Unknown storage status received."; - break; - } - + _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 = - static_cast(storage_information.type); + _status.data.storage_type = storage_type_from_mavlink(storage_information.type); _status.received_storage_information = true; } check_status(); } +Camera::Status::StorageStatus +CameraImpl::storage_status_from_mavlink(const int storage_status) const +{ + switch (storage_status) { + case STORAGE_STATUS_EMPTY: + return Camera::Status::StorageStatus::NotAvailable; + case STORAGE_STATUS_UNFORMATTED: + return Camera::Status::StorageStatus::Unformatted; + case STORAGE_STATUS_READY: + return Camera::Status::StorageStatus::Formatted; + break; + case STORAGE_STATUS_NOT_SUPPORTED: + return Camera::Status::StorageStatus::NotSupported; + default: + LogErr() << "Unknown storage status received."; + return Camera::Status::StorageStatus::NotSupported; + } +} + +Camera::Status::StorageType CameraImpl::storage_type_from_mavlink(const int storage_type) const +{ + switch (storage_type) { + default: + LogErr() << "Unknown storage_type enum value: " << storage_type; + // FALLTHROUGH + case STORAGE_TYPE_UNKNOWN: + return mavsdk::Camera::Status::StorageType::Unknown; + case STORAGE_TYPE_USB_STICK: + return mavsdk::Camera::Status::StorageType::UsbStick; + case STORAGE_TYPE_SD: + return mavsdk::Camera::Status::StorageType::Sd; + case STORAGE_TYPE_MICROSD: + return mavsdk::Camera::Status::StorageType::Microsd; + case STORAGE_TYPE_HD: + return mavsdk::Camera::Status::StorageType::Hd; + case STORAGE_TYPE_OTHER: + return mavsdk::Camera::Status::StorageType::Other; + } +} + void CameraImpl::process_camera_image_captured(const mavlink_message_t& message) { mavlink_camera_image_captured_t image_captured; diff --git a/src/mavsdk/plugins/camera/camera_impl.h b/src/mavsdk/plugins/camera/camera_impl.h index 3f3789e36e..8fd922bda7 100644 --- a/src/mavsdk/plugins/camera/camera_impl.h +++ b/src/mavsdk/plugins/camera/camera_impl.h @@ -145,6 +145,9 @@ class CameraImpl : public PluginImplBase { void process_flight_information(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; + Camera::EulerAngle to_euler_angle_from_quaternion(Camera::Quaternion quaternion); void notify_mode();