From 2d207ec609d1280bbfcdb822cceb8adc42afac3a Mon Sep 17 00:00:00 2001 From: german Date: Sun, 23 Aug 2020 19:26:47 -0500 Subject: Implement a basic class for motion devices --- src/input_common/CMakeLists.txt | 2 + src/input_common/motion_input.cpp | 185 ++++++++++++++++++++++++++++++++++++++ src/input_common/motion_input.h | 62 +++++++++++++ 3 files changed, 249 insertions(+) create mode 100644 src/input_common/motion_input.cpp create mode 100644 src/input_common/motion_input.h (limited to 'src/input_common') diff --git a/src/input_common/CMakeLists.txt b/src/input_common/CMakeLists.txt index 56267c8a8..c673b8389 100644 --- a/src/input_common/CMakeLists.txt +++ b/src/input_common/CMakeLists.txt @@ -7,6 +7,8 @@ add_library(input_common STATIC main.h motion_emu.cpp motion_emu.h + motion_input.cpp + motion_input.h settings.cpp settings.h gcadapter/gc_adapter.cpp diff --git a/src/input_common/motion_input.cpp b/src/input_common/motion_input.cpp new file mode 100644 index 000000000..e3aec04e1 --- /dev/null +++ b/src/input_common/motion_input.cpp @@ -0,0 +1,185 @@ +#include "input_common/motion_input.h" + +namespace InputCommon { + +MotionInput::MotionInput(f32 new_kp, f32 new_ki, f32 new_kd) : kp(new_kp), ki(new_ki), kd(new_kd) { + accel = {}; + gyro = {}; + gyro_drift = {}; + gyro_threshold = 0; + rotations = {}; + + quat.w = 0; + quat.xyz[0] = 0; + quat.xyz[1] = 0; + quat.xyz[2] = -1; + + real_error = {}; + integral_error = {}; + derivative_error = {}; + + reset_counter = 0; + reset_enabled = true; +} + +void MotionInput::SetAcceleration(Common::Vec3f acceleration) { + accel = acceleration; +} + +void MotionInput::SetGyroscope(Common::Vec3f gyroscope) { + gyro = gyroscope - gyro_drift; + if (gyro.Length2() < gyro_threshold) { + gyro = {}; + } +} + +void MotionInput::SetQuaternion(Common::Quaternion quaternion) { + quat = quaternion; +} + +void MotionInput::SetGyroDrift(Common::Vec3f drift) { + drift = gyro_drift; +} + +void MotionInput::SetGyroThreshold(f32 threshold) { + gyro_threshold = threshold; +} + +void MotionInput::EnableReset(bool reset) { + reset_enabled = reset; +} + +void MotionInput::ResetRotations() { + rotations = {}; +} + +bool MotionInput::IsMoving(f32 sensitivity) { + return gyro.Length2() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f; +} + +bool MotionInput::IsCalibrated(f32 sensitivity) { + return real_error.Length() > sensitivity; +} + +void MotionInput::UpdateRotation(u64 elapsed_time) { + rotations += gyro * elapsed_time; +} + +void MotionInput::UpdateOrientation(u64 elapsed_time) { + // Short name local variable for readability + f32 q1 = quat.w, q2 = quat.xyz[0], q3 = quat.xyz[1], q4 = quat.xyz[2]; + f32 sample_period = elapsed_time / 1000000.0f; + + auto normal_accel = accel.Normalized(); + auto rad_gyro = gyro * 3.1415926535f; + rad_gyro.z = -rad_gyro.z; + + // Ignore drift correction if acceleration is not present + if (normal_accel.Length() == 1.0f) { + f32 ax = -normal_accel.x; + f32 ay = normal_accel.y; + f32 az = -normal_accel.z; + f32 vx, vy, vz; + Common::Vec3f new_real_error; + + // Estimated direction of gravity + vx = 2.0f * (q2 * q4 - q1 * q3); + vy = 2.0f * (q1 * q2 + q3 * q4); + vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; + + // Error is cross product between estimated direction and measured direction of gravity + new_real_error.x = ay * vz - az * vy; + new_real_error.y = az * vx - ax * vz; + new_real_error.x = ax * vy - ay * vx; + + derivative_error = new_real_error - real_error; + real_error = new_real_error; + + // Prevent integral windup + if (ki != 0.0f) { + integral_error += real_error; + } else { + integral_error = {}; + } + + // Apply feedback terms + rad_gyro += kp * real_error; + rad_gyro += ki * integral_error; + rad_gyro += kd * derivative_error; + } + + f32 gx = rad_gyro.y; + f32 gy = rad_gyro.x; + f32 gz = rad_gyro.z; + + // Integrate rate of change of quaternion + f32 pa, pb, pc; + pa = q2; + pb = q3; + pc = q4; + q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period); + q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period); + q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period); + q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period); + + quat.w = q1; + quat.xyz[0] = q2; + quat.xyz[1] = q3; + quat.xyz[2] = q4; + quat = quat.Normalized(); +} + +std::array MotionInput::GetOrientation() { + std::array orientation = {}; + Common::Quaternion quad; + + quad.w = -quat.xyz[2]; + quad.xyz[0] = -quat.xyz[1]; + quad.xyz[1] = -quat.xyz[0]; + quad.xyz[2] = -quat.w; + + std::array matrix4x4 = quad.ToMatrix(); + + orientation[0] = Common::Vec3f(matrix4x4[0], matrix4x4[1], matrix4x4[2]); + orientation[1] = Common::Vec3f(matrix4x4[4], matrix4x4[5], matrix4x4[6]); + orientation[2] = Common::Vec3f(matrix4x4[8], matrix4x4[9], matrix4x4[10]); + + return orientation; +} + +Common::Vec3f MotionInput::GetAcceleration() { + return accel; +} + +Common::Vec3f MotionInput::GetGyroscope() { + return gyro; +} + +Common::Quaternion MotionInput::GetQuaternion() { + return quat; +} + +Common::Vec3f MotionInput::GetRotations() { + return rotations; +} + +void MotionInput::resetOrientation() { + if (!reset_enabled) { + return; + } + if (!IsMoving(0.5f) && accel.z <= -0.9f) { + ++reset_counter; + if (reset_counter > 900) { + // TODO: calculate quaternion from gravity vector + quat.w = 0; + quat.xyz[0] = 0; + quat.xyz[1] = 0; + quat.xyz[2] = -1; + integral_error = {}; + reset_counter = 0; + } + } else { + reset_counter = 0; + } +} +} // namespace InputCommon \ No newline at end of file diff --git a/src/input_common/motion_input.h b/src/input_common/motion_input.h new file mode 100644 index 000000000..4b8093d8c --- /dev/null +++ b/src/input_common/motion_input.h @@ -0,0 +1,62 @@ +// Copyright 2014 Dolphin Emulator Project +// Licensed under GPLv2+ +// Refer to the license.txt file included. + +#pragma once + +#include "common/common_types.h" +#include "common/quaternion.h" +#include "common/vector_math.h" + +namespace InputCommon { + +class MotionInput { +public: + MotionInput(f32 new_kp, f32 new_ki, f32 new_kd); + + void SetAcceleration(Common::Vec3f acceleration); + void SetGyroscope(Common::Vec3f acceleration); + void SetQuaternion(Common::Quaternion quaternion); + void SetGyroDrift(Common::Vec3f drift); + void SetGyroThreshold(f32 threshold); + + void EnableReset(bool reset); + void ResetRotations(); + + void UpdateRotation(u64 elapsed_time); + void UpdateOrientation(u64 elapsed_time); + + std::array GetOrientation(); + Common::Vec3f GetAcceleration(); + Common::Vec3f GetGyroscope(); + Common::Vec3f GetRotations(); + Common::Quaternion GetQuaternion(); + + bool IsMoving(f32 sensitivity); + bool IsCalibrated(f32 sensitivity); + + // PID constants + const f32 kp; + const f32 ki; + const f32 kd; + +private: + void resetOrientation(); + + // PID errors + Common::Vec3f real_error; + Common::Vec3f integral_error; + Common::Vec3f derivative_error; + + Common::Quaternion quat; + Common::Vec3f rotations; + Common::Vec3f accel; + Common::Vec3f gyro; + Common::Vec3f gyro_drift; + + f32 gyro_threshold; + f32 reset_counter; + bool reset_enabled; +}; + +} // namespace InputCommon -- cgit v1.2.3 From e6fc3b5662d441db7e1191a857bbb1b6ec1b06f4 Mon Sep 17 00:00:00 2001 From: german Date: Sun, 23 Aug 2020 20:41:59 -0500 Subject: Address comments --- src/input_common/motion_input.cpp | 110 +++++++++++++++----------------------- src/input_common/motion_input.h | 40 ++++++++------ 2 files changed, 65 insertions(+), 85 deletions(-) (limited to 'src/input_common') diff --git a/src/input_common/motion_input.cpp b/src/input_common/motion_input.cpp index e3aec04e1..c86a53106 100644 --- a/src/input_common/motion_input.cpp +++ b/src/input_common/motion_input.cpp @@ -2,43 +2,26 @@ namespace InputCommon { -MotionInput::MotionInput(f32 new_kp, f32 new_ki, f32 new_kd) : kp(new_kp), ki(new_ki), kd(new_kd) { - accel = {}; - gyro = {}; - gyro_drift = {}; - gyro_threshold = 0; - rotations = {}; - - quat.w = 0; - quat.xyz[0] = 0; - quat.xyz[1] = 0; - quat.xyz[2] = -1; - - real_error = {}; - integral_error = {}; - derivative_error = {}; - - reset_counter = 0; - reset_enabled = true; -} +MotionInput::MotionInput(f32 new_kp, f32 new_ki, f32 new_kd) + : kp(new_kp), ki(new_ki), kd(new_kd), quat{{0, 0, -1}, 0} {} -void MotionInput::SetAcceleration(Common::Vec3f acceleration) { +void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) { accel = acceleration; } -void MotionInput::SetGyroscope(Common::Vec3f gyroscope) { +void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) { gyro = gyroscope - gyro_drift; if (gyro.Length2() < gyro_threshold) { gyro = {}; } } -void MotionInput::SetQuaternion(Common::Quaternion quaternion) { +void MotionInput::SetQuaternion(const Common::Quaternion& quaternion) { quat = quaternion; } -void MotionInput::SetGyroDrift(Common::Vec3f drift) { - drift = gyro_drift; +void MotionInput::SetGyroDrift(const Common::Vec3f& drift) { + gyro_drift = drift; } void MotionInput::SetGyroThreshold(f32 threshold) { @@ -53,11 +36,11 @@ void MotionInput::ResetRotations() { rotations = {}; } -bool MotionInput::IsMoving(f32 sensitivity) { +bool MotionInput::IsMoving(f32 sensitivity) const { return gyro.Length2() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f; } -bool MotionInput::IsCalibrated(f32 sensitivity) { +bool MotionInput::IsCalibrated(f32 sensitivity) const { return real_error.Length() > sensitivity; } @@ -67,30 +50,30 @@ void MotionInput::UpdateRotation(u64 elapsed_time) { void MotionInput::UpdateOrientation(u64 elapsed_time) { // Short name local variable for readability - f32 q1 = quat.w, q2 = quat.xyz[0], q3 = quat.xyz[1], q4 = quat.xyz[2]; - f32 sample_period = elapsed_time / 1000000.0f; + f32 q1 = quat.w; + f32 q2 = quat.xyz[0]; + f32 q3 = quat.xyz[1]; + f32 q4 = quat.xyz[2]; + const f32 sample_period = elapsed_time / 1000000.0f; - auto normal_accel = accel.Normalized(); + const auto normal_accel = accel.Normalized(); auto rad_gyro = gyro * 3.1415926535f; rad_gyro.z = -rad_gyro.z; // Ignore drift correction if acceleration is not present if (normal_accel.Length() == 1.0f) { - f32 ax = -normal_accel.x; - f32 ay = normal_accel.y; - f32 az = -normal_accel.z; - f32 vx, vy, vz; - Common::Vec3f new_real_error; + const f32 ax = -normal_accel.x; + const f32 ay = normal_accel.y; + const f32 az = -normal_accel.z; // Estimated direction of gravity - vx = 2.0f * (q2 * q4 - q1 * q3); - vy = 2.0f * (q1 * q2 + q3 * q4); - vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; + const f32 vx = 2.0f * (q2 * q4 - q1 * q3); + const f32 vy = 2.0f * (q1 * q2 + q3 * q4); + const f32 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; // Error is cross product between estimated direction and measured direction of gravity - new_real_error.x = ay * vz - az * vy; - new_real_error.y = az * vx - ax * vz; - new_real_error.x = ax * vy - ay * vx; + const Common::Vec3f new_real_error = {ay * vz - az * vy, az * vx - ax * vz, + ax * vy - ay * vx}; derivative_error = new_real_error - real_error; real_error = new_real_error; @@ -108,15 +91,14 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { rad_gyro += kd * derivative_error; } - f32 gx = rad_gyro.y; - f32 gy = rad_gyro.x; - f32 gz = rad_gyro.z; + const f32 gx = rad_gyro.y; + const f32 gy = rad_gyro.x; + const f32 gz = rad_gyro.z; // Integrate rate of change of quaternion - f32 pa, pb, pc; - pa = q2; - pb = q3; - pc = q4; + const f32 pa = q2; + const f32 pb = q3; + const f32 pc = q4; q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period); q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period); q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period); @@ -129,41 +111,33 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { quat = quat.Normalized(); } -std::array MotionInput::GetOrientation() { - std::array orientation = {}; - Common::Quaternion quad; - - quad.w = -quat.xyz[2]; - quad.xyz[0] = -quat.xyz[1]; - quad.xyz[1] = -quat.xyz[0]; - quad.xyz[2] = -quat.w; - - std::array matrix4x4 = quad.ToMatrix(); - - orientation[0] = Common::Vec3f(matrix4x4[0], matrix4x4[1], matrix4x4[2]); - orientation[1] = Common::Vec3f(matrix4x4[4], matrix4x4[5], matrix4x4[6]); - orientation[2] = Common::Vec3f(matrix4x4[8], matrix4x4[9], matrix4x4[10]); +std::array MotionInput::GetOrientation() const { + const Common::Quaternion quad{.xyz = {-quat.xyz[1], -quat.xyz[0], -quat.w}, + .w = -quat.xyz[2]}; + const std::array matrix4x4 = quad.ToMatrix(); - return orientation; + return {Common::Vec3f(matrix4x4[0], matrix4x4[1], matrix4x4[2]), + Common::Vec3f(matrix4x4[4], matrix4x4[5], matrix4x4[6]), + Common::Vec3f(matrix4x4[8], matrix4x4[9], matrix4x4[10])}; } -Common::Vec3f MotionInput::GetAcceleration() { +Common::Vec3f MotionInput::GetAcceleration() const { return accel; } -Common::Vec3f MotionInput::GetGyroscope() { +Common::Vec3f MotionInput::GetGyroscope() const { return gyro; } -Common::Quaternion MotionInput::GetQuaternion() { +Common::Quaternion MotionInput::GetQuaternion() const { return quat; } -Common::Vec3f MotionInput::GetRotations() { +Common::Vec3f MotionInput::GetRotations() const { return rotations; } -void MotionInput::resetOrientation() { +void MotionInput::ResetOrientation() { if (!reset_enabled) { return; } @@ -182,4 +156,4 @@ void MotionInput::resetOrientation() { reset_counter = 0; } } -} // namespace InputCommon \ No newline at end of file +} // namespace InputCommon diff --git a/src/input_common/motion_input.h b/src/input_common/motion_input.h index 4b8093d8c..d1a7a9e13 100644 --- a/src/input_common/motion_input.h +++ b/src/input_common/motion_input.h @@ -14,10 +14,16 @@ class MotionInput { public: MotionInput(f32 new_kp, f32 new_ki, f32 new_kd); - void SetAcceleration(Common::Vec3f acceleration); - void SetGyroscope(Common::Vec3f acceleration); - void SetQuaternion(Common::Quaternion quaternion); - void SetGyroDrift(Common::Vec3f drift); + MotionInput(const MotionInput&) = default; + MotionInput& operator=(const MotionInput&) = default; + + MotionInput(MotionInput&&) = default; + MotionInput& operator=(MotionInput&&) = default; + + void SetAcceleration(const Common::Vec3f& acceleration); + void SetGyroscope(const Common::Vec3f& acceleration); + void SetQuaternion(const Common::Quaternion& quaternion); + void SetGyroDrift(const Common::Vec3f& drift); void SetGyroThreshold(f32 threshold); void EnableReset(bool reset); @@ -26,23 +32,23 @@ public: void UpdateRotation(u64 elapsed_time); void UpdateOrientation(u64 elapsed_time); - std::array GetOrientation(); - Common::Vec3f GetAcceleration(); - Common::Vec3f GetGyroscope(); - Common::Vec3f GetRotations(); - Common::Quaternion GetQuaternion(); + std::array GetOrientation() const; + Common::Vec3f GetAcceleration() const; + Common::Vec3f GetGyroscope() const; + Common::Vec3f GetRotations() const; + Common::Quaternion GetQuaternion() const; + + bool IsMoving(f32 sensitivity) const; + bool IsCalibrated(f32 sensitivity) const; - bool IsMoving(f32 sensitivity); - bool IsCalibrated(f32 sensitivity); +private: + void ResetOrientation(); // PID constants const f32 kp; const f32 ki; const f32 kd; -private: - void resetOrientation(); - // PID errors Common::Vec3f real_error; Common::Vec3f integral_error; @@ -54,9 +60,9 @@ private: Common::Vec3f gyro; Common::Vec3f gyro_drift; - f32 gyro_threshold; - f32 reset_counter; - bool reset_enabled; + f32 gyro_threshold = 0.0f; + f32 reset_counter = 0.0f; + bool reset_enabled = true; }; } // namespace InputCommon -- cgit v1.2.3 From 1be18dc110e1c7193930c62542855c91179d179b Mon Sep 17 00:00:00 2001 From: german Date: Wed, 26 Aug 2020 16:49:24 -0500 Subject: Fix orientation errors and improve drift correction --- src/input_common/motion_input.cpp | 43 +++++++++++++++++++++++++++------------ src/input_common/motion_input.h | 2 +- 2 files changed, 31 insertions(+), 14 deletions(-) (limited to 'src/input_common') diff --git a/src/input_common/motion_input.cpp b/src/input_common/motion_input.cpp index c86a53106..88fddb1b9 100644 --- a/src/input_common/motion_input.cpp +++ b/src/input_common/motion_input.cpp @@ -37,18 +37,25 @@ void MotionInput::ResetRotations() { } bool MotionInput::IsMoving(f32 sensitivity) const { - return gyro.Length2() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f; + return gyro.Length() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f; } bool MotionInput::IsCalibrated(f32 sensitivity) const { - return real_error.Length() > sensitivity; + return real_error.Length() < sensitivity; } void MotionInput::UpdateRotation(u64 elapsed_time) { - rotations += gyro * elapsed_time; + const f32 sample_period = elapsed_time / 1000000.0f; + if (sample_period > 0.1f) { + return; + } + rotations += gyro * sample_period; } void MotionInput::UpdateOrientation(u64 elapsed_time) { + if (!IsCalibrated(0.1f)) { + ResetOrientation(); + } // Short name local variable for readability f32 q1 = quat.w; f32 q2 = quat.xyz[0]; @@ -56,12 +63,20 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { f32 q4 = quat.xyz[2]; const f32 sample_period = elapsed_time / 1000000.0f; + // ignore invalid elapsed time + if (sample_period > 0.1f) { + return; + } + const auto normal_accel = accel.Normalized(); - auto rad_gyro = gyro * 3.1415926535f; + auto rad_gyro = gyro * 3.1415926535f * 2; + const f32 swap = rad_gyro.x; + rad_gyro.x = rad_gyro.y; + rad_gyro.y = -swap; rad_gyro.z = -rad_gyro.z; - // Ignore drift correction if acceleration is not present - if (normal_accel.Length() == 1.0f) { + // Ignore drift correction if acceleration is not reliable + if (accel.Length() >= 0.75f && accel.Length() <= 1.25f) { const f32 ax = -normal_accel.x; const f32 ay = normal_accel.y; const f32 az = -normal_accel.z; @@ -72,14 +87,14 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { const f32 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; // Error is cross product between estimated direction and measured direction of gravity - const Common::Vec3f new_real_error = {ay * vz - az * vy, az * vx - ax * vz, + const Common::Vec3f new_real_error = {az * vx - ax * vz, ay * vz - az * vy, ax * vy - ay * vx}; derivative_error = new_real_error - real_error; real_error = new_real_error; // Prevent integral windup - if (ki != 0.0f) { + if (ki != 0.0f && !IsCalibrated(0.05f)) { integral_error += real_error; } else { integral_error = {}; @@ -112,13 +127,15 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { } std::array MotionInput::GetOrientation() const { - const Common::Quaternion quad{.xyz = {-quat.xyz[1], -quat.xyz[0], -quat.w}, - .w = -quat.xyz[2]}; + const Common::Quaternion quad{ + .xyz = {-quat.xyz[1], -quat.xyz[0], -quat.w}, + .w = -quat.xyz[2], + }; const std::array matrix4x4 = quad.ToMatrix(); - return {Common::Vec3f(matrix4x4[0], matrix4x4[1], matrix4x4[2]), - Common::Vec3f(matrix4x4[4], matrix4x4[5], matrix4x4[6]), - Common::Vec3f(matrix4x4[8], matrix4x4[9], matrix4x4[10])}; + return {Common::Vec3f(matrix4x4[0], matrix4x4[1], -matrix4x4[2]), + Common::Vec3f(matrix4x4[4], matrix4x4[5], -matrix4x4[6]), + Common::Vec3f(-matrix4x4[8], -matrix4x4[9], matrix4x4[10])}; } Common::Vec3f MotionInput::GetAcceleration() const { diff --git a/src/input_common/motion_input.h b/src/input_common/motion_input.h index d1a7a9e13..445798a54 100644 --- a/src/input_common/motion_input.h +++ b/src/input_common/motion_input.h @@ -61,7 +61,7 @@ private: Common::Vec3f gyro_drift; f32 gyro_threshold = 0.0f; - f32 reset_counter = 0.0f; + u32 reset_counter = 0; bool reset_enabled = true; }; -- cgit v1.2.3