core: hid: Restore motion state on refresh and clamp motion values

This commit is contained in:
Narr the Reg 2023-02-21 21:47:47 -06:00
parent 673accd630
commit 739a81055f
3 changed files with 30 additions and 2 deletions

View File

@ -363,7 +363,17 @@ void EmulatedController::ReloadInput() {
SetMotion(callback, index); SetMotion(callback, index);
}, },
}); });
motion_devices[index]->ForceUpdate();
// Restore motion state
auto& emulated_motion = controller.motion_values[index].emulated;
auto& motion = controller.motion_state[index];
emulated_motion.ResetRotations();
emulated_motion.ResetQuaternion();
motion.accel = emulated_motion.GetAcceleration();
motion.gyro = emulated_motion.GetGyroscope();
motion.rotation = emulated_motion.GetRotations();
motion.orientation = emulated_motion.GetOrientation();
motion.is_at_rest = !emulated_motion.IsMoving(motion_sensitivity);
} }
for (std::size_t index = 0; index < camera_devices.size(); ++index) { for (std::size_t index = 0; index < camera_devices.size(); ++index) {

View File

@ -10,6 +10,8 @@ MotionInput::MotionInput() {
// Initialize PID constants with default values // Initialize PID constants with default values
SetPID(0.3f, 0.005f, 0.0f); SetPID(0.3f, 0.005f, 0.0f);
SetGyroThreshold(ThresholdStandard); SetGyroThreshold(ThresholdStandard);
ResetQuaternion();
ResetRotations();
} }
void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) { void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
@ -20,11 +22,19 @@ void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) { void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) {
accel = acceleration; accel = acceleration;
accel.x = std::clamp(accel.x, -AccelMaxValue, AccelMaxValue);
accel.y = std::clamp(accel.y, -AccelMaxValue, AccelMaxValue);
accel.z = std::clamp(accel.z, -AccelMaxValue, AccelMaxValue);
} }
void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) { void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
gyro = gyroscope - gyro_bias; gyro = gyroscope - gyro_bias;
gyro.x = std::clamp(gyro.x, -GyroMaxValue, GyroMaxValue);
gyro.y = std::clamp(gyro.y, -GyroMaxValue, GyroMaxValue);
gyro.z = std::clamp(gyro.z, -GyroMaxValue, GyroMaxValue);
// Auto adjust drift to minimize drift // Auto adjust drift to minimize drift
if (!IsMoving(IsAtRestRelaxed)) { if (!IsMoving(IsAtRestRelaxed)) {
gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f); gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
@ -61,6 +71,10 @@ void MotionInput::ResetRotations() {
rotations = {}; rotations = {};
} }
void MotionInput::ResetQuaternion() {
quat = {{0.0f, 0.0f, -1.0f}, 0.0f};
}
bool MotionInput::IsMoving(f32 sensitivity) const { bool MotionInput::IsMoving(f32 sensitivity) const {
return gyro.Length() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f; return gyro.Length() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f;
} }

View File

@ -20,6 +20,9 @@ public:
static constexpr float IsAtRestStandard = 0.01f; static constexpr float IsAtRestStandard = 0.01f;
static constexpr float IsAtRestThight = 0.005f; static constexpr float IsAtRestThight = 0.005f;
static constexpr float GyroMaxValue = 5.0f;
static constexpr float AccelMaxValue = 7.0f;
explicit MotionInput(); explicit MotionInput();
MotionInput(const MotionInput&) = default; MotionInput(const MotionInput&) = default;
@ -40,6 +43,7 @@ public:
void EnableReset(bool reset); void EnableReset(bool reset);
void ResetRotations(); void ResetRotations();
void ResetQuaternion();
void UpdateRotation(u64 elapsed_time); void UpdateRotation(u64 elapsed_time);
void UpdateOrientation(u64 elapsed_time); void UpdateOrientation(u64 elapsed_time);
@ -69,7 +73,7 @@ private:
Common::Vec3f derivative_error; Common::Vec3f derivative_error;
// Quaternion containing the device orientation // Quaternion containing the device orientation
Common::Quaternion<f32> quat{{0.0f, 0.0f, -1.0f}, 0.0f}; Common::Quaternion<f32> quat;
// Number of full rotations in each axis // Number of full rotations in each axis
Common::Vec3f rotations; Common::Vec3f rotations;