九轴打印

Assets/BowArrow/Scripts/Bluetooth/New/Axis9Handler.cs

@@ -25,6 +25,8 @@ public class Axis9Handler : AxisBaseHandler
         _9Axis = new o0.Bow.o09AxisAfterXiaMenFromDll(GyrByteIndex, AccByteIndex, MagByteIndex);
         _9Axis.Attitude = new o0.IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
         _9Axis.LoadIdentity();
+        Debug.Log("9轴启动成功（首次初始化）");
+        PrintAxisInfo();
     }
 
     public override void Update(byte[] bytes)
@@ -73,10 +75,12 @@ public class Axis9Handler : AxisBaseHandler
     public override void ResetGyr() {
         _9Axis.Attitude.GyrCalibrate = true;
         _9Axis.Attitude.GyrCalibrate = false;
+        Debug.Log("陀螺仪校准结果主动重置！");
     }
 
     public override void ResetMag() {
         _9Axis.Attitude.MagCalibrater = new o0.IMU.MagnetometerAutoCalibrater(0.001d);
+        Debug.Log("地磁计校准结果主动重置！");
     }
 
     public override bool IsGyrCompleted() {
@@ -122,17 +126,15 @@ public class Axis9Handler : AxisBaseHandler
         try
         {
             _9Axis.Attitude = attitudeJson.Parse<o0.IMU._9AxisPreProcessor>(record);
-            Type t = _9Axis.Attitude.GetType();
-            object o = _9Axis.Attitude;
-            bool gyrEqual = GyrByteIndex.Equals(t.GetField("GyrByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o));
-            bool accEqual = AccByteIndex.Equals(t.GetField("AccByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o));
-            bool magEqual = MagByteIndex.Equals(t.GetField("MagByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o));
-            if (!(gyrEqual && accEqual && magEqual))
+            Debug.Log("9轴反序列化完成");
+            PrintAxisInfo();
+            if (!IsAxisRight())
             {
-                Debug.Log("跟保存的轴向不相同，重置校准记录！");
                 _9Axis.Attitude = new o0.IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
+                Debug.Log("跟保存的轴向不相同，重置校准记录！");
+                PrintAxisInfo();
             }
-            Debug.Log("9轴数据恢复成功" + ", MagComplete" + _9Axis.Attitude.MagCalibrater.Complete);
+            Debug.Log("9轴数据恢复结果: " + _9Axis.Attitude.MagCalibrater.Complete);
         }
         catch (Exception e)
         {
@@ -141,4 +143,30 @@ public class Axis9Handler : AxisBaseHandler
         }
         m_aimHandler.CorrectMagCompleted(_9Axis.Attitude.MagCalibrater.Complete);
     }
+
+    //判断轴向是否正确
+    private bool IsAxisRight() 
+    { 
+        object o = _9Axis.Attitude;
+        Type t = o.GetType();
+        var _gyr = t.GetField("GyrByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
+        var _acc = t.GetField("AccByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
+        var _mag = t.GetField("MagByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
+        bool gyrEqual = GyrByteIndex.Equals(_gyr);
+        bool accEqual = AccByteIndex.Equals(_acc);
+        bool magEqual = MagByteIndex.Equals(_mag);
+        return gyrEqual && accEqual && magEqual;
+    }
+
+    //打印当前轴向
+    public void PrintAxisInfo()
+    {
+        object o = _9Axis.Attitude;
+        Type t = o.GetType();
+        var _gyr = t.GetField("GyrByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
+        var _acc = t.GetField("AccByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
+        var _mag = t.GetField("MagByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
+        Debug.Log($"当前轴向：gyr:{_gyr.ToString()} acc:{_acc.ToString()} mag:{_mag.ToString()}");
+        Debug.Log("当前轴向是否正确：" + IsAxisRight());
+    }
 }