diff --git a/rpcs3/Emu/Cell/Modules/cellGem.cpp b/rpcs3/Emu/Cell/Modules/cellGem.cpp index eb043ed2bb..61c9020287 100644 --- a/rpcs3/Emu/Cell/Modules/cellGem.cpp +++ b/rpcs3/Emu/Cell/Modules/cellGem.cpp @@ -225,7 +225,7 @@ public: u32 ext_status = CELL_GEM_NO_EXTERNAL_PORT_DEVICE; // External port connection status u32 ext_id = 0; // External device ID (type). For example SHARP_SHOOTER_DEVICE_ID u32 port = 0; // Assigned port - bool enabled_magnetometer = false; // Whether the magnetometer is enabled (probably used for additional rotational precision) + bool enabled_magnetometer = true; // Whether the magnetometer is enabled (probably used for additional rotational precision) bool calibrated_magnetometer = false; // Whether the magnetometer is calibrated bool enabled_filtering = false; // Whether filtering is enabled bool enabled_tracking = false; // Whether tracking is enabled @@ -270,23 +270,60 @@ public: return controllers[gem_num].status == CELL_GEM_STATUS_READY; } - bool is_controller_calibrating(u32 gem_num) + void update_calibration_status() { - gem_controller& gem = controllers[gem_num]; + std::scoped_lock lock(mtx); - if (gem.is_calibrating) + for (u32 gem_num = 0; gem_num < CELL_GEM_MAX_NUM; gem_num++) { - if ((get_guest_system_time() - gem.calibration_start_us) >= gem_controller::calibration_time_us) + gem_controller& controller = controllers[gem_num]; + if (!controller.is_calibrating) continue; + + bool controller_calibrated = true; + + // Request controller calibration + if (g_cfg.io.move == move_handler::real) { - gem.is_calibrating = false; - gem.calibration_start_us = 0; - gem.calibration_status_flags = CELL_GEM_FLAG_CALIBRATION_SUCCEEDED | CELL_GEM_FLAG_CALIBRATION_OCCURRED; - gem.calibrated_magnetometer = true; - gem.enabled_tracking = true; + std::lock_guard pad_lock(pad::g_pad_mutex); + const auto handler = pad::get_current_handler(); + const auto& pad = ::at32(handler->GetPads(), pad_num(gem_num)); + if (pad && pad->m_pad_handler == pad_handler::move) + { + if (!pad->move_data.calibration_requested || !pad->move_data.calibration_succeeded) + { + pad->move_data.calibration_requested = true; + controller_calibrated = false; + } + } + } + + // The calibration takes ~0.5 seconds on real hardware + if ((get_guest_system_time() - controller.calibration_start_us) < gem_controller::calibration_time_us) continue; + + if (!controller_calibrated) + { + cellGem.warning("Reached calibration timeout but ps move controller %d is still calibrating", gem_num); + } + + controller.is_calibrating = false; + controller.calibration_start_us = 0; + controller.calibration_status_flags = CELL_GEM_FLAG_CALIBRATION_SUCCEEDED | CELL_GEM_FLAG_CALIBRATION_OCCURRED; + controller.calibrated_magnetometer = true; + controller.enabled_tracking = true; + + // Reset controller calibration request + if (g_cfg.io.move == move_handler::real) + { + std::lock_guard pad_lock(pad::g_pad_mutex); + const auto handler = pad::get_current_handler(); + const auto& pad = ::at32(handler->GetPads(), pad_num(gem_num)); + if (pad && pad->m_pad_handler == pad_handler::move) + { + pad->move_data.calibration_requested = false; + pad->move_data.calibration_succeeded = false; + } } } - - return gem.is_calibrating; } void reset_controller(u32 gem_num) @@ -296,6 +333,10 @@ public: return; } + gem_controller& controller = ::at32(controllers, gem_num); + controller = {}; + controller.sphere_rgb = gem_color::get_default_color(gem_num); + bool is_connected = false; switch (g_cfg.io.move) @@ -314,6 +355,7 @@ public: if (gem_num == i) { + pad->move_data.magnetometer_enabled = controller.enabled_magnetometer; is_connected = true; } } @@ -376,10 +418,6 @@ public: break; } - gem_controller& controller = ::at32(controllers, gem_num); - controller = {}; - controller.sphere_rgb = gem_color::get_default_color(gem_num); - // Assign status and port number if (is_connected) { @@ -749,6 +787,11 @@ void gem_config_data::operator()() { while (!video_conversion_in_progress && thread_ctrl::state() != thread_state::aborting && !Emu.IsStopped()) { + if (state) + { + update_calibration_status(); + } + thread_ctrl::wait_for(1000); } @@ -1105,10 +1148,10 @@ static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& con // Calculate orientation if (g_cfg.io.move == move_handler::real) { - gem_state->quat[0] = move_data.quaternion[1]; // x - gem_state->quat[1] = move_data.quaternion[2]; // y - gem_state->quat[2] = move_data.quaternion[3]; // z - gem_state->quat[3] = move_data.quaternion[0]; // w + gem_state->quat[0] = move_data.quaternion[0]; // x + gem_state->quat[1] = move_data.quaternion[1]; // y + gem_state->quat[2] = move_data.quaternion[2]; // z + gem_state->quat[3] = move_data.quaternion[3]; // w } else { @@ -1595,13 +1638,15 @@ error_code cellGemCalibrate(u32 gem_num) return CELL_GEM_ERROR_INVALID_PARAMETER; } - if (gem.is_controller_calibrating(gem_num)) + auto& controller = gem.controllers[gem_num]; + + if (controller.is_calibrating) { return CELL_EBUSY; } - gem.controllers[gem_num].is_calibrating = true; - gem.controllers[gem_num].calibration_start_us = get_guest_system_time(); + controller.is_calibrating = true; + controller.calibration_start_us = get_guest_system_time(); return CELL_OK; } @@ -1728,13 +1773,28 @@ error_code cellGemEnableMagnetometer(u32 gem_num, u32 enable) return CELL_GEM_NOT_CONNECTED; } + auto& controller = gem.controllers[gem_num]; + // NOTE: RE doesn't show this check but it is mentioned in the docs, so I'll leave it here for now. - //if (!gem.controllers[gem_num].calibrated_magnetometer) + //if (!controller.calibrated_magnetometer) //{ // return CELL_GEM_NOT_CALIBRATED; //} - gem.controllers[gem_num].enabled_magnetometer = !!enable; + controller.enabled_magnetometer = !!enable; + + if (g_cfg.io.move == move_handler::real) + { + std::lock_guard lock(pad::g_pad_mutex); + + const auto handler = pad::get_current_handler(); + const auto& pad = ::at32(handler->GetPads(), pad_num(gem_num)); + + if (pad && pad->m_pad_handler == pad_handler::move) + { + pad->move_data.magnetometer_enabled = controller.enabled_magnetometer; + } + } return CELL_OK; } @@ -1762,12 +1822,27 @@ error_code cellGemEnableMagnetometer2(u32 gem_num, u32 enable) return CELL_GEM_NOT_CONNECTED; } - if (!gem.controllers[gem_num].calibrated_magnetometer) + auto& controller = gem.controllers[gem_num]; + + if (!controller.calibrated_magnetometer) { return CELL_GEM_NOT_CALIBRATED; } - gem.controllers[gem_num].enabled_magnetometer = !!enable; + controller.enabled_magnetometer = !!enable; + + if (g_cfg.io.move == move_handler::real) + { + std::lock_guard lock(pad::g_pad_mutex); + + const auto handler = pad::get_current_handler(); + const auto& pad = ::at32(handler->GetPads(), pad_num(gem_num)); + + if (pad && pad->m_pad_handler == pad_handler::move) + { + pad->move_data.magnetometer_enabled = controller.enabled_magnetometer; + } + } return CELL_OK; } @@ -1845,11 +1920,13 @@ error_code cellGemForceRGB(u32 gem_num, f32 r, f32 g, f32 b) // color = color * (2.f / sum) //} - gem.controllers[gem_num].sphere_rgb = gem_config::gem_color(r, g, b); - gem.controllers[gem_num].enabled_tracking = false; + auto& controller = gem.controllers[gem_num]; + + controller.sphere_rgb = gem_config::gem_color(r, g, b); + controller.enabled_tracking = false; const auto [h, s, v] = ps_move_tracker::rgb_to_hsv(r, g, b); - gem.controllers[gem_num].hue = h; + controller.hue = h; return CELL_OK; } @@ -2377,7 +2454,7 @@ error_code cellGemGetState(u32 gem_num, u32 flag, u64 time_parameter, vm::ptr quaternion { 1.0f, 0.0f, 0.0f, 0.0f }; // quaternion orientation (x,y,z,w) of controller relative to default (facing the camera with buttons up) - f32 accelerometer_x = 0; // linear velocity in m/s² - f32 accelerometer_y = 0; // linear velocity in m/s² - f32 accelerometer_z = 0; // linear velocity in m/s² - f32 gyro_x = 0; // angular velocity in rad/s - f32 gyro_y = 0; // angular velocity in rad/s - f32 gyro_z = 0; // angular velocity in rad/s + f32 accelerometer_x = 0.0f; // linear velocity in m/s² + f32 accelerometer_y = 0.0f; // linear velocity in m/s² + f32 accelerometer_z = 0.0f; // linear velocity in m/s² + f32 gyro_x = 0.0f; // angular velocity in rad/s + f32 gyro_y = 0.0f; // angular velocity in rad/s + f32 gyro_z = 0.0f; // angular velocity in rad/s + f32 magnetometer_x = 0.0f; + f32 magnetometer_y = 0.0f; + f32 magnetometer_z = 0.0f; s16 temperature = 0; }; diff --git a/rpcs3/Emu/RSX/RSXTexture.h b/rpcs3/Emu/RSX/RSXTexture.h index a7d94db658..5517a39e3f 100644 --- a/rpcs3/Emu/RSX/RSXTexture.h +++ b/rpcs3/Emu/RSX/RSXTexture.h @@ -142,4 +142,7 @@ namespace rsx rsx::texture_dimension_extended get_extended_texture_dimension() const; u16 get_exact_mipmap_count() const; }; + + template + concept Texture = std::is_same_v || std::is_same_v; } diff --git a/rpcs3/Emu/RSX/VK/VKDraw.cpp b/rpcs3/Emu/RSX/VK/VKDraw.cpp index ad0bca997f..f15541a968 100644 --- a/rpcs3/Emu/RSX/VK/VKDraw.cpp +++ b/rpcs3/Emu/RSX/VK/VKDraw.cpp @@ -244,6 +244,13 @@ void VKGSRender::load_texture_env() return false; }; + auto get_border_color = [&](const rsx::Texture auto& tex) + { + return m_device->get_custom_border_color_support().require_border_color_remap + ? tex.remapped_border_color() + : rsx::decode_border_color(tex.border_color()); + }; + std::lock_guard lock(m_sampler_mutex); for (u32 textures_ref = current_fp_metadata.referenced_textures_mask, i = 0; textures_ref; textures_ref >>= 1, ++i) @@ -304,10 +311,10 @@ void VKGSRender::load_texture_env() const auto wrap_t = vk::vk_wrap_mode(tex.wrap_t()); const auto wrap_r = vk::vk_wrap_mode(tex.wrap_r()); - // NOTE: In vulkan, the border color bypasses the swizzling defined in the image view. - // It is a direct texel replacement and must be remapped before attaching to the sampler. + // NOTE: In vulkan, the border color can bypass the sample swizzle stage. + // Check the device properties to determine whether to pre-swizzle the colors or not. const auto border_color = rsx::is_border_clamped_texture(tex) - ? vk::border_color_t(tex.remapped_border_color()) + ? vk::border_color_t(get_border_color(tex)) : vk::border_color_t(VK_BORDER_COLOR_FLOAT_OPAQUE_BLACK); // Check if non-point filtering can even be used on this format @@ -449,10 +456,10 @@ void VKGSRender::load_texture_env() const auto wrap_s = vk::vk_wrap_mode(tex.wrap_s()); const auto wrap_t = vk::vk_wrap_mode(tex.wrap_t()); - // NOTE: In vulkan, the border color bypasses the swizzling defined in the image view. - // It is a direct texel replacement and must be remapped before attaching to the sampler. + // NOTE: In vulkan, the border color can bypass the sample swizzle stage. + // Check the device properties to determine whether to pre-swizzle the colors or not. const auto border_color = is_border_clamped_texture(tex) - ? vk::border_color_t(tex.remapped_border_color()) + ? vk::border_color_t(get_border_color(tex)) : vk::border_color_t(VK_BORDER_COLOR_FLOAT_OPAQUE_BLACK); if (vs_sampler_handles[i]) diff --git a/rpcs3/Emu/RSX/VK/vkutils/device.cpp b/rpcs3/Emu/RSX/VK/vkutils/device.cpp index c3a96ba237..58e3a8df47 100644 --- a/rpcs3/Emu/RSX/VK/vkutils/device.cpp +++ b/rpcs3/Emu/RSX/VK/vkutils/device.cpp @@ -34,6 +34,7 @@ namespace vk VkPhysicalDeviceAttachmentFeedbackLoopLayoutFeaturesEXT fbo_loops_info{}; VkPhysicalDeviceFragmentShaderBarycentricFeaturesKHR shader_barycentric_info{}; VkPhysicalDeviceCustomBorderColorFeaturesEXT custom_border_color_info{}; + VkPhysicalDeviceBorderColorSwizzleFeaturesEXT border_color_swizzle_info{}; if (device_extensions.is_supported(VK_KHR_SHADER_FLOAT16_INT8_EXTENSION_NAME)) { @@ -70,6 +71,13 @@ namespace vk features2.pNext = &custom_border_color_info; } + if (device_extensions.is_supported(VK_EXT_BORDER_COLOR_SWIZZLE_EXTENSION_NAME)) + { + border_color_swizzle_info.sType = VK_STRUCTURE_TYPE_PHYSICAL_DEVICE_BORDER_COLOR_SWIZZLE_FEATURES_EXT; + border_color_swizzle_info.pNext = features2.pNext; + features2.pNext = &border_color_swizzle_info; + } + auto _vkGetPhysicalDeviceFeatures2KHR = reinterpret_cast(vkGetInstanceProcAddr(parent, "vkGetPhysicalDeviceFeatures2KHR")); ensure(_vkGetPhysicalDeviceFeatures2KHR); // "vkGetInstanceProcAddress failed to find entry point!" _vkGetPhysicalDeviceFeatures2KHR(dev, &features2); @@ -78,7 +86,10 @@ namespace vk shader_types_support.allow_float16 = !!shader_support_info.shaderFloat16; shader_types_support.allow_int8 = !!shader_support_info.shaderInt8; - optional_features_support.custom_border_color = !!custom_border_color_info.customBorderColors && !!custom_border_color_info.customBorderColorWithoutFormat; + custom_border_color_support.supported = !!custom_border_color_info.customBorderColors && !!custom_border_color_info.customBorderColorWithoutFormat; + custom_border_color_support.swizzle_extension_supported = border_color_swizzle_info.sType == VK_STRUCTURE_TYPE_PHYSICAL_DEVICE_BORDER_COLOR_SWIZZLE_FEATURES_EXT; + custom_border_color_support.require_border_color_remap = !border_color_swizzle_info.borderColorSwizzleFromImage; + optional_features_support.barycentric_coords = !!shader_barycentric_info.fragmentShaderBarycentric; optional_features_support.framebuffer_loops = !!fbo_loops_info.attachmentFeedbackLoopLayout; @@ -107,6 +118,13 @@ namespace vk optional_features_support.debug_utils = instance_extensions.is_supported(VK_EXT_DEBUG_UTILS_EXTENSION_NAME); optional_features_support.surface_capabilities_2 = instance_extensions.is_supported(VK_KHR_GET_PHYSICAL_DEVICE_PROPERTIES_2_EXTENSION_NAME); + + // Post-initialization checks + if (!custom_border_color_support.swizzle_extension_supported) + { + // So far only AMD is known to remap image view and border color together. Mark as not required. + custom_border_color_support.require_border_color_remap = get_driver_vendor() != driver_vendor::AMD; + } } void physical_device::get_physical_device_properties(bool allow_extensions) @@ -438,6 +456,11 @@ namespace vk requested_extensions.push_back(VK_KHR_SHADER_FLOAT16_INT8_EXTENSION_NAME); } + if (pgpu->custom_border_color_support) + { + requested_extensions.push_back(VK_EXT_CUSTOM_BORDER_COLOR_EXTENSION_NAME); + } + if (pgpu->optional_features_support.conditional_rendering) { requested_extensions.push_back(VK_EXT_CONDITIONAL_RENDERING_EXTENSION_NAME); @@ -480,11 +503,6 @@ namespace vk requested_extensions.push_back(VK_KHR_FRAGMENT_SHADER_BARYCENTRIC_EXTENSION_NAME); } - if (pgpu->optional_features_support.custom_border_color) - { - requested_extensions.push_back(VK_EXT_CUSTOM_BORDER_COLOR_EXTENSION_NAME); - } - if (pgpu->optional_features_support.synchronization_2) { requested_extensions.push_back(VK_KHR_SYNCHRONIZATION_2_EXTENSION_NAME); @@ -665,6 +683,16 @@ namespace vk device.pNext = &indexing_features; } + VkPhysicalDeviceCustomBorderColorFeaturesEXT custom_border_color_features{}; + if (pgpu->custom_border_color_support) + { + custom_border_color_features.sType = VK_STRUCTURE_TYPE_PHYSICAL_DEVICE_CUSTOM_BORDER_COLOR_FEATURES_EXT; + custom_border_color_features.customBorderColors = VK_TRUE; + custom_border_color_features.customBorderColorWithoutFormat = VK_TRUE; + custom_border_color_features.pNext = const_cast(device.pNext); + device.pNext = &custom_border_color_features; + } + VkPhysicalDeviceAttachmentFeedbackLoopLayoutFeaturesEXT fbo_loop_features{}; if (pgpu->optional_features_support.framebuffer_loops) { @@ -674,16 +702,6 @@ namespace vk device.pNext = &fbo_loop_features; } - VkPhysicalDeviceCustomBorderColorFeaturesEXT custom_border_color_features{}; - if (pgpu->optional_features_support.custom_border_color) - { - custom_border_color_features.sType = VK_STRUCTURE_TYPE_PHYSICAL_DEVICE_CUSTOM_BORDER_COLOR_FEATURES_EXT; - custom_border_color_features.customBorderColors = VK_TRUE; - custom_border_color_features.customBorderColorWithoutFormat = VK_TRUE; - custom_border_color_features.pNext = const_cast(device.pNext); - device.pNext = &custom_border_color_features; - } - VkPhysicalDeviceSynchronization2FeaturesKHR synchronization2_info{}; if (pgpu->optional_features_support.synchronization_2) { diff --git a/rpcs3/Emu/RSX/VK/vkutils/device.h b/rpcs3/Emu/RSX/VK/vkutils/device.h index 4a9a0fca3d..3b0a10f44b 100644 --- a/rpcs3/Emu/RSX/VK/vkutils/device.h +++ b/rpcs3/Emu/RSX/VK/vkutils/device.h @@ -49,7 +49,16 @@ namespace vk descriptor_indexing_features(bool supported = false) : supported(supported) {} - operator bool() { return supported; } + operator bool() const { return supported; } + }; + + struct custom_border_color_features + { + bool supported = false; + bool swizzle_extension_supported = false; + bool require_border_color_remap = true; // Assume that without the swizzle extension and explicit remap the device just samples border color as replacement. + + operator bool() const { return supported; } }; class physical_device @@ -68,11 +77,12 @@ namespace vk u32 descriptor_max_draw_calls = DESCRIPTOR_MAX_DRAW_CALLS; descriptor_indexing_features descriptor_indexing_support{}; + custom_border_color_features custom_border_color_support{}; + struct { bool barycentric_coords = false; bool conditional_rendering = false; - bool custom_border_color = false; bool debug_utils = false; bool external_memory_host = false; bool framebuffer_loops = false; @@ -161,6 +171,7 @@ namespace vk const gpu_formats_support& get_formats_support() const { return m_formats_support; } const pipeline_binding_table& get_pipeline_binding_table() const { return m_pipeline_binding_table; } const gpu_shader_types_support& get_shader_types_support() const { return pgpu->shader_types_support; } + const custom_border_color_features& get_custom_border_color_support() const { return pgpu->custom_border_color_support; } bool get_shader_stencil_export_support() const { return pgpu->optional_features_support.shader_stencil_export; } bool get_depth_bounds_support() const { return pgpu->features.depthBounds != VK_FALSE; } @@ -175,7 +186,6 @@ namespace vk bool get_descriptor_indexing_support() const { return pgpu->descriptor_indexing_support; } bool get_framebuffer_loops_support() const { return pgpu->optional_features_support.framebuffer_loops; } bool get_barycoords_support() const { return pgpu->optional_features_support.barycentric_coords; } - bool get_custom_border_color_support() const { return pgpu->optional_features_support.custom_border_color; } bool get_synchronization2_support() const { return pgpu->optional_features_support.synchronization_2; } u64 get_descriptor_update_after_bind_support() const { return pgpu->descriptor_indexing_support.update_after_bind_mask; } diff --git a/rpcs3/Input/ps_move_handler.cpp b/rpcs3/Input/ps_move_handler.cpp index 80742f0dc8..83049f5fd0 100644 --- a/rpcs3/Input/ps_move_handler.cpp +++ b/rpcs3/Input/ps_move_handler.cpp @@ -359,12 +359,7 @@ void ps_move_handler::check_add_device(hid_device* hidDevice, std::string_view p psmove_parse_calibration(calibration, *device); } - // Initialize Fusion - FusionAhrsInitialise(&device->ahrs); - device->ahrs.settings.convention = FusionConvention::FusionConventionEnu; - device->ahrs.settings.gain = 0.0f; // If gain is set, the algorithm tries to adjust the orientation over time. - FusionAhrsSetSettings(&device->ahrs, &device->ahrs.settings); - FusionAhrsReset(&device->ahrs); + device->reset_orientation(); // Activate if (send_output_report(device) == -1) @@ -683,12 +678,19 @@ void ps_move_handler::get_extended_info(const pad_ensemble& binding) if (dev->model == ps_move_model::ZCM1) { - accel_x = (input.accel_x_1 + input.accel_x_2) / 2 - zero_shift; - accel_y = (input.accel_y_1 + input.accel_y_2) / 2 - zero_shift; - accel_z = (input.accel_z_1 + input.accel_z_2) / 2 - zero_shift; - gyro_x = (input.gyro_x_1 + input.gyro_x_2) / 2 - zero_shift; - gyro_y = (input.gyro_y_1 + input.gyro_y_2) / 2 - zero_shift; - gyro_z = (input.gyro_z_1 + input.gyro_z_2) / 2 - zero_shift; + accel_x -= zero_shift; + accel_y -= zero_shift; + accel_z -= zero_shift; + gyro_x -= zero_shift; + gyro_y -= zero_shift; + gyro_z -= zero_shift; + + const ps_move_input_report_ZCM1& input_zcm1 = dev->input_report_ZCM1; + + #define TWELVE_BIT_SIGNED(x) (((x) & 0x800) ? (-(((~(x)) & 0xFFF) + 1)) : (x)) + pad->move_data.magnetometer_x = static_cast(TWELVE_BIT_SIGNED(((input.magnetometer_x & 0x0F) << 8) | input_zcm1.magnetometer_x2)); + pad->move_data.magnetometer_y = static_cast(TWELVE_BIT_SIGNED((input_zcm1.magnetometer_y << 4) | (input_zcm1.magnetometer_yz & 0xF0) >> 4)); + pad->move_data.magnetometer_z = static_cast(TWELVE_BIT_SIGNED(((input_zcm1.magnetometer_yz & 0x0F) << 8) | input_zcm1.magnetometer_z)); } // Apply calibration @@ -720,57 +722,7 @@ void ps_move_handler::get_extended_info(const pad_ensemble& binding) pad->m_sensors[2].m_value = Clamp0To1023(512.0f + (MOTION_ONE_G * pad->move_data.accelerometer_z)); pad->m_sensors[3].m_value = Clamp0To1023(512.0f + (MOTION_ONE_G * pad->move_data.gyro_z * -1.0f)); - // Get elapsed time since last update - const u64 now_us = get_system_time(); - const float elapsed_sec = (dev->last_ahrs_update_time_us == 0) ? 0.0f : ((now_us - dev->last_ahrs_update_time_us) / 1'000'000.0f); - dev->last_ahrs_update_time_us = now_us; - - // The ps move handler's axis may differ from the Fusion axis, so we have to map them correctly. - // Don't ask how the axis work. It's basically been trial and error. - ensure(dev->ahrs.settings.convention == FusionConvention::FusionConventionEnu); // East-North-Up - - const FusionVector accelerometer{ - .axis { - .x = -pad->move_data.accelerometer_x, - .y = +pad->move_data.accelerometer_y, - .z = +pad->move_data.accelerometer_z - } - }; - - static constexpr f32 PI = 3.14159265f; - const auto rad_to_degree = [](f32 radians) -> f32 { return radians * 180.0f / PI; }; - const FusionVector gyroscope{ - .axis { - .x = +rad_to_degree(pad->move_data.gyro_x), - .y = +rad_to_degree(pad->move_data.gyro_z), - .z = -rad_to_degree(pad->move_data.gyro_y) - } - }; - - FusionVector magnetometer {}; - - // TODO: use magnetometer if possible - //if (dev->model == ps_move_model::ZCM1) - //{ - // const ps_move_input_report_ZCM1& input = dev->input_report_ZCM1; - // magnetometer = FusionVector{ - // .axis { - // .x = input.magnetometer_x2, - // .y = input.magnetometer_y, - // .z = input.magnetometer_z - // } - // }; - //} - - // Update Fusion - FusionAhrsUpdate(&dev->ahrs, gyroscope, accelerometer, magnetometer, elapsed_sec); - - // Get quaternion - const FusionQuaternion quaternion = FusionAhrsGetQuaternion(&dev->ahrs); - pad->move_data.quaternion[0] = quaternion.array[0]; - pad->move_data.quaternion[1] = quaternion.array[1]; - pad->move_data.quaternion[2] = quaternion.array[2]; - pad->move_data.quaternion[3] = quaternion.array[3]; + dev->update_orientation(pad->move_data); handle_external_device(binding); } @@ -911,3 +863,74 @@ u32 ps_move_handler::get_battery_level(const std::string& padId) // 0 to 5 return std::clamp(device->battery_level * 20, 0, 100); } + +void ps_move_device::reset_orientation() +{ + // Initialize Fusion + ahrs = {}; + FusionAhrsInitialise(&ahrs); + ahrs.settings.convention = FusionConvention::FusionConventionEnu; + ahrs.settings.gain = 0.0f; // If gain is set, the algorithm tries to adjust the orientation over time. + FusionAhrsSetSettings(&ahrs, &ahrs.settings); + FusionAhrsReset(&ahrs); +} + +void ps_move_device::update_orientation(ps_move_data& move_data) +{ + if (move_data.calibration_requested) + { + reset_orientation(); + + move_data.calibration_succeeded = true; + } + + // Get elapsed time since last update + const u64 now_us = get_system_time(); + const float elapsed_sec = (last_ahrs_update_time_us == 0) ? 0.0f : ((now_us - last_ahrs_update_time_us) / 1'000'000.0f); + last_ahrs_update_time_us = now_us; + + // The ps move handler's axis may differ from the Fusion axis, so we have to map them correctly. + // Don't ask how the axis work. It's basically been trial and error. + ensure(ahrs.settings.convention == FusionConvention::FusionConventionEnu); // East-North-Up + + const FusionVector accelerometer{ + .axis { + .x = -move_data.accelerometer_x, + .y = +move_data.accelerometer_y, + .z = +move_data.accelerometer_z + } + }; + + static constexpr f32 PI = 3.14159265f; + const auto rad_to_degree = [](f32 radians) -> f32 { return radians * 180.0f / PI; }; + const FusionVector gyroscope{ + .axis { + .x = +rad_to_degree(move_data.gyro_x), + .y = +rad_to_degree(move_data.gyro_z), + .z = -rad_to_degree(move_data.gyro_y) + } + }; + + FusionVector magnetometer {}; + + if (move_data.magnetometer_enabled) + { + magnetometer = FusionVector{ + .axis { + .x = move_data.magnetometer_x, + .y = move_data.magnetometer_y, + .z = move_data.magnetometer_z + } + }; + } + + // Update Fusion + FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, elapsed_sec); + + // Get quaternion + const FusionQuaternion quaternion = FusionAhrsGetQuaternion(&ahrs); + move_data.quaternion[0] = quaternion.array[1]; + move_data.quaternion[1] = quaternion.array[2]; + move_data.quaternion[2] = quaternion.array[3]; + move_data.quaternion[3] = quaternion.array[0]; +} diff --git a/rpcs3/Input/ps_move_handler.h b/rpcs3/Input/ps_move_handler.h index 434a3c81a2..0a6939e6f8 100644 --- a/rpcs3/Input/ps_move_handler.h +++ b/rpcs3/Input/ps_move_handler.h @@ -54,8 +54,9 @@ namespace reports // ID Size Description u8 magnetometer_x2{}; // 0x27 1- X-axis magnetometer - u8 magnetometer_y{}; // 0x28 1+ Z-axis magnetometer - u16 magnetometer_z{}; // 0x29 1- Y-axis magnetometer + u8 magnetometer_y{}; // 0x28 1+ Y-axis magnetometer + u8 magnetometer_yz{}; // 0x29 1 XZ-axis magnetometer + u8 magnetometer_z{}; // 0x2A 1- Z-axis magnetometer u8 timestamp_lower{}; // 0x2B 1 Timestamp (lower byte) std::array ext_device_data{}; // 0x2C 5 External device data }; @@ -148,6 +149,9 @@ public: FusionAhrs ahrs {}; // Used to calculate quaternions from sensor data u64 last_ahrs_update_time_us = 0; // Last ahrs update + void update_orientation(ps_move_data& move_data); + void reset_orientation(); + const reports::ps_move_input_report_common& input_report_common() const; };