diff --git a/src/dusk/gyro.cpp b/src/dusk/gyro.cpp index 5b6e880a1f..abe22909c1 100644 --- a/src/dusk/gyro.cpp +++ b/src/dusk/gyro.cpp @@ -1,16 +1,29 @@ #include "dusk/gyro.h" #include "d/actor/d_a_alink.h" +#include namespace dusk::gyro { namespace { constexpr s32 kRollgoalTableMaxOffset = 6500; constexpr float kGyroEmaAlphaMin = 0.05f; constexpr float kGyroEmaAlphaMax = 1.0f; +// Smooth gravity separately so the yaw/roll blend doesn't twitch with raw accel noise. +constexpr float kGravityEmaAlpha = 0.1f; +constexpr float kMinGravityProjection = 0.2f; +// Let roll contribute more strongly as the pad approaches an upright posture. +constexpr float kRollAimBoostMax = 2.0f; bool s_sensor_enabled = false; +bool s_accel_enabled = false; +bool s_was_aiming = false; +bool s_have_gravity_baseline = false; float s_smooth_gx = 0.0f; float s_smooth_gy = 0.0f; float s_smooth_gz = 0.0f; +float s_gravity_y = 0.0f; +float s_gravity_z = 0.0f; +float s_baseline_gravity_y = 0.0f; +float s_baseline_gravity_z = 0.0f; float s_yaw_rad = 0.0f; float s_pitch_rad = 0.0f; float s_roll_rad = 0.0f; @@ -19,6 +32,10 @@ s32 s_rollgoal_az = 0; void reset_filter_state() { s_smooth_gx = s_smooth_gy = s_smooth_gz = 0.0f; + s_gravity_y = s_gravity_z = 0.0f; + s_baseline_gravity_y = s_baseline_gravity_z = 0.0f; + s_was_aiming = false; + s_have_gravity_baseline = false; s_yaw_rad = s_pitch_rad = s_roll_rad = 0.0f; s_rollgoal_ax = s_rollgoal_az = 0; } @@ -49,15 +66,30 @@ bool queryGyroAimContext() { } void read(float dt) { - if (!s_sensor_keep_alive && !queryGyroAimContext()) { + const bool aim_active = queryGyroAimContext(); + const bool aim_just_started = aim_active && !s_was_aiming; + const bool aim_just_ended = !aim_active && s_was_aiming; + s_was_aiming = aim_active; + + if (!s_sensor_keep_alive && !aim_active) { if (s_sensor_enabled) { PADSetSensorEnabled(PAD_CHAN0, PAD_SENSOR_GYRO, FALSE); s_sensor_enabled = false; } + if (s_accel_enabled) { + PADSetSensorEnabled(PAD_CHAN0, PAD_SENSOR_ACCEL, FALSE); + s_accel_enabled = false; + } reset_filter_state(); return; } + if (aim_just_started || aim_just_ended) { + s_gravity_y = s_gravity_z = 0.0f; + s_baseline_gravity_y = s_baseline_gravity_z = 0.0f; + s_have_gravity_baseline = false; + } + if (!s_sensor_enabled) { if (!PADHasSensor(PAD_CHAN0, PAD_SENSOR_GYRO)) { return; @@ -68,6 +100,13 @@ void read(float dt) { s_sensor_enabled = true; } + if (!s_accel_enabled && PADHasSensor(PAD_CHAN0, PAD_SENSOR_ACCEL) && + PADSetSensorEnabled(PAD_CHAN0, PAD_SENSOR_ACCEL, TRUE)) + { + // We only need accel for the gravity-aware yaw/roll mix. + s_accel_enabled = true; + } + f32 gyro[3]; if (!PADGetSensorData(PAD_CHAN0, PAD_SENSOR_GYRO, gyro, 3)) { return; @@ -80,9 +119,50 @@ void read(float dt) { s_smooth_gy += smooth_alpha * (gyro[1] - s_smooth_gy); s_smooth_gz += smooth_alpha * (gyro[2] - s_smooth_gz); - s_pitch_rad = -apply_deadband(s_smooth_gx, deadband) * dt * dusk::getSettings().game.gyroSensitivityX; - s_yaw_rad = apply_deadband(s_smooth_gy, deadband) * dt * dusk::getSettings().game.gyroSensitivityY; - s_roll_rad = apply_deadband(s_smooth_gz, deadband) * dt * dusk::getSettings().game.gyroSensitivityX; // GYRO NOTE: Exposing Z sensitivity seems unusual, so I'm just using X + const float pitch_rate = apply_deadband(s_smooth_gx, deadband); + const float yaw_rate = apply_deadband(s_smooth_gy, deadband); + const float roll_rate = apply_deadband(s_smooth_gz, deadband); + + s_pitch_rad = -pitch_rate * dt * dusk::getSettings().game.gyroSensitivityX; + s_roll_rad = roll_rate * dt * dusk::getSettings().game.gyroSensitivityX; // GYRO NOTE: Exposing Z sensitivity seems unusual, so I'm just using X + + float horizontal_rate = yaw_rate; + if (aim_active && s_accel_enabled) { + f32 accel[3]; + if (PADGetSensorData(PAD_CHAN0, PAD_SENSOR_ACCEL, accel, 3)) { + if (!s_have_gravity_baseline) { + s_gravity_y = accel[1]; + s_gravity_z = accel[2]; + } else { + s_gravity_y += kGravityEmaAlpha * (accel[1] - s_gravity_y); + s_gravity_z += kGravityEmaAlpha * (accel[2] - s_gravity_z); + } + + // Compare the current gravity projection against the gravity vector from + // aim start so the user's resting hold angle becomes the neutral baseline. + const float gravity_yz_len = std::sqrt((s_gravity_y * s_gravity_y) + (s_gravity_z * s_gravity_z)); + if (gravity_yz_len >= kMinGravityProjection) { + const float current_gravity_y = s_gravity_y / gravity_yz_len; + const float current_gravity_z = s_gravity_z / gravity_yz_len; + + if (!s_have_gravity_baseline) { + s_baseline_gravity_y = current_gravity_y; + s_baseline_gravity_z = current_gravity_z; + s_have_gravity_baseline = true; + } + + const float yaw_weight = + (s_baseline_gravity_y * current_gravity_y) + (s_baseline_gravity_z * current_gravity_z); + const float roll_weight = + (s_baseline_gravity_y * current_gravity_z) - (s_baseline_gravity_z * current_gravity_y); + const float roll_mix = std::fabs(roll_weight); + const float roll_boost = 1.0f + (roll_mix * (kRollAimBoostMax - 1.0f)); + horizontal_rate = (yaw_rate * yaw_weight) + (roll_rate * roll_weight * roll_boost); + } + } + } + + s_yaw_rad = horizontal_rate * dt * dusk::getSettings().game.gyroSensitivityY; s_pitch_rad = dusk::getSettings().game.gyroInvertPitch ? -s_pitch_rad : s_pitch_rad; s_yaw_rad = dusk::getSettings().game.gyroInvertYaw ? -s_yaw_rad : s_yaw_rad;