From 316f80af87c3290ad3ceda99fe9cf02f1d935b0c Mon Sep 17 00:00:00 2001 From: Narr the Reg Date: Mon, 29 Nov 2021 12:40:29 -0600 Subject: [PATCH] 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 +++-- .../hid/controllers/console_sixaxis.cpp | 20 +++++++++---------- .../service/hid/controllers/console_sixaxis.h | 10 +++++++--- 6 files changed, 32 insertions(+), 20 deletions(-) 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 orientation{}; Common::Quaternion 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& 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 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& 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 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 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; diff --git a/src/core/hle/service/hid/controllers/console_sixaxis.cpp b/src/core/hle/service/hid/controllers/console_sixaxis.cpp index f0f3105dc..a727b3582 100644 --- a/src/core/hle/service/hid/controllers/console_sixaxis.cpp +++ b/src/core/hle/service/hid/controllers/console_sixaxis.cpp @@ -33,15 +33,14 @@ void Controller_ConsoleSixAxis::OnUpdate(const Core::Timing::CoreTiming& core_ti const auto& last_entry = seven_sixaxis_lifo.ReadCurrentEntry().state; next_seven_sixaxis_state.sampling_number = last_entry.sampling_number + 1; - // Try to read sixaxis sensor states const auto motion_status = console->GetMotion(); + last_global_timestamp = core_timing.GetGlobalTimeNs().count(); - console_six_axis.is_seven_six_axis_sensor_at_rest = motion_status.is_at_rest; - + // This value increments every time the switch goes to sleep + next_seven_sixaxis_state.unknown = 1; + next_seven_sixaxis_state.timestamp = last_global_timestamp - last_saved_timestamp; next_seven_sixaxis_state.accel = motion_status.accel; - // Zero gyro values as they just mess up with the camera - // Note: Probably a correct sensivity setting must be set - next_seven_sixaxis_state.gyro = {}; + next_seven_sixaxis_state.gyro = motion_status.gyro; next_seven_sixaxis_state.quaternion = { { motion_status.quaternion.xyz.y, @@ -52,9 +51,9 @@ void Controller_ConsoleSixAxis::OnUpdate(const Core::Timing::CoreTiming& core_ti }; console_six_axis.sampling_number++; - // TODO(German77): Find the purpose of those values - console_six_axis.verticalization_error = 0.0f; - console_six_axis.gyro_bias = {0.0f, 0.0f, 0.0f}; + console_six_axis.is_seven_six_axis_sensor_at_rest = motion_status.is_at_rest; + console_six_axis.verticalization_error = motion_status.verticalization_error; + console_six_axis.gyro_bias = motion_status.gyro_bias; // Update console six axis shared memory std::memcpy(data + SHARED_MEMORY_OFFSET, &console_six_axis, sizeof(console_six_axis)); @@ -69,7 +68,6 @@ void Controller_ConsoleSixAxis::SetTransferMemoryPointer(u8* t_mem) { } void Controller_ConsoleSixAxis::ResetTimestamp() { - seven_sixaxis_lifo.buffer_count = 0; - seven_sixaxis_lifo.buffer_tail = 0; + last_saved_timestamp = last_global_timestamp; } } // namespace Service::HID diff --git a/src/core/hle/service/hid/controllers/console_sixaxis.h b/src/core/hle/service/hid/controllers/console_sixaxis.h index 279241858..26d153f0c 100644 --- a/src/core/hle/service/hid/controllers/console_sixaxis.h +++ b/src/core/hle/service/hid/controllers/console_sixaxis.h @@ -39,8 +39,9 @@ public: private: struct SevenSixAxisState { - INSERT_PADDING_WORDS(4); // unused - s64 sampling_number{}; + INSERT_PADDING_WORDS(2); // unused + u64 timestamp{}; + u64 sampling_number{}; u64 unknown{}; Common::Vec3f accel{}; Common::Vec3f gyro{}; @@ -52,9 +53,10 @@ private: struct ConsoleSharedMemory { u64 sampling_number{}; bool is_seven_six_axis_sensor_at_rest{}; - INSERT_PADDING_BYTES(4); // padding + INSERT_PADDING_BYTES(3); // padding f32 verticalization_error{}; Common::Vec3f gyro_bias{}; + INSERT_PADDING_BYTES(4); // padding }; static_assert(sizeof(ConsoleSharedMemory) == 0x20, "ConsoleSharedMemory is an invalid size"); @@ -64,6 +66,8 @@ private: Core::HID::EmulatedConsole* console; u8* transfer_memory = nullptr; bool is_transfer_memory_set = false; + u64 last_saved_timestamp{}; + u64 last_global_timestamp{}; ConsoleSharedMemory console_six_axis{}; SevenSixAxisState next_seven_sixaxis_state{}; };