From e588f341edfa208743e1202382ddf0929bff13be Mon Sep 17 00:00:00 2001 From: Narr the Reg Date: Tue, 7 Nov 2023 11:02:13 -0600 Subject: [PATCH] service: irs: Implement moment image processor --- src/core/hle/service/hid/irs.cpp | 2 +- .../hid/irsensor/clustering_processor.cpp | 16 ++- .../hid/irsensor/clustering_processor.h | 9 +- .../hid/irsensor/image_transfer_processor.cpp | 2 +- .../service/hid/irsensor/moment_processor.cpp | 123 +++++++++++++++++- .../service/hid/irsensor/moment_processor.h | 34 ++++- 6 files changed, 169 insertions(+), 17 deletions(-) diff --git a/src/core/hle/service/hid/irs.cpp b/src/core/hle/service/hid/irs.cpp index 221c33b860..d383a266d9 100644 --- a/src/core/hle/service/hid/irs.cpp +++ b/src/core/hle/service/hid/irs.cpp @@ -138,7 +138,7 @@ void IRS::RunMomentProcessor(HLERequestContext& ctx) { if (result.IsSuccess()) { auto& device = GetIrCameraSharedMemoryDeviceEntry(parameters.camera_handle); - MakeProcessor(parameters.camera_handle, device); + MakeProcessorWithCoreContext(parameters.camera_handle, device); auto& image_transfer_processor = GetProcessor(parameters.camera_handle); image_transfer_processor.SetConfig(parameters.processor_config); npad_device->SetPollingMode(Core::HID::EmulatedDeviceIndex::RightIndex, diff --git a/src/core/hle/service/hid/irsensor/clustering_processor.cpp b/src/core/hle/service/hid/irsensor/clustering_processor.cpp index e2f4ae876c..c559eb0d5f 100644 --- a/src/core/hle/service/hid/irsensor/clustering_processor.cpp +++ b/src/core/hle/service/hid/irsensor/clustering_processor.cpp @@ -3,16 +3,18 @@ #include +#include "core/core.h" +#include "core/core_timing.h" #include "core/hid/emulated_controller.h" #include "core/hid/hid_core.h" #include "core/hle/service/hid/irsensor/clustering_processor.h" namespace Service::IRS { -ClusteringProcessor::ClusteringProcessor(Core::HID::HIDCore& hid_core_, +ClusteringProcessor::ClusteringProcessor(Core::System& system_, Core::IrSensor::DeviceFormat& device_format, std::size_t npad_index) - : device{device_format} { - npad_device = hid_core_.GetEmulatedControllerByIndex(npad_index); + : device{device_format}, system{system_} { + npad_device = system.HIDCore().GetEmulatedControllerByIndex(npad_index); device.mode = Core::IrSensor::IrSensorMode::ClusteringProcessor; device.camera_status = Core::IrSensor::IrCameraStatus::Unconnected; @@ -48,7 +50,7 @@ void ClusteringProcessor::OnControllerUpdate(Core::HID::ControllerTriggerType ty } next_state = {}; - const auto camera_data = npad_device->GetCamera(); + const auto& camera_data = npad_device->GetCamera(); auto filtered_image = camera_data.data; RemoveLowIntensityData(filtered_image); @@ -83,7 +85,7 @@ void ClusteringProcessor::OnControllerUpdate(Core::HID::ControllerTriggerType ty } next_state.sampling_number = camera_data.sample; - next_state.timestamp = next_state.timestamp + 131; + next_state.timestamp = system.CoreTiming().GetGlobalTimeNs().count(); next_state.ambient_noise_level = Core::IrSensor::CameraAmbientNoiseLevel::Low; shared_memory->clustering_lifo.WriteNextEntry(next_state); @@ -202,14 +204,14 @@ ClusteringProcessor::ClusteringData ClusteringProcessor::MergeCluster( } u8 ClusteringProcessor::GetPixel(const std::vector& data, std::size_t x, std::size_t y) const { - if ((y * width) + x > data.size()) { + if ((y * width) + x >= data.size()) { return 0; } return data[(y * width) + x]; } void ClusteringProcessor::SetPixel(std::vector& data, std::size_t x, std::size_t y, u8 value) { - if ((y * width) + x > data.size()) { + if ((y * width) + x >= data.size()) { return; } data[(y * width) + x] = value; diff --git a/src/core/hle/service/hid/irsensor/clustering_processor.h b/src/core/hle/service/hid/irsensor/clustering_processor.h index dc01a8ea78..83f34734a7 100644 --- a/src/core/hle/service/hid/irsensor/clustering_processor.h +++ b/src/core/hle/service/hid/irsensor/clustering_processor.h @@ -8,6 +8,10 @@ #include "core/hle/service/hid/irs_ring_lifo.h" #include "core/hle/service/hid/irsensor/processor_base.h" +namespace Core { +class System; +} + namespace Core::HID { class EmulatedController; } // namespace Core::HID @@ -15,8 +19,7 @@ class EmulatedController; namespace Service::IRS { class ClusteringProcessor final : public ProcessorBase { public: - explicit ClusteringProcessor(Core::HID::HIDCore& hid_core_, - Core::IrSensor::DeviceFormat& device_format, + explicit ClusteringProcessor(Core::System& system_, Core::IrSensor::DeviceFormat& device_format, std::size_t npad_index); ~ClusteringProcessor() override; @@ -106,5 +109,7 @@ private: Core::IrSensor::DeviceFormat& device; Core::HID::EmulatedController* npad_device; int callback_key{}; + + Core::System& system; }; } // namespace Service::IRS diff --git a/src/core/hle/service/hid/irsensor/image_transfer_processor.cpp b/src/core/hle/service/hid/irsensor/image_transfer_processor.cpp index 803a6277c1..22067a591e 100644 --- a/src/core/hle/service/hid/irsensor/image_transfer_processor.cpp +++ b/src/core/hle/service/hid/irsensor/image_transfer_processor.cpp @@ -49,7 +49,7 @@ void ImageTransferProcessor::OnControllerUpdate(Core::HID::ControllerTriggerType return; } - const auto camera_data = npad_device->GetCamera(); + const auto& camera_data = npad_device->GetCamera(); // This indicates how much ambient light is present processor_state.ambient_noise_level = Core::IrSensor::CameraAmbientNoiseLevel::Low; diff --git a/src/core/hle/service/hid/irsensor/moment_processor.cpp b/src/core/hle/service/hid/irsensor/moment_processor.cpp index dbaca420ad..cf045bda7c 100644 --- a/src/core/hle/service/hid/irsensor/moment_processor.cpp +++ b/src/core/hle/service/hid/irsensor/moment_processor.cpp @@ -1,24 +1,137 @@ // SPDX-FileCopyrightText: Copyright 2022 yuzu Emulator Project // SPDX-License-Identifier: GPL-3.0-or-later +#include "core/core.h" +#include "core/core_timing.h" +#include "core/hid/emulated_controller.h" +#include "core/hid/hid_core.h" #include "core/hle/service/hid/irsensor/moment_processor.h" namespace Service::IRS { -MomentProcessor::MomentProcessor(Core::IrSensor::DeviceFormat& device_format) - : device(device_format) { +static constexpr auto format = Core::IrSensor::ImageTransferProcessorFormat::Size40x30; +static constexpr std::size_t ImageWidth = 40; +static constexpr std::size_t ImageHeight = 30; + +MomentProcessor::MomentProcessor(Core::System& system_, Core::IrSensor::DeviceFormat& device_format, + std::size_t npad_index) + : device(device_format), system{system_} { + npad_device = system.HIDCore().GetEmulatedControllerByIndex(npad_index); + device.mode = Core::IrSensor::IrSensorMode::MomentProcessor; device.camera_status = Core::IrSensor::IrCameraStatus::Unconnected; device.camera_internal_status = Core::IrSensor::IrCameraInternalStatus::Stopped; + + shared_memory = std::construct_at( + reinterpret_cast(&device_format.state.processor_raw_data)); + + Core::HID::ControllerUpdateCallback engine_callback{ + .on_change = [this](Core::HID::ControllerTriggerType type) { OnControllerUpdate(type); }, + .is_npad_service = true, + }; + callback_key = npad_device->SetCallback(engine_callback); } -MomentProcessor::~MomentProcessor() = default; +MomentProcessor::~MomentProcessor() { + npad_device->DeleteCallback(callback_key); +}; -void MomentProcessor::StartProcessor() {} +void MomentProcessor::StartProcessor() { + device.camera_status = Core::IrSensor::IrCameraStatus::Available; + device.camera_internal_status = Core::IrSensor::IrCameraInternalStatus::Ready; +} void MomentProcessor::SuspendProcessor() {} void MomentProcessor::StopProcessor() {} +void MomentProcessor::OnControllerUpdate(Core::HID::ControllerTriggerType type) { + if (type != Core::HID::ControllerTriggerType::IrSensor) { + return; + } + + next_state = {}; + const auto& camera_data = npad_device->GetCamera(); + + const auto window_width = static_cast(current_config.window_of_interest.width); + const auto window_height = static_cast(current_config.window_of_interest.height); + const auto window_start_x = static_cast(current_config.window_of_interest.x); + const auto window_start_y = static_cast(current_config.window_of_interest.y); + + const std::size_t block_width = window_width / Columns; + const std::size_t block_height = window_height / Rows; + + for (std::size_t row = 0; row < Rows; row++) { + for (std::size_t column = 0; column < Columns; column++) { + const size_t x_pos = (column * block_width) + window_start_x; + const size_t y_pos = (row * block_height) + window_start_y; + auto& statistic = next_state.statistic[column + (row * Columns)]; + statistic = GetStatistic(camera_data.data, x_pos, y_pos, block_width, block_height); + } + } + + next_state.sampling_number = camera_data.sample; + next_state.timestamp = system.CoreTiming().GetGlobalTimeNs().count(); + next_state.ambient_noise_level = Core::IrSensor::CameraAmbientNoiseLevel::Low; + shared_memory->moment_lifo.WriteNextEntry(next_state); + + if (!IsProcessorActive()) { + StartProcessor(); + } +} + +u8 MomentProcessor::GetPixel(const std::vector& data, std::size_t x, std::size_t y) const { + if ((y * ImageWidth) + x >= data.size()) { + return 0; + } + return data[(y * ImageWidth) + x]; +} + +MomentProcessor::MomentStatistic MomentProcessor::GetStatistic(const std::vector& data, + std::size_t start_x, + std::size_t start_y, + std::size_t width, + std::size_t height) const { + // The actual implementation is always 320x240 + static constexpr std::size_t RealWidth = 320; + static constexpr std::size_t RealHeight = 240; + static constexpr std::size_t Threshold = 30; + MomentStatistic statistic{}; + std::size_t active_points{}; + + // Sum all data points on the block that meet with the threshold + for (std::size_t y = 0; y < width; y++) { + for (std::size_t x = 0; x < height; x++) { + const size_t x_pos = x + start_x; + const size_t y_pos = y + start_y; + const auto pixel = + GetPixel(data, x_pos * ImageWidth / RealWidth, y_pos * ImageHeight / RealHeight); + + if (pixel < Threshold) { + continue; + } + + statistic.average_intensity += pixel; + + statistic.centroid.x += static_cast(x_pos); + statistic.centroid.y += static_cast(y_pos); + + active_points++; + } + } + + // Return an empty field if no points were available + if (active_points == 0) { + return {}; + } + + // Finally calculate the actual centroid and average intensity + statistic.centroid.x /= static_cast(active_points); + statistic.centroid.y /= static_cast(active_points); + statistic.average_intensity /= static_cast(width * height); + + return statistic; +} + void MomentProcessor::SetConfig(Core::IrSensor::PackedMomentProcessorConfig config) { current_config.camera_config.exposure_time = config.camera_config.exposure_time; current_config.camera_config.gain = config.camera_config.gain; @@ -29,6 +142,8 @@ void MomentProcessor::SetConfig(Core::IrSensor::PackedMomentProcessorConfig conf current_config.preprocess = static_cast(config.preprocess); current_config.preprocess_intensity_threshold = config.preprocess_intensity_threshold; + + npad_device->SetCameraFormat(format); } } // namespace Service::IRS diff --git a/src/core/hle/service/hid/irsensor/moment_processor.h b/src/core/hle/service/hid/irsensor/moment_processor.h index d4bd22e0f2..398cfbdc10 100644 --- a/src/core/hle/service/hid/irsensor/moment_processor.h +++ b/src/core/hle/service/hid/irsensor/moment_processor.h @@ -6,12 +6,22 @@ #include "common/bit_field.h" #include "common/common_types.h" #include "core/hid/irs_types.h" +#include "core/hle/service/hid/irs_ring_lifo.h" #include "core/hle/service/hid/irsensor/processor_base.h" +namespace Core { +class System; +} + +namespace Core::HID { +class EmulatedController; +} // namespace Core::HID + namespace Service::IRS { class MomentProcessor final : public ProcessorBase { public: - explicit MomentProcessor(Core::IrSensor::DeviceFormat& device_format); + explicit MomentProcessor(Core::System& system_, Core::IrSensor::DeviceFormat& device_format, + std::size_t npad_index); ~MomentProcessor() override; // Called when the processor is initialized @@ -27,6 +37,9 @@ public: void SetConfig(Core::IrSensor::PackedMomentProcessorConfig config); private: + static constexpr std::size_t Columns = 8; + static constexpr std::size_t Rows = 6; + // This is nn::irsensor::MomentProcessorConfig struct MomentProcessorConfig { Core::IrSensor::CameraConfig camera_config; @@ -50,12 +63,29 @@ private: u64 timestamp; Core::IrSensor::CameraAmbientNoiseLevel ambient_noise_level; INSERT_PADDING_BYTES(4); - std::array stadistic; + std::array statistic; }; static_assert(sizeof(MomentProcessorState) == 0x258, "MomentProcessorState is an invalid size"); + struct MomentSharedMemory { + Service::IRS::Lifo moment_lifo; + }; + static_assert(sizeof(MomentSharedMemory) == 0xE20, "MomentSharedMemory is an invalid size"); + + void OnControllerUpdate(Core::HID::ControllerTriggerType type); + u8 GetPixel(const std::vector& data, std::size_t x, std::size_t y) const; + MomentStatistic GetStatistic(const std::vector& data, std::size_t start_x, + std::size_t start_y, std::size_t width, std::size_t height) const; + + MomentSharedMemory* shared_memory = nullptr; + MomentProcessorState next_state{}; + MomentProcessorConfig current_config{}; Core::IrSensor::DeviceFormat& device; + Core::HID::EmulatedController* npad_device; + int callback_key{}; + + Core::System& system; }; } // namespace Service::IRS