Merge branch 'master' into patch-1

schm1dtmac · web-flow · commit 97b8b4d888df · 2024-12-13T08:47:20.000Z
diff --git a/rpcs3/Emu/Cell/Modules/cellGem.cpp b/rpcs3/Emu/Cell/Modules/cellGem.cpp
@@ -232,8 +232,8 @@ struct gem_config_data
 		u8 rumble = 0;                                     // Rumble intensity
 		gem_color sphere_rgb = {};                         // RGB color of the sphere LED
 		u32 hue = 0;                                       // Tracking hue of the motion controller
-		f32 distance_mm{1500.0f};                          // Distance from the camera in mm
-		f32 radius{10.0f};                                 // Radius of the sphere in camera pixels
+		f32 distance_mm{3000.0f};                          // Distance from the camera in mm
+		f32 radius{5.0f};                                  // Radius of the sphere in camera pixels
 		bool radius_valid = true;                          // If the radius and distance of the sphere was computed.
 
 		bool is_calibrating{false};                        // Whether or not we are currently calibrating
@@ -919,9 +919,13 @@ static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& con
 	const f32 image_x = static_cast<f32>(x_pos) / scaling_width;
 	const f32 image_y = static_cast<f32>(y_pos) / scaling_height;
 
+	// Half of the camera image
+	const f32 half_width = shared_data.width / 2.f;
+	const f32 half_height = shared_data.height / 2.f;
+
 	// Centered image coordinates in pixels
-	const f32 centered_x = image_x - (shared_data.width / 2.f);
-	const f32 centered_y = (shared_data.height / 2.f) - image_y; // Image coordinates increase downwards, so we have to invert this
+	const f32 centered_x = image_x - half_width;
+	const f32 centered_y = half_height - image_y; // Image coordinates increase downwards, so we have to invert this
 
 	// Camera coordinates in mm (centered, so it's the same as world coordinates)
 	const f32 camera_x = centered_x * mmPerPixel;
@@ -949,10 +953,29 @@ static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& con
 	}
 	else
 	{
-		gem_state->quat[0] = 320.f - image_x;
-		gem_state->quat[1] = (y_pos / scaling_width) - 180.f;
-		gem_state->quat[2] = 1200.f;
-		gem_state->quat[3] = 1.f;
+		static constexpr f32 PI = 3.14159265f;
+		const auto degree_to_rad = [](f32 degree) -> f32 { return degree * PI / 180.0f; };
+
+		static constexpr f32 CONE = 10.0f / 2.0f;
+		const f32 roll = -degree_to_rad((image_y - half_height) / half_height * CONE); // This is actually the pitch
+		const f32 pitch = -degree_to_rad((image_x - half_width) / half_width * CONE); // This is actually the yaw
+		const f32 yaw = 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);
+		const f32 sp = std::sin(pitch * 0.5f);
+		const f32 cy = std::cos(yaw * 0.5f);
+		const f32 sy = std::sin(yaw * 0.5f);
+
+		const f32 q_x = sr * cp * cy - cr * sp * sy;
+		const f32 q_y = cr * sp * cy + sr * cp * sy;
+		const f32 q_z = cr * cp * sy - sr * sp * cy;
+		const f32 q_w = cr * cp * cy + sr * sp * sy;
+
+		gem_state->quat[0] = q_x;
+		gem_state->quat[1] = q_y;
+		gem_state->quat[2] = q_z;
+		gem_state->quat[3] = q_w;
 	}
 
 	if (g_cfg.io.show_move_cursor)
diff --git a/rpcs3/Input/ps_move_calibration.cpp b/rpcs3/Input/ps_move_calibration.cpp
@@ -49,82 +49,86 @@ static void psmove_dump_calibration(const reports::ps_move_calibration_blob& cal
 
 	const u8* data = calibration.data.data();
 
+	std::string msg;
+
 	switch (device.model)
 	{
 	case ps_move_model::ZCM1:
 		t = psmove_calibration_decode_12bits(data, 0x02);
-		move_log.error("Temperature: 0x%04X", t);
+		fmt::append(msg, "Temperature: 0x%04X\n", t);
 		for (int orientation = 0; orientation < 6; orientation++)
 		{
 			x = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * orientation);
 			y = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * orientation + 2);
 			z = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * orientation + 4);
-			move_log.error("Orientation #%d:      (%5d | %5d | %5d)", orientation, x, y, z);
+			fmt::append(msg, "Orientation #%d:      (%5d | %5d | %5d)\n", orientation, x, y, z);
 		}
 
 		t = psmove_calibration_decode_12bits(data, 0x42);
-		move_log.error("Temperature: 0x%04X", t);
+		fmt::append(msg, "Temperature: 0x%04X\n", t);
 		for (int orientation = 0; orientation < 3; orientation++)
 		{
 			x = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * orientation);
 			y = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * orientation + 2);
 			z = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * orientation + 4);
-			move_log.error("Gyro %c, 80 rpm:      (%5d | %5d | %5d)", "XYZ"[orientation], x, y, z);
+			fmt::append(msg, "Gyro %c, 80 rpm:      (%5d | %5d | %5d)\n", "XYZ"[orientation], x, y, z);
 		}
 
 		t = psmove_calibration_decode_12bits(data, 0x28);
 		x = psmove_calibration_decode_16bit_unsigned(data, 0x2a);
 		y = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 2);
 		z = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 4);
