PS Move: more gyro support

rpcs3/Emu/Cell/Modules/cellGem.cpp

@@ -1142,7 +1142,7 @@ static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& con
 	gem_state->handle_pos[3] = 0.f;
 
 	// Calculate orientation
-	if (g_cfg.io.move == move_handler::real)
+	if (g_cfg.io.move == move_handler::real || (g_cfg.io.move == move_handler::fake && move_data.orientation_enabled))
 	{
 		gem_state->quat[0] = move_data.quaternion[0]; // x
 		gem_state->quat[1] = move_data.quaternion[1]; // y
@@ -1151,14 +1151,11 @@ static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& con
 	}
 	else
 	{
-		static constexpr f32 PI = 3.14159265f;
-		const auto degree_to_rad = [](f32 degree) -> f32 { return degree * PI / 180.0f; };
-
 		const f32 max_angle_per_side_h = g_cfg.io.fake_move_rotation_cone_h / 2.0f;
 		const f32 max_angle_per_side_v = g_cfg.io.fake_move_rotation_cone_v / 2.0f;
-		const f32 roll = -degree_to_rad((image_y - half_height) / half_height * max_angle_per_side_v); // This is actually the pitch
-		const f32 pitch = -degree_to_rad((image_x - half_width) / half_width * max_angle_per_side_h); // This is actually the yaw
-		const f32 yaw = degree_to_rad(0.0f);
+		const f32 roll = -PadHandlerBase::degree_to_rad((image_y - half_height) / half_height * max_angle_per_side_v); // This is actually the pitch
+		const f32 pitch = -PadHandlerBase::degree_to_rad((image_x - half_width) / half_width * max_angle_per_side_h); // This is actually the yaw
+		const f32 yaw = PadHandlerBase::degree_to_rad(0.0f);
 		const f32 cr = std::cos(roll * 0.5f);
 		const f32 sr = std::sin(roll * 0.5f);
 		const f32 cp = std::cos(pitch * 0.5f);
@@ -1318,7 +1315,7 @@ static void ds3_pos_to_gem_state(u32 gem_num, gem_config::gem_controller& contro
 
 	if constexpr (std::is_same_v<T, vm::ptr<CellGemState>>)
 	{
-		pos_to_gem_state(gem_num, controller, gem_state, ds3_pos_x, ds3_pos_y, ds3_max_x, ds3_max_y, {});
+		pos_to_gem_state(gem_num, controller, gem_state, ds3_pos_x, ds3_pos_y, ds3_max_x, ds3_max_y, pad->move_data);
 	}
 	else if constexpr (std::is_same_v<T, vm::ptr<CellGemImageState>>)
 	{
@@ -2147,6 +2144,7 @@ error_code cellGemGetInertialState(u32 gem_num, u32 state_flag, u64 timestamp, v
 		switch (g_cfg.io.move)
 		{
 		case move_handler::real:
+		case move_handler::fake:
 		{
 			// Get temperature and sensor data
 			{
@@ -2155,7 +2153,7 @@ error_code cellGemGetInertialState(u32 gem_num, u32 state_flag, u64 timestamp, v
 				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->m_port_status & CELL_PAD_STATUS_CONNECTED))
+				if (pad && (pad->m_port_status & CELL_PAD_STATUS_CONNECTED))
 				{
 					inertial_state->temperature = pad->move_data.temperature;
 					inertial_state->accelerometer[0] = pad->move_data.accelerometer_x;
@@ -2170,9 +2168,6 @@ error_code cellGemGetInertialState(u32 gem_num, u32 state_flag, u64 timestamp, v
 			ds3_input_to_pad(gem_num, inertial_state->pad.digitalbuttons, inertial_state->pad.analog_T);
 			break;
 		}
-		case move_handler::fake:
-			ds3_input_to_pad(gem_num, inertial_state->pad.digitalbuttons, inertial_state->pad.analog_T);
-			break;
 		case move_handler::mouse:
 		case move_handler::raw_mouse:
 			mouse_input_to_pad(gem_num, inertial_state->pad.digitalbuttons, inertial_state->pad.analog_T);

rpcs3/Emu/Cell/SPUThread.cpp

@@ -5481,7 +5481,7 @@ bool spu_thread::reservation_check(u32 addr, u32 hash, atomic_t<u64, 64>* range_
 
 usz spu_thread::register_cache_line_waiter(u32 addr)
 {
-	const u64 value = u64{compute_rdata_hash32(rdata)} << 32 | raddr;
+	const u64 value = u64{compute_rdata_hash32(rdata)} << 32 | addr;
 
 	for (usz i = 0; i < std::size(g_spu_waiters_by_value); i++)
 	{

rpcs3/Emu/Io/Null/NullPadHandler.h

@@ -48,6 +48,7 @@ class NullPadHandler final : public PadHandlerBase
 
 		cfg->pressure_intensity_button.def = "";
 		cfg->analog_limiter_button.def = "";
+		cfg->orientation_reset_button.def = "";
 
 		// Apply defaults
 		cfg->from_default();

rpcs3/Emu/Io/PadHandler.cpp

@@ -2,6 +2,7 @@
 #include "PadHandler.h"
 #include "Emu/system_utils.hpp"
 #include "Emu/system_config.h"
+#include "Emu/Cell/timers.hpp"
 #include "Input/pad_thread.h"
 #include "Input/product_info.h"
 
@@ -494,6 +495,12 @@ bool PadHandlerBase::bindPadToDevice(std::shared_ptr<Pad> pad)
 		pad->m_analog_limiter_button_index = static_cast<s32>(pad->m_buttons.size()) - 1;
 	}
 
+	if (b_has_orientation)
+	{
+		pad->m_buttons.emplace_back(special_button_offset, mapping[button::orientation_reset_button], special_button_value::orientation_reset);
+		pad->m_orientation_reset_button_index = static_cast<s32>(pad->m_buttons.size()) - 1;
+	}
+
 	pad->m_buttons.emplace_back(CELL_PAD_BTN_OFFSET_DIGITAL1, mapping[button::up], CELL_PAD_CTRL_UP);
 	pad->m_buttons.emplace_back(CELL_PAD_BTN_OFFSET_DIGITAL1, mapping[button::down], CELL_PAD_CTRL_DOWN);
 	pad->m_buttons.emplace_back(CELL_PAD_BTN_OFFSET_DIGITAL1, mapping[button::left], CELL_PAD_CTRL_LEFT);
@@ -600,6 +607,11 @@ std::array<std::set<u32>, PadHandlerBase::button::button_count> PadHandlerBase::
 		mapping[button::analog_limiter_button] = FindKeyCodes<u32, u32>(button_list, cfg->analog_limiter_button);
 	}
 
+	if (b_has_orientation)
+	{
+		mapping[button::orientation_reset_button] = FindKeyCodes<u32, u32>(button_list, cfg->orientation_reset_button);
+	}
+
 	return mapping;
 }
 
@@ -739,6 +751,8 @@ void PadHandlerBase::process()
 		if (!device || !pad)
 			continue;
 
+		pad->move_data.orientation_enabled = b_has_orientation && device->config && device->config->orientation_enabled.get();
+
 		const connection status = update_connection(device);
 
 		switch (status)
@@ -754,6 +768,11 @@ void PadHandlerBase::process()
 
 				last_connection_status[i] = true;
 				connected_devices++;
+
+				if (b_has_orientation)
+				{
+					device->reset_orientation();
+				}
 			}
 
 			if (status == connection::no_data)
@@ -790,13 +809,154 @@ void PadHandlerBase::process()
 
 				last_connection_status[i] = false;
 				connected_devices--;
+
+				if (b_has_orientation)
+				{
+					device->reset_orientation();
+				}
 			}
 			continue;
 		}
 		}
 
 		get_mapping(m_bindings[i]);
 		get_extended_info(m_bindings[i]);
+		get_orientation(m_bindings[i]);
 		apply_pad_data(m_bindings[i]);
 	}
 }
+
+void PadHandlerBase::set_raw_orientation(ps_move_data& move_data, f32 accel_x, f32 accel_y, f32 accel_z, f32 gyro_x, f32 gyro_y, f32 gyro_z)
+{
+	if (!move_data.orientation_enabled)
+	{
+		move_data.reset_sensors();
+		return;
+	}
+
+	// This function expects DS3 sensor accel values in linear velocity (m/s²) and gyro values in angular velocity (degree/s)
+	// The default position is flat on the ground, pointing forward.
+	// The accelerometers constantly measure G forces.
+	// The gyros measure changes in orientation and will reset when the device isn't moved anymore.
+	move_data.accelerometer_x = -accel_x;      // move_data: Increases if the device is rolled to the left
+	move_data.accelerometer_y = accel_z;       // move_data: Increases if the device is pitched upwards
+	move_data.accelerometer_z = accel_y;       // move_data: Increases if the device is moved upwards
+	move_data.gyro_x = degree_to_rad(-gyro_x); // move_data: Increases if the device is pitched upwards
+	move_data.gyro_y = degree_to_rad(gyro_z);  // move_data: Increases if the device is rolled to the right
+	move_data.gyro_z = degree_to_rad(-gyro_y); // move_data: Increases if the device is yawed to the left
+}
+
+void PadHandlerBase::set_raw_orientation(Pad& pad)
+{
+	if (!pad.move_data.orientation_enabled)
+	{
+		pad.move_data.reset_sensors();
+		return;
+	}
+
+	// acceleration (linear velocity in m/s²)
+	const f32 accel_x = (pad.m_sensors[0].m_value - 512) / static_cast<f32>(MOTION_ONE_G);
+	const f32 accel_y = (pad.m_sensors[1].m_value - 512) / static_cast<f32>(MOTION_ONE_G);
+	const f32 accel_z = (pad.m_sensors[2].m_value - 512) / static_cast<f32>(MOTION_ONE_G);
+
+	// gyro (angular velocity in degree/s)
+	constexpr f32 gyro_x = 0.0f;
+	const f32 gyro_y = (pad.m_sensors[3].m_value - 512) / (123.f / 90.f);
+	constexpr f32 gyro_z = 0.0f;
+
+	set_raw_orientation(pad.move_data, accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z);
+}
+
+void PadHandlerBase::get_orientation(const pad_ensemble& binding) const
+{
+	if (!b_has_orientation) return;
+
+	const auto& pad = binding.pad;
+	const auto& device = binding.device;
+	if (!pad || !device) return;
+
+	if (pad->move_data.calibration_requested)
+	{
+		device->reset_orientation();
+		pad->move_data.quaternion = ps_move_data::default_quaternion;
+		pad->move_data.calibration_succeeded = true;
+		return;
+	}
+
+	if (!pad->move_data.orientation_enabled || pad->get_orientation_reset_button_active())
+	{
+		// This can be called extensively in quick succession, so let's just reset the pointer instead of creating a new object.
+		device->ahrs.reset();
+		pad->move_data.quaternion = ps_move_data::default_quaternion;
+		return;
+	}
+
+	device->update_orientation(pad->move_data);
+}
+
+void PadDevice::reset_orientation()
+{
+	// Initialize Fusion
+	ahrs = std::make_shared<FusionAhrs>();
+	FusionAhrsInitialise(ahrs.get());
+	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.get(), &ahrs->settings);
+	FusionAhrsReset(ahrs.get());
+}
+
+void PadDevice::update_orientation(ps_move_data& move_data)
+{
+	if (!ahrs)
+	{
+		reset_orientation();
+	}
+
+	// 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
+		}
+	};
+
+	const FusionVector gyroscope{
+		.axis {
+			.x = +PadHandlerBase::rad_to_degree(move_data.gyro_x),
+			.y = +PadHandlerBase::rad_to_degree(move_data.gyro_z),
+			.z = -PadHandlerBase::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.get(), gyroscope, accelerometer, magnetometer, elapsed_sec);
+
+	// Get quaternion
+	const FusionQuaternion quaternion = FusionAhrsGetQuaternion(ahrs.get());
+	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/Emu/Io/PadHandler.h

@@ -5,6 +5,15 @@
 #include "pad_config_types.h"
 #include "util/types.hpp"
 
+#ifndef _MSC_VER
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wold-style-cast"
+#endif
+#include "3rdparty/fusion/fusion/Fusion/Fusion.h"
+#ifndef _MSC_VER
+#pragma GCC diagnostic pop
+#endif
+
 #include <cmath>
 #include <functional>
 #include <string>
@@ -38,6 +47,12 @@ class PadDevice
 	};
 	color color_override{};
 	bool color_override_active{};
