算法update

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

@@ -20,7 +20,7 @@ public class Axis663Handler : AxisBaseHandler
         CommonConfig.devicePlan = 4;
         _663Axis = new o0.Bow.o0663Axis(GyrByteIndex, GyrByteIndex, AccByteIndex, AccByteIndex, MagByteIndex);
         _663Axis.Attitude = new o0.IMU._663AxisPreProcessor(GyrByteIndex, GyrByteIndex, AccByteIndex, AccByteIndex, MagByteIndex);
-        _663Axis.LoadIdentity();
+        LoadIdentity();
     }
 
     public override void Update(byte[] bytes)
@@ -53,7 +53,7 @@ public class Axis663Handler : AxisBaseHandler
 
     public override void DoIdentity()
     {
-        _663Axis.SetIdentityAndSave();
+        SetIdentityAndSave();
         m_aimHandler.SetNewRotation(_663Axis.getLastState().Qua);
     }
 
@@ -129,4 +129,39 @@ public class Axis663Handler : AxisBaseHandler
         }
         m_aimHandler.CorrectMagCompleted(_663Axis.Attitude.MagCalibrater.Complete);
     }
+
+    #region 视角归位-恢复和保存
+    private void LoadIdentity() 
+    {
+        try {
+            string accStr = PlayerPrefs.GetString("AccIdentity0", "");
+            if (accStr.Length > 0) {
+                double[] arr = JsonConvert.DeserializeObject<double[]>(accStr);
+                o0.Bow.o0663Axis.AccIdentity = new o0.Geometry.Vector<double>(arr);
+            }
+            string magStr = PlayerPrefs.GetString("MagIdentity0", "");
+            if (magStr.Length > 0) {
+                double[] arr = JsonConvert.DeserializeObject<double[]>(magStr);
+                o0.Bow.o0663Axis.MagIdentity = new o0.Geometry.Vector<double>(arr);
+            }
+        } catch (System.Exception e) { 
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace); 
+        }
+    }
+
+    private void SetIdentityAndSave()
+    {
+        _663Axis.SetIdentity();
+        //save
+        var a = o0.Bow.o0663Axis.AccIdentity;
+        var m = o0.Bow.o0663Axis.MagIdentity;
+        PlayerPrefs.SetString("AccIdentity0", JsonConvert.SerializeObject(new double[]{
+            a.x, a.y, a.z
+        }));
+        PlayerPrefs.SetString("MagIdentity0",JsonConvert.SerializeObject(new double[]{
+            m.x, m.y, m.z
+        }));
+    }
+    #endregion
 }

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

@@ -24,7 +24,7 @@ public class Axis9Handler : AxisBaseHandler
         CommonConfig.devicePlan = 3;
         _9Axis = new o0.Bow.o09AxisAfterXiaMenFromDll(GyrByteIndex, AccByteIndex, MagByteIndex);
         _9Axis.Attitude = new o0.IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
-        _9Axis.LoadIdentity();
+        LoadIdentity();
         Debug.Log("9轴启动成功（首次初始化）");
         PrintAxisInfo();
     }
@@ -57,7 +57,7 @@ public class Axis9Handler : AxisBaseHandler
 
     public override void DoIdentity()
     {
-        _9Axis.SetIdentityAndSave();
+        SetIdentityAndSave();
         m_aimHandler.SetNewRotation(_9Axis.getLastState().Qua);
     }
 
@@ -169,4 +169,39 @@ public class Axis9Handler : AxisBaseHandler
         Debug.Log($"当前轴向：gyr:{_gyr.ToString()} acc:{_acc.ToString()} mag:{_mag.ToString()}");
         Debug.Log("当前轴向是否正确：" + IsAxisRight());
     }
+
+    #region 视角归位-恢复和保存
+    private void LoadIdentity() 
+    {
+        try {
+            string accStr = PlayerPrefs.GetString("AccIdentity0", "");
+            if (accStr.Length > 0) {
+                double[] arr = JsonConvert.DeserializeObject<double[]>(accStr);
+                o0.Bow.o09AxisAfterXiaMenFromDll.AccIdentity = new o0.Geometry.Vector<double>(arr);
+            }
+            string magStr = PlayerPrefs.GetString("MagIdentity0", "");
+            if (magStr.Length > 0) {
+                double[] arr = JsonConvert.DeserializeObject<double[]>(magStr);
+                o0.Bow.o09AxisAfterXiaMenFromDll.MagIdentity = new o0.Geometry.Vector<double>(arr);
+            }
+        } catch (System.Exception e) { 
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace); 
+        }
+    }
+
+    private void SetIdentityAndSave()
+    {
+        _9Axis.SetIdentity();
+        //save
+        var a = o0.Bow.o09AxisAfterXiaMenFromDll.AccIdentity;
+        var m = o0.Bow.o09AxisAfterXiaMenFromDll.MagIdentity;
+        PlayerPrefs.SetString("AccIdentity0", JsonConvert.SerializeObject(new double[]{
+            a.x, a.y, a.z
+        }));
+        PlayerPrefs.SetString("MagIdentity0",JsonConvert.SerializeObject(new double[]{
+            m.x, m.y, m.z
+        }));
+    }
+    #endregion
 }

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

