Merge pull request #7481 from german77/gyro-bias

service/hid: Improve console motion accuracy

src/core/hid/emulated_console.cpp

@@ -161,7 +161,10 @@ void EmulatedConsole::SetMotion(const 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);
 }

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{};
 };
 

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;
 }

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;

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

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{};
 };