From 316f80af87c3290ad3ceda99fe9cf02f1d935b0c Mon Sep 17 00:00:00 2001
From: Narr the Reg <juangerman-13@hotmail.com>
Date: Mon, 29 Nov 2021 12:40:29 -0600
Subject: service/hid: Improve console motion accuracy

---
 src/core/hid/emulated_console.cpp |  3 +++
 src/core/hid/emulated_console.h   |  2 ++
 src/core/hid/motion_input.cpp     | 12 ++++++++----
 src/core/hid/motion_input.h       |  5 +++--
 4 files changed, 16 insertions(+), 6 deletions(-)

(limited to 'src/core/hid')

diff --git a/src/core/hid/emulated_console.cpp b/src/core/hid/emulated_console.cpp
index 80db8e9c6..6744c6846 100644
--- a/src/core/hid/emulated_console.cpp
+++ b/src/core/hid/emulated_console.cpp
@@ -157,7 +157,10 @@ void EmulatedConsole::SetMotion(Common::Input::CallbackStatus callback) {
     motion.rotation = emulated.GetGyroscope();
     motion.orientation = emulated.GetOrientation();
     motion.quaternion = emulated.GetQuaternion();
+    motion.gyro_bias = emulated.GetGyroBias();
     motion.is_at_rest = !emulated.IsMoving(motion_sensitivity);
+    // Find what is this value
+    motion.verticalization_error = 0.0f;
 
     TriggerOnChange(ConsoleTriggerType::Motion);
 }
diff --git a/src/core/hid/emulated_console.h b/src/core/hid/emulated_console.h
index bb3d7ab90..e682a76c2 100644
--- a/src/core/hid/emulated_console.h
+++ b/src/core/hid/emulated_console.h
@@ -50,6 +50,8 @@ struct ConsoleMotion {
     Common::Vec3f rotation{};
     std::array<Common::Vec3f, 3> orientation{};
     Common::Quaternion<f32> quaternion{};
+    Common::Vec3f gyro_bias{};
+    f32 verticalization_error{};
     bool is_at_rest{};
 };
 
diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp
index c25fea966..a23f192d7 100644
--- a/src/core/hid/motion_input.cpp
+++ b/src/core/hid/motion_input.cpp
@@ -23,11 +23,11 @@ void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) {
 }
 
 void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
-    gyro = gyroscope - gyro_drift;
+    gyro = gyroscope - gyro_bias;
 
     // Auto adjust drift to minimize drift
     if (!IsMoving(0.1f)) {
-        gyro_drift = (gyro_drift * 0.9999f) + (gyroscope * 0.0001f);
+        gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
     }
 
     if (gyro.Length2() < gyro_threshold) {
@@ -41,8 +41,8 @@ void MotionInput::SetQuaternion(const Common::Quaternion<f32>& quaternion) {
     quat = quaternion;
 }
 
-void MotionInput::SetGyroDrift(const Common::Vec3f& drift) {
-    gyro_drift = drift;
+void MotionInput::SetGyroBias(const Common::Vec3f& bias) {
+    gyro_bias = bias;
 }
 
 void MotionInput::SetGyroThreshold(f32 threshold) {
@@ -192,6 +192,10 @@ Common::Vec3f MotionInput::GetGyroscope() const {
     return gyro;
 }
 
+Common::Vec3f MotionInput::GetGyroBias() const {
+    return gyro_bias;
+}
+
 Common::Quaternion<f32> MotionInput::GetQuaternion() const {
     return quat;
 }
diff --git a/src/core/hid/motion_input.h b/src/core/hid/motion_input.h
index 5b5b420bb..bca4520fa 100644
--- a/src/core/hid/motion_input.h
+++ b/src/core/hid/motion_input.h
@@ -24,7 +24,7 @@ public:
     void SetAcceleration(const Common::Vec3f& acceleration);
     void SetGyroscope(const Common::Vec3f& gyroscope);
     void SetQuaternion(const Common::Quaternion<f32>& quaternion);
-    void SetGyroDrift(const Common::Vec3f& drift);
+    void SetGyroBias(const Common::Vec3f& bias);
     void SetGyroThreshold(f32 threshold);
 
     void EnableReset(bool reset);
@@ -36,6 +36,7 @@ public:
     [[nodiscard]] std::array<Common::Vec3f, 3> GetOrientation() const;
     [[nodiscard]] Common::Vec3f GetAcceleration() const;
     [[nodiscard]] Common::Vec3f GetGyroscope() const;
+    [[nodiscard]] Common::Vec3f GetGyroBias() const;
     [[nodiscard]] Common::Vec3f GetRotations() const;
     [[nodiscard]] Common::Quaternion<f32> GetQuaternion() const;
 
@@ -69,7 +70,7 @@ private:
     Common::Vec3f gyro;
 
     // Vector to be substracted from gyro measurements
-    Common::Vec3f gyro_drift;
+    Common::Vec3f gyro_bias;
 
     // Minimum gyro amplitude to detect if the device is moving
     f32 gyro_threshold = 0.0f;
-- 
cgit v1.2.3