@@ -249,43 +249,6 @@ namespace o0.Bow
         {
             return this.States.Last();
         }
-
-        public void LoadIdentity() 
-        {
-            try {
-                string magIdentityStr = PlayerPrefs.GetString("MagIdentity", "");
-                if (magIdentityStr.Length > 0) {
-                    double[] arr = JsonConvert.DeserializeObject<double[]>(magIdentityStr);
-                    AccIdentity = new Vector<double>(arr[0], arr[1], arr[2]);
-                }
-                string accIdentityStr = PlayerPrefs.GetString("AccIdentity", "");
-                if (accIdentityStr.Length > 0) {
-                    double[] arr = JsonConvert.DeserializeObject<double[]>(accIdentityStr);
-                    MagIdentity = new Vector<double>(arr[0], arr[1], arr[2]);
-                }
-            } catch (System.Exception e) { 
-                Debug.LogError(e.Message);
-                Debug.LogError(e.StackTrace); 
-            }
-        }
-
-        public void SetIdentityAndSave()
-        {
-            SetIdentity();
-            SaveIdentity();
-        }
-        
-        private void SaveIdentity() {
-            Vector<double> m = MagIdentity;
-            Vector<double> a = AccIdentity;
-            PlayerPrefs.SetString("MagIdentity",JsonConvert.SerializeObject(new double[]{
-                m.x, m.y, m.z
-            }));
-            PlayerPrefs.SetString("AccIdentity", JsonConvert.SerializeObject(new double[]{
-                a.x, a.y, a.z
-            }));
-        }
-
     }
 
 }

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

@@ -3,7 +3,6 @@ using System;
 using System.Collections.Generic;
 using System.Linq;
 using UnityEngine;
-using Newtonsoft.Json;
 
 
 namespace o0.Bow
@@ -15,7 +14,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 +22,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);
@@ -77,7 +76,7 @@ namespace o0.Bow
             if (Buffer != default)
                 Debug.Log("MagHardwareVariance: " + Buffer.Length);
 
-            var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
+            // var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
             // TestVector.Update9AxisRotation(GyrOperator.ToUnityQuaternion(), 0);
             // TestVector.SetAcc(Acc.ToUnityVector(), 0);
             // TestVector.SetMag(Mag.ToUnityVector(), 0);
@@ -89,10 +88,6 @@ namespace o0.Bow
                 States.RemoveAt(0);
             var state = States.Last();
             state.Acc = Acc;
-
-
-            //Debug.Log(Gyr.magnitude);
-
             state.Gyr = Gyr;
             state.Mag = Mag;/**/
             state.TimeGap = TimeGap;
@@ -121,7 +116,7 @@ namespace o0.Bow
             this.ShakeFrame = ShakeFrame;
             Debug.Log("OnShot");/**/
         }
-        public double diff = 0.001;
+        public double diff = 0.00001;
         public Geometry.Quaternion Process9Axis(State Last, State state)
         {
             var Acc = state.Acc;
@@ -136,61 +131,58 @@ namespace o0.Bow
 
 
 
-            var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
+            Vector<double> gyrApplied;
+            if (Gyr.Length > 0.001)
+                gyrApplied = Gyr;
+            else
+                gyrApplied = Vector<double>.Zero;
+
+            var GyrOperator = Geometry.Quaternion.Euler(gyrApplied * TimeGap);
             var quaGyr = LastQuaternion * GyrOperator;
 
+            Debug.Log("Gyr.Length: " + (Gyr).Length);
 
             //TestVector.Update9AxisRotation(GyrOperator, 1);
             //TestVector.SetAcc(Acc / 10, 1);
             //TestVector.SetMag(Mag, 1);
-            var accTest = Geometry.Quaternion.FromToRotation(Last.Acc, Acc).Inversed;
+            // var accTest = Geometry.Quaternion.FromToRotation(Last.Acc, Acc).Inversed;
             // TestVector.Update9AxisRotation(accTest.ToUnityQuaternion(), 2);
-            var magTest = Geometry.Quaternion.FromToRotation(Last.Mag, Mag).Inversed;
+            // var magTest = Geometry.Quaternion.FromToRotation(Last.Mag, Mag).Inversed;
             // TestVector.Update9AxisRotation(magTest.ToUnityQuaternion(), 3);
             //TestVector.Set9AxisRotation(Last.Qua, 3);
 
             double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
             double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
 
-            double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).Length * 0.3, 3) + diff;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+            double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).Length * 0.3, 3) + diff;// + diff;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
 
+            double AccAngleDiff = state.Acc.Angle(Last.Acc);
+            if (double.IsNaN(AccAngleDiff))
+            {
+                AccAngleDiff = 0;
+            }
             //double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).magnitude * 0.3, 3) + 0.1;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
             //    Debug.Log("GyrVariance==" + GyrVariance);
