From 5a0445f1506ba11585666ddcefadefe315ea14f0 Mon Sep 17 00:00:00 2001
From: Yogi <1273750265@qq.com>
Date: Sun, 1 Dec 2024 17:42:40 +0800
Subject: [PATCH] update

---
 Assets/Scripts/GameManager.cs | 26 ++++++++++++++++++++++----
 1 file changed, 22 insertions(+), 4 deletions(-)

diff --git a/Assets/Scripts/GameManager.cs b/Assets/Scripts/GameManager.cs
index 68516ee..c85f31c 100644
--- a/Assets/Scripts/GameManager.cs
+++ b/Assets/Scripts/GameManager.cs
@@ -16,8 +16,6 @@ public class GameManager : MonoBehaviour
     public bool isTestInput = false;
     float angleX, angleY, angleZ;
 
-    public float smoothValue = 100;
-
     public void Awake()
     {
         SampleMessageListener.OnReceivedMSG += OnReceivedMessage;
@@ -61,8 +59,8 @@ public class GameManager : MonoBehaviour
             if (isCanControlModel)
             {
                 //ControlModelRotate(new Vector3(filteredRoll, filteredPitch, angleZ)); // 传入过滤后的值
-                var result = CalculateRotation(brainTrans.rotation, ax, ay, az, gx, gy, gz);
-                brainTrans.rotation = Quaternion.Slerp(brainTrans.rotation, result, Time.deltaTime * smoothValue);
+                //brainTrans.rotation = CalculateRotation(brainTrans.rotation, ax, ay, az, gx, gy, gz);
+                CalculateRotation(ax, ay, az, gx, gy, gz);
             }
         }
     }
@@ -80,6 +78,26 @@ public class GameManager : MonoBehaviour
     {
         brainTrans.rotation = Quaternion.Euler(angleModify);
     }
+
+    private void CalculateRotation(float ax, float ay, float az, float gx, float gy, float gz)
+    {
+        // 通过加速度计算姿态（俯仰角和滚转角）
+        float roll = Mathf.Atan2(ay, az) * Mathf.Rad2Deg; // 滚转角
+        float pitch = Mathf.Atan2(-ax, Mathf.Sqrt(ay * ay + az * az)) * Mathf.Rad2Deg; // 俯仰角
+
+        // 计算角速度的变化（可以设定角速度的阈值或限制其过快变化）
+        angleX += gx * dt; // 绕X轴旋转
+        angleY += gy * dt; // 绕Y轴旋转
+        angleZ += gz * dt; // 绕Z轴旋转
+
+        // 使用互补滤波来平滑姿态估计
+        // 这里的系数决定了加速度和陀螺仪的权重，通常设置为0.98
+        float alpha = 0.98f; // 互补滤波的权重
+        float filteredPitch = alpha * (angleX) + (1 - alpha) * pitch; // 过滤后的俯仰角
+        float filteredRoll = alpha * (angleY) + (1 - alpha) * roll; // 过滤后的滚转角
+        brainTrans.rotation = Quaternion.Euler(new Vector3(filteredRoll, filteredPitch, angleZ));
+    }
+
     private Quaternion CalculateRotation(Quaternion rotation, float ax, float ay, float az, float gx, float gy, float gz)
     {
         // 假设你已经有初始四元数