update

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;
+				std::lock_guard pad_lock(pad::g_pad_mutex);
-				gem.calibration_start_us = 0;
+				const auto handler = pad::get_current_handler();
-				gem.calibration_status_flags = CELL_GEM_FLAG_CALIBRATION_SUCCEEDED | CELL_GEM_FLAG_CALIBRATION_OCCURRED;
+				const auto& pad = ::at32(handler->GetPads(), pad_num(gem_num));
-				gem.calibrated_magnetometer = true;
+				if (pad && pad->m_pad_handler == pad_handler::move)
-				gem.enabled_tracking = true;
+				{
+					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[0] = move_data.quaternion[0]; // x
-		gem_state->quat[1] = move_data.quaternion[2]; // y
+		gem_state->quat[1] = move_data.quaternion[1]; // y
-		gem_state->quat[2] = move_data.quaternion[3]; // z
+		gem_state->quat[2] = move_data.quaternion[2]; // z
-		gem_state->quat[3] = move_data.quaternion[0]; // w
+		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;
+	controller.is_calibrating = true;
-	gem.controllers[gem_num].calibration_start_us = get_guest_system_time();
+	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);
+	auto& controller = gem.controllers[gem_num];
-	gem.controllers[gem_num].enabled_tracking = false;
+
+	controller.sphere_rgb = gem_config::gem_color(r, g, b);
+	controller.enabled_tracking = false;
 
 	const auto [h, s, v] = ps_move_tracker<false>::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<Ce
 		return CELL_GEM_COMPUTING_AVAILABLE_COLORS;
 	}
 
-	if (gem.is_controller_calibrating(gem_num))
+	if (controller.is_calibrating)
 	{
 		return CELL_GEM_SPHERE_CALIBRATING;
 	}
@@ -2574,14 +2651,15 @@ error_code cellGemInvalidateCalibration(s32 gem_num)
 		return CELL_GEM_ERROR_INVALID_PARAMETER;
 	}
 
-	gem.controllers[gem_num].calibrated_magnetometer = false;
+	auto& controller = gem.controllers[gem_num];
 
 	// TODO: does this really stop an ongoing calibration ?
-	gem.controllers[gem_num].is_calibrating = false;
+	controller.calibrated_magnetometer = false;
-	gem.controllers[gem_num].calibration_start_us = 0;
+	controller.is_calibrating = false;
-	gem.controllers[gem_num].calibration_status_flags = 0;
+	controller.calibration_start_us = 0;
-	gem.controllers[gem_num].hue_set = false;
+	controller.calibration_status_flags = 0;
-	gem.controllers[gem_num].enabled_tracking = false;
+	controller.hue_set = false;
+	controller.enabled_tracking = false;
 
 	return CELL_OK;
 }

rpcs3/Emu/Io/pad_types.h

@@ -466,13 +466,21 @@ struct ps_move_data
 	bool external_device_read_requested = false;
 	bool external_device_write_requested = false;
 
+	bool calibration_requested = false;
+	bool calibration_succeeded = false;
+
+	bool magnetometer_enabled = false;
+
 	std::array<f32, 4> 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_x = 0.0f; // linear velocity in m/s²
-	f32 accelerometer_y = 0; // linear velocity in m/s²
+	f32 accelerometer_y = 0.0f; // linear velocity in m/s²
-	f32 accelerometer_z = 0; // linear velocity in m/s²
+	f32 accelerometer_z = 0.0f; // linear velocity in m/s²
-	f32 gyro_x = 0; // angular velocity in rad/s
+	f32 gyro_x = 0.0f; // angular velocity in rad/s
-	f32 gyro_y = 0; // angular velocity in rad/s
+	f32 gyro_y = 0.0f; // angular velocity in rad/s
-	f32 gyro_z = 0; // 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;
 };
 

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<typename T>
+	concept Texture = std::is_same_v<T, fragment_texture> || std::is_same_v<T, vertex_texture>;
 }

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.
+				// NOTE: In vulkan, the border color can bypass the sample swizzle stage.
-				// It is a direct texel replacement and must be remapped before attaching to the sampler.
+				// 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.
+				// NOTE: In vulkan, the border color can bypass the sample swizzle stage.
-				// It is a direct texel replacement and must be remapped before attaching to the sampler.
+				// 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])

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<PFN_vkGetPhysicalDeviceFeatures2KHR>(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<void*>(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<void*>(device.pNext);
-			device.pNext = &custom_border_color_features;
-		}
-
 		VkPhysicalDeviceSynchronization2FeaturesKHR synchronization2_info{};
 		if (pgpu->optional_features_support.synchronization_2)
 		{

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; }

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
+	device->reset_orientation();
-	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);
 
 	// 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_x -= zero_shift;
-		accel_y = (input.accel_y_1 + input.accel_y_2) / 2 - zero_shift;
+		accel_y -= zero_shift;
-		accel_z = (input.accel_z_1 + input.accel_z_2) / 2 - zero_shift;
+		accel_z -= zero_shift;
-		gyro_x = (input.gyro_x_1 + input.gyro_x_2) / 2 - zero_shift;
+		gyro_x -= zero_shift;
-		gyro_y = (input.gyro_y_1 + input.gyro_y_2) / 2 - zero_shift;
+		gyro_y -= zero_shift;
-		gyro_z = (input.gyro_z_1 + input.gyro_z_2) / 2 - 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<f32>(TWELVE_BIT_SIGNED(((input.magnetometer_x & 0x0F) << 8) | input_zcm1.magnetometer_x2));
+		pad->move_data.magnetometer_y = static_cast<f32>(TWELVE_BIT_SIGNED((input_zcm1.magnetometer_y << 4) | (input_zcm1.magnetometer_yz & 0xF0) >> 4));
+		pad->move_data.magnetometer_z = static_cast<f32>(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
+	dev->update_orientation(pad->move_data);
-	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];
 
 	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<u32>(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];
+}

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
+		u8 magnetometer_y{};                 // 0x28    1+   Y-axis magnetometer
-		u16 magnetometer_z{};                // 0x29    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<u8, 5> 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;
 };