cellGem: add magnetometer support

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
@@ -333,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)
@@ -351,6 +355,7 @@ public:
 
 					if (gem_num == i)
 					{
+						pad->move_data.magnetometer_enabled = controller.enabled_magnetometer;
 						is_connected = true;
 					}
 				}
@@ -413,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)
 		{
@@ -1772,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;
 }
@@ -1815,6 +1831,19 @@ error_code cellGemEnableMagnetometer2(u32 gem_num, u32 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;
 }
 

rpcs3/Emu/Io/pad_types.h

@@ -469,6 +469,8 @@ struct ps_move_data
 	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.0f; // linear velocity in m/s²
 	f32 accelerometer_y = 0.0f; // linear velocity in m/s²

rpcs3/Input/ps_move_handler.cpp

@@ -684,6 +684,13 @@ void ps_move_handler::get_extended_info(const pad_ensemble& binding)
 		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;
+
+		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
@@ -906,18 +913,16 @@ void ps_move_device::update_orientation(ps_move_data& move_data)
 
 	FusionVector magnetometer {};
 
-	// TODO: use magnetometer if possible
+	if (move_data.magnetometer_enabled)
-	//if (dev->model == ps_move_model::ZCM1)
+	{
-	//{
+		magnetometer = FusionVector{
-	//	const ps_move_input_report_ZCM1& input = dev->input_report_ZCM1;
+			.axis {
-	//	magnetometer = FusionVector{
+				.x = move_data.magnetometer_x,
-	//		.axis {
+				.y = move_data.magnetometer_y,
-	//			.x = input.magnetometer_x2,
+				.z = move_data.magnetometer_z
-	//			.y = input.magnetometer_y,
+			}
-	//			.z = input.magnetometer_z
+		};
-	//		}
+	}
-	//	};
-	//}
 
 	// Update Fusion
 	FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, elapsed_sec);

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