diff --git a/rpcs3/Emu/Cell/Modules/cellGem.cpp b/rpcs3/Emu/Cell/Modules/cellGem.cpp index ac2914f797..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 @@ -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; } diff --git a/rpcs3/Emu/Io/pad_types.h b/rpcs3/Emu/Io/pad_types.h index 27d3a73e4e..1f9552a6f8 100644 --- a/rpcs3/Emu/Io/pad_types.h +++ b/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 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² diff --git a/rpcs3/Input/ps_move_handler.cpp b/rpcs3/Input/ps_move_handler.cpp index 7b77fa2759..7b141abd25 100644 --- a/rpcs3/Input/ps_move_handler.cpp +++ b/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(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 @@ -906,18 +913,16 @@ void ps_move_device::update_orientation(ps_move_data& move_data) 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 - // } - // }; - //} + 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); diff --git a/rpcs3/Input/ps_move_handler.h b/rpcs3/Input/ps_move_handler.h index d125fad14c..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 };