nan

Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

@@ -50,7 +50,14 @@ public class AimHandler : MonoBehaviour
     //     UserComp.Instance.saveMac();
     // }
 
-    [NonSerialized] public Quaternion newRotation = Quaternion.identity;
+    [NonSerialized] private Quaternion newRotation = Quaternion.identity;
+    public void SetNewRotation(Quaternion quat) {
+        if (float.IsNaN(quat.x) || float.IsNaN(quat.y) || float.IsNaN(quat.z) || float.IsNaN(quat.w)) {
+            Debug.LogWarning("九轴算法返回的Rotation存在Nan值：" + quat.ToString());
+            return;
+        }
+        this.newRotation = quat;
+    }
     [NonSerialized] public bool lerpForRotation = true;
     [NonSerialized] public float lerpTimeRate = 7;
     public void Update()

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

@@ -48,13 +48,13 @@ public class Axis663Handler : AxisBaseHandler
             Debug.LogError(e.Message);
             Debug.LogError(e.StackTrace);
         }
-        m_aimHandler.newRotation = o0.Bow.Extension.ToUnityQuaternion(Qua);
+        m_aimHandler.SetNewRotation(o0.Bow.Extension.ToUnityQuaternion(Qua));
     }
 
     public override void DoIdentity()
     {
         _663Axis.SetIdentityAndSave();
-        m_aimHandler.newRotation = o0.Bow.Extension.ToUnityQuaternion(_663Axis.getLastState().Qua);
+        m_aimHandler.SetNewRotation(o0.Bow.Extension.ToUnityQuaternion(_663Axis.getLastState().Qua));
     }
 
     public override void NotifyAxisOnShot()

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

@@ -52,13 +52,13 @@ public class Axis9Handler : AxisBaseHandler
             Debug.LogError(e.Message);
             Debug.LogError(e.StackTrace);
         }
-        m_aimHandler.newRotation = o0.Bow.Extension.ToUnityQuaternion(Qua);
+        m_aimHandler.SetNewRotation(o0.Bow.Extension.ToUnityQuaternion(Qua));
     }
 
     public override void DoIdentity()
     {
         _9Axis.SetIdentityAndSave();
-        m_aimHandler.newRotation = o0.Bow.Extension.ToUnityQuaternion(_9Axis.getLastState().Qua);
+        m_aimHandler.SetNewRotation(o0.Bow.Extension.ToUnityQuaternion(_9Axis.getLastState().Qua));
     }
 
     public override void NotifyAxisOnShot()

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

@@ -104,13 +104,13 @@ public class Axis9NopackHandler : AxisBaseHandler
         acc0o = distanceToAxis.AccCorrection(gyr0o, acc0o, GapMs);/**///轴心偏离矫正
         Acc = o0VectorToUnity(acc0o);
 
-        m_aimHandler.newRotation = _9Axis.update(Acc, Gyr, Mag, TimeGap);
+        m_aimHandler.SetNewRotation(_9Axis.update(Acc, Gyr, Mag, TimeGap));
     }
 
     public override void DoIdentity()
     {
         _9Axis.SetIdentityAndSave();
-        m_aimHandler.newRotation =  _9Axis.getLastState().Qua;;
+        m_aimHandler.SetNewRotation(_9Axis.getLastState().Qua);
     }
 
     public override void NotifyAxisOnShot()

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

@@ -15,7 +15,7 @@ namespace o0.Bow
         public IMU.HardwareVariance AccHardwareVariance;
         public IMU.HardwareVariance MagHardwareVariance;
 
-        public static List<UnityEngine.Quaternion> QuaTest = new List<UnityEngine.Quaternion>();
+        // public static List<UnityEngine.Quaternion> QuaTest = new List<UnityEngine.Quaternion>();
         public o09AxisAfterXiaMenFromDll(Vector<int> GyrByteIndex = default, Vector<int> AccByteIndex = default, Vector<int> MagByteIndex = default)
         {
             Attitude = new IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
@@ -23,8 +23,8 @@ namespace o0.Bow
             AccHardwareVariance = new IMU.HardwareVariance();
             MagHardwareVariance = new IMU.HardwareVariance();
 
-            QuaTest.Add(UnityEngine.Quaternion.identity);
-            QuaTest.Add(UnityEngine.Quaternion.identity);
+            // QuaTest.Add(UnityEngine.Quaternion.identity);
+            // QuaTest.Add(UnityEngine.Quaternion.identity);
         }
 
         static public Vector<double> AccIdentity = new Vector<double>(0, -1, 0);
@@ -64,6 +64,8 @@ namespace o0.Bow
             var (Gyr, Acc, Mag, TimeGap) = Attitude.Update(gyrByte, accByte, magByte, min, sec, ms1, ms2);
             if((Gyr, Acc, Mag, TimeGap) == default)
                 return Geometry.Quaternion.Identity;
+
+
             Vector<double> Buffer = default;
             Buffer = GyrHardwareVariance.Update(Attitude.LastGyr);
             if (Buffer != default)
@@ -76,6 +78,9 @@ namespace o0.Bow
                 Debug.Log("MagHardwareVariance: " + Buffer.Length);
 
             var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
+            // TestVector.Update9AxisRotation(GyrOperator.ToUnityQuaternion(), 0);
+            // TestVector.SetAcc(Acc.ToUnityVector(), 0);
+            // TestVector.SetMag(Mag.ToUnityVector(), 0);
 
 
             var Last = States.LastOrDefault() ?? new State();
@@ -139,7 +144,9 @@ namespace o0.Bow
             //TestVector.SetAcc(Acc / 10, 1);
             //TestVector.SetMag(Mag, 1);
             var accTest = Geometry.Quaternion.FromToRotation(Last.Acc, Acc).Inversed;
+            // TestVector.Update9AxisRotation(accTest.ToUnityQuaternion(), 2);
             var magTest = Geometry.Quaternion.FromToRotation(Last.Mag, Mag).Inversed;
+            // TestVector.Update9AxisRotation(magTest.ToUnityQuaternion(), 3);
             //TestVector.Set9AxisRotation(Last.Qua, 3);
 
             double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
@@ -189,7 +196,7 @@ namespace o0.Bow
             var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
             var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
             Geometry.Quaternion quaFirst = Geometry.Quaternion.SLerp(quaGyr, quaAccMag, (float)quaMinRate);
-            if (double.IsNaN(quaFirst.w))
+            if (double.IsNaN(quaFirst.x) || double.IsNaN(quaFirst.y) || double.IsNaN(quaFirst.z) || double.IsNaN(quaFirst.w))
                 quaFirst = Last.Qua;
 
             var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
@@ -201,7 +208,7 @@ namespace o0.Bow
 
             ///////////////////////////////////////////////////////////////
 
-            if (double.IsNaN(state.Qua.w))
+            if (double.IsNaN(state.Qua.x) || double.IsNaN(state.Qua.y) || double.IsNaN(state.Qua.z) || double.IsNaN(state.Qua.w))
                 state.Qua = Last.Qua;/**/
             state.QuaSmooth = Geometry.Quaternion.SLerp(Last.QuaSmooth, state.Qua, 0.3f);//Last.QuaSmooth - state.Qua    0 - 1