+
+	std::shared_ptr<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();
 };
 
 struct pad_ensemble
@@ -125,6 +140,7 @@ class PadHandlerBase
 
 		pressure_intensity_button,
 		analog_limiter_button,
+		orientation_reset_button,
 
 		button_count
 	};
@@ -153,6 +169,7 @@ class PadHandlerBase
 	bool b_has_config = false;
 	bool b_has_pressure_intensity_button = true;
 	bool b_has_analog_limiter_button = true;
+	bool b_has_orientation = false;
 
 	std::array<cfg_pad, MAX_GAMEPADS> m_pad_configs;
 	std::vector<pad_ensemble> m_bindings;
@@ -301,6 +318,7 @@ class PadHandlerBase
 	bool has_battery_led() const { return b_has_battery_led; }
 	bool has_pressure_intensity_button() const { return b_has_pressure_intensity_button; }
 	bool has_analog_limiter_button() const { return b_has_analog_limiter_button; }
+	bool has_orientation() const { return b_has_orientation; }
 
 	u16 NormalizeStickInput(u16 raw_value, s32 threshold, s32 multiplier, bool ignore_threshold = false) const;
 	void convert_stick_values(u16& x_out, u16& y_out, s32 x_in, s32 y_in, u32 deadzone, u32 anti_deadzone, u32 padsquircling) const;