-            double AccVariance = AccVarianceInput != default ? AccVarianceInput : Math.Max((Gyr * TimeGap).Length, state.Acc.Angle(Last.Acc)) * 1 + Math.Pow(Math.Abs(state.Acc.Length - 1) / 1 * AccLengthToAngle, 4);
+            double AccVariance = AccVarianceInput != default ? AccVarianceInput : Math.Max((Gyr * TimeGap).Length, AccAngleDiff) * 1 + Math.Pow(Math.Abs(state.Acc.Length - 1) / 1 * AccLengthToAngle, 4);
             double MagVariance = 10 + Math.Pow(Math.Abs(state.Mag.Length - 1) / 1 * MagLengthToAngle, 4);/**/
 
 
 
             if (double.IsInfinity(GyrVariance))
-            {
-                //Debug.Log("GyrVariance IsInfinity: " + state.GyrVariance);
                 GyrVariance = 0.0000001;
-            }
             if (double.IsNaN(GyrVariance))
-            {
-                //Debug.Log("GyrVariance IsNaN: " + state.GyrVariance);
                 GyrVariance = 0.0000001;
-            }
             if (double.IsNaN(AccVariance))
-            {
-                //Debug.Log("AccVariance IsNaN: " + state.AccVariance);
                 AccVariance = 0.0000001;
-            }
             if (double.IsNaN(MagVariance))
-            {
-                //Debug.Log("MagVariance IsNaN: " + state.MagVariance);
                 MagVariance = 0.0000001;
-            }
 
             state.Variance = GyrVariance;
             state.Variance = state.Variance * (AccVariance + MagVariance) / (state.Variance + (AccVariance + MagVariance));
 
 
             if (double.IsNaN(state.Variance))
-            {
-                //Debug.Log("Variance IsNaN: " + state.Variance);
                 state.Variance = 0.0000001;
-            }
 
             var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.Acc, state.Mag, (float)(AccVariance / (AccVariance + MagVariance)));
             var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
@@ -212,10 +204,12 @@ namespace o0.Bow
                 state.Qua = Last.Qua;/**/
             state.QuaSmooth = Geometry.Quaternion.SLerp(Last.QuaSmooth, state.Qua, 0.3f);//Last.QuaSmooth - state.Qua    0 - 1
 
+            if (double.IsNaN(state.QuaSmooth.x) || double.IsNaN(state.QuaSmooth.y) || double.IsNaN(state.QuaSmooth.z) || double.IsNaN(state.QuaSmooth.w))
+                state.QuaSmooth = Last.Qua;/**/
 
             //QuaTest[0] = o0Project.o0.FormQuaternion(QuaTest[0] * GyrOperator, AccIdentity, state.AccSmooth, 1);
-            // QuaTest[0] = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance))).ToUnityQuaternion();
-            // QuaTest[1] = state.QuaAccMag.ToUnityQuaternion();
+            QuaTest[0] = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance))).ToUnityQuaternion();
+            QuaTest[1] = state.QuaAccMag.ToUnityQuaternion();
             //QuaTest[1] = o0Project.o0.FormQuaternion(QuaTest[1] * GyrOperator, MagIdentity, state.MagSmooth, 1);
             //Debug.Log(o09AxisCS.QuaTest[0]);
 
@@ -279,40 +273,6 @@ namespace o0.Bow
             return this.States.Last();
         }
 
-        public void LoadIdentity() 
-        {
-            try {
-                string magIdentityStr = PlayerPrefs.GetString("MagIdentity", "");
-                if (magIdentityStr.Length > 0) {
-                    double[] arr = JsonConvert.DeserializeObject<double[]>(magIdentityStr);
-                    AccIdentity = new Vector<double>(arr[0], arr[1], arr[2]);
-                }
-                string accIdentityStr = PlayerPrefs.GetString("AccIdentity", "");
-                if (accIdentityStr.Length > 0) {
-                    double[] arr = JsonConvert.DeserializeObject<double[]>(accIdentityStr);
-                    MagIdentity = new Vector<double>(arr[0], arr[1], arr[2]);
-                }
-            } catch (System.Exception e) { 
-                Debug.LogError(e.Message);
-                Debug.LogError(e.StackTrace); 
-            }
-        }
-
-        public void SetIdentityAndSave()
-        {
-            SetIdentity();
-            SaveIdentity();
-        }
-        
-        private void SaveIdentity() {
-            Vector<double> m = MagIdentity;
-            Vector<double> a = AccIdentity;
-            PlayerPrefs.SetString("MagIdentity",JsonConvert.SerializeObject(new double[]{
-                m.x, m.y, m.z
-            }));
-            PlayerPrefs.SetString("AccIdentity", JsonConvert.SerializeObject(new double[]{
-                a.x, a.y, a.z
-            }));
-        }
     }
-}
+
+}