-		move_log.error("Temperature: 0x%04X", t);
-		move_log.error("Gyro, 0 rpm (@0x2a): (%5d | %5d | %5d)", x, y, z);
+		fmt::append(msg, "Temperature: 0x%04X\n", t);
+		fmt::append(msg, "Gyro, 0 rpm (@0x2a): (%5d | %5d | %5d)\n", x, y, z);
 
 		t = psmove_calibration_decode_12bits(data, 0x30);
 		x = psmove_calibration_decode_16bit_unsigned(data, 0x32);
 		y = psmove_calibration_decode_16bit_unsigned(data, 0x32 + 2);
 		z = psmove_calibration_decode_16bit_unsigned(data, 0x32 + 4);
-		move_log.error("Temperature: 0x%04X", t);
-		move_log.error("Gyro, 0 rpm (@0x32): (%5d | %5d | %5d)", x, y, z);
+		fmt::append(msg, "Temperature: 0x%04X\n", t);
+		fmt::append(msg, "Gyro, 0 rpm (@0x32): (%5d | %5d | %5d)\n", x, y, z);
 
 		t = psmove_calibration_decode_12bits(data, 0x5c);
 		fx = psmove_calibration_decode_float(data, 0x5e);
 		fy = psmove_calibration_decode_float(data, 0x5e + 4);
 		fz = psmove_calibration_decode_float(data, 0x5e + 8);
-		move_log.error("Temperature: 0x%04X", t);
-		move_log.error("Vector @0x5e: (%f | %f | %f)", fx, fy, fz);
+		fmt::append(msg, "Temperature: 0x%04X\n", t);
+		fmt::append(msg, "Vector @0x5e: (%f | %f | %f)\n", fx, fy, fz);
 
 		fx = psmove_calibration_decode_float(data, 0x6a);
 		fy = psmove_calibration_decode_float(data, 0x6a + 4);
 		fz = psmove_calibration_decode_float(data, 0x6a + 8);
-		move_log.error("Vector @0x6a: (%f | %f | %f)", fx, fy, fz);
+		fmt::append(msg, "Vector @0x6a: (%f | %f | %f)\n", fx, fy, fz);
 
-		move_log.error("byte @0x3f:  0x%02x", static_cast<u8>(data[0x3f]));
-		move_log.error("float @0x76: %f", psmove_calibration_decode_float(data, 0x76));
-		move_log.error("float @0x7a: %f", psmove_calibration_decode_float(data, 0x7a));
+		fmt::append(msg, "byte @0x3f:  0x%02x\n", static_cast<u8>(data[0x3f]));
+		fmt::append(msg, "float @0x76: %f\n", psmove_calibration_decode_float(data, 0x76));
+		fmt::append(msg, "float @0x7a: %f\n", psmove_calibration_decode_float(data, 0x7a));
 		break;
 	case ps_move_model::ZCM2:
 		for (int orientation = 0; orientation < 6; orientation++)
 		{
 			x = psmove_calibration_decode_16bit_signed(data, 0x04 + 6 * orientation);
 			y = psmove_calibration_decode_16bit_signed(data, 0x04 + 6 * orientation + 2);
 			z = psmove_calibration_decode_16bit_signed(data, 0x04 + 6 * orientation + 4);
-			move_log.error("Orientation #%d:      (%5d | %5d | %5d)", orientation, x, y, z);
+			fmt::append(msg, "Orientation #%d:      (%5d | %5d | %5d)\n", orientation, x, y, z);
 		}
 
 		x = psmove_calibration_decode_16bit_signed(data, 0x26);
 		y = psmove_calibration_decode_16bit_signed(data, 0x26 + 2);
 		z = psmove_calibration_decode_16bit_signed(data, 0x26 + 4);
-		move_log.error("Gyro Bias?, 0 rpm (@0x26): (%5d | %5d | %5d)", x, y, z);
+		fmt::append(msg, "Gyro Bias?, 0 rpm (@0x26): (%5d | %5d | %5d)\n", x, y, z);
 
 		for (int orientation = 0; orientation < 6; orientation++)
 		{
 			x = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * orientation);
 			y = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * orientation + 2);
 			z = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * orientation + 4);
-			move_log.error("Gyro %c, 90 rpm:      (%5d | %5d | %5d)", "XYZXYZ"[orientation], x, y, z);
+			fmt::append(msg, "Gyro %c, 90 rpm:      (%5d | %5d | %5d)\n", "XYZXYZ"[orientation], x, y, z);
 		}
 		break;
 	}
+
+	move_log.notice("Calibration:\n%s", msg);
 }
 
 void psmove_calibration_get_usb_accel_values(const reports::ps_move_calibration_blob& calibration, ps_move_device& device)
diff --git a/rpcs3/Input/ps_move_handler.cpp b/rpcs3/Input/ps_move_handler.cpp
@@ -362,6 +362,7 @@ void ps_move_handler::check_add_device(hid_device* hidDevice, std::string_view p
 	// 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);
 
