cellGem: add magnetometer support

This commit is contained in:
Megamouse 2024-12-21 00:55:05 +01:00
parent 1cf927aef9
commit d450a5ee95
4 changed files with 58 additions and 21 deletions

View File

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

View File

@ -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²

View File

@ -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 (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);

View File

@ -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<u8, 5> ext_device_data{}; // 0x2C 5 External device data
};