@@ -323,6 +341,18 @@ class PadHandlerBase
 	virtual void get_motion_sensors(const std::string& pad_id, const motion_callback& callback, const motion_fail_callback& fail_callback, motion_preview_values preview_values, const std::array<AnalogSensor, 4>& sensors);
 	virtual std::unordered_map<u32, std::string> get_motion_axis_list() const { return {}; }
 
+	static constexpr f32 PI = 3.14159265f;
+
+	static f32 degree_to_rad(f32 degree)
+	{
+		return degree * PI / 180.0f;
+	}
+
+	static f32 rad_to_degree(f32 radians)
+	{
+		return radians * 180.0f / PI;
+	};
+
 private:
 	virtual std::shared_ptr<PadDevice> get_device(const std::string& /*device*/) { return nullptr; }
 	virtual bool get_is_left_trigger(const std::shared_ptr<PadDevice>& /*device*/, u64 /*keyCode*/) { return false; }
@@ -336,10 +366,15 @@ class PadHandlerBase
 	virtual std::unordered_map<u64, u16> get_button_values(const std::shared_ptr<PadDevice>& /*device*/) { return {}; }
 	virtual pad_preview_values get_preview_values(const std::unordered_map<u64, u16>& /*data*/) { return {}; }
 
+	void get_orientation(const pad_ensemble& binding) const;
+
 protected:
 	virtual std::array<std::set<u32>, PadHandlerBase::button::button_count> get_mapped_key_codes(const std::shared_ptr<PadDevice>& device, const cfg_pad* cfg);
 	virtual void get_mapping(const pad_ensemble& binding);
 	void TranslateButtonPress(const std::shared_ptr<PadDevice>& device, u64 keyCode, bool& pressed, u16& val, bool use_stick_multipliers, bool ignore_stick_threshold = false, bool ignore_trigger_threshold = false);
 	void init_configs();
 	cfg_pad* get_config(const std::string& pad_id);
+
+	static void set_raw_orientation(ps_move_data& move_data, f32 accel_x, f32 accel_y, f32 accel_z, f32 gyro_x, f32 gyro_y, f32 gyro_z);
+	static void set_raw_orientation(Pad& pad);
 };

rpcs3/Emu/Io/pad_config.h

@@ -66,6 +66,9 @@ struct cfg_pad final : cfg::node
 	cfg_sensor motion_sensor_z{ this, "Motion Sensor Z" };
 	cfg_sensor motion_sensor_g{ this, "Motion Sensor G" };
 
+	cfg::string orientation_reset_button{ this, "Orientation Reset Button", "" };
+	cfg::_bool orientation_enabled{ this, "Orientation Enabled", false };
+
 	cfg::string pressure_intensity_button{ this, "Pressure Intensity Button", "" };
 	cfg::uint<0, 100> pressure_intensity{ this, "Pressure Intensity Percent", 50 };
 	cfg::_bool pressure_intensity_toggle_mode{ this, "Pressure Intensity Toggle Mode", false };