update

Assets/BowArrow/Scripts/Bluetooth/BluetoothAim.cs

@@ -101,6 +101,7 @@ public class BluetoothAim : MonoBehaviour
             bluetoothHelper.OnCharacteristicChanged += (helper, value, characteristic) =>
             {
                 byte[] bytes = value;
+                Log(String.Join(",", bytes));
                 BluetoothClient.UploadData(0, bytes);
                 aimHandler.OnDataReceived(bytes);
             };
@@ -157,10 +158,80 @@ class AimHandler
     Button CalibrationButton = default;
     Text MagScaleText = default;
 
+    // Vector3 Acc = default;
+    // Vector3 Gyr = default;
+    // Vector3 Mag = default;
+    // long TimeGap = default;
+
+    // static Vector3 AccIdentity = new Vector3(0, -1, 0);
+    // static Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
+    // public class State
+    // {
+    //     public long TimeGap;
+    //     public Vector3 Acc = AccIdentity;
+    //     public Vector3 Gyr;
+    //     public Vector3 Mag = MagIdentity;
+    //     public Quaternion Qua;
+    //     public float Variance = 1;
+    // }
+    // List<State> States = new List<State>();
+
+    // Vector3 AccOld;
+    // Vector3 GyrOld;
+    // Vector3 MagOld;
+    // long TimeGapOld;
+
+    // Quaternion o06DOFUpdate(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
+    // {
+    //     var Acc = this.AccOld;
+    //     var Gyr = this.GyrOld;
+    //     var Mag = this.MagOld;
+    //     float TimeGap = (TimeGapOld + this.TimeGapOld) / 2;
+    //     this.AccOld = AccOld;
+    //     this.GyrOld = GyrOld;
+    //     this.MagOld = MagOld;
+    //     this.TimeGapOld = TimeGapOld;
+        
+    //     var Last = States.LastOrDefault() ?? new State();
+    //     States.Add(new State());
+    //     if (States.Count > 10)
+    //         States.RemoveAt(0);
+    //     var state = States.Last();
+    //     state.Acc = Acc;
+    //     state.Gyr = Gyr;
+    //     state.Mag = Mag;
+
+    //     var LastQuaternion = Last.Qua;
+
+    //     var newQua = new Quaternion();
+    //     newQua.eulerAngles = Gyr * TimeGap;
+    //     var quaGyr = LastQuaternion * newQua;
+
+    //     float AccLengthToAngle = 90;//1倍引力差相当于多少度方差
+    //     float MagLengthToAngle = 90;//1倍磁力差相当于多少度方差
+
+    //     float GyrVariance = state.Variance + TimeGap/100 + (Gyr * TimeGap).magnitude * 0.02f;
+    //     float AccVariance = TimeGap / 5 + Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2)+ Mathf.Pow(Vector3.Angle(Acc,Last.Acc) * 0.5f, 2));
+    //     float MagVariance = TimeGap / 5 + Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.1f, 2));
+    //     state.Variance = state.Variance * AccVariance / (state.Variance + AccVariance);
+    //     state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);
+
+    //     var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, AccVariance / (AccVariance + MagVariance));
+    //     var quaMinRate = GyrVariance / (GyrVariance + Mathf.Max(AccVariance, MagVariance));
+    //     var quaMaxRate = GyrVariance / (GyrVariance + Mathf.Min(AccVariance, MagVariance));
+
+    //     Quaternion quaFirst = Quaternion.Slerp(quaGyr, quaAccMag, quaMinRate).normalized;
+
+    //     float quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
+    //     state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, quaSecondRate): o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, quaSecondRate);
+
+    //     return state.Qua;
+    // }
+
+    long TimeGap = default;
     Vector3 Acc = default;
     Vector3 Gyr = default;
     Vector3 Mag = default;
-    long TimeGap = default;
 
     static Vector3 AccIdentity = new Vector3(0, -1, 0);
     static Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
@@ -175,10 +246,10 @@ class AimHandler
     }
     List<State> States = new List<State>();
 
+    long TimeGapOld;
     Vector3 AccOld;
     Vector3 GyrOld;
     Vector3 MagOld;
-    long TimeGapOld;
 
     Quaternion o06DOFUpdate(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
     {
@@ -190,7 +261,7 @@ class AimHandler
         this.GyrOld = GyrOld;
         this.MagOld = MagOld;
         this.TimeGapOld = TimeGapOld;
-        
+
         var Last = States.LastOrDefault() ?? new State();
         States.Add(new State());
         if (States.Count > 10)
@@ -206,19 +277,20 @@ class AimHandler
         newQua.eulerAngles = Gyr * TimeGap;
         var quaGyr = LastQuaternion * newQua;
 
-        float AccLengthToAngle = 90;//1倍引力差相当于多少度方差
-        float MagLengthToAngle = 90;//1倍磁力差相当于多少度方差
+        float AccLengthToAngle = 180;//1倍引力差相当于多少度方差
+        float MagLengthToAngle = 180;//1倍磁力差相当于多少度方差
+
+        float GyrVariance = state.Variance + TimeGap/1000 + (Gyr * TimeGap).magnitude * 0.05f;
+        float AccVariance = Mathf.Max(TimeGap / 10, Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2)+ Mathf.Pow(Vector3.Angle(Acc,Last.Acc) * 0.8f, 2)));
 
-        float GyrVariance = state.Variance + TimeGap/100 + (Gyr * TimeGap).magnitude * 0.02f;
-        float AccVariance = TimeGap / 5 + Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2)+ Mathf.Pow(Vector3.Angle(Acc,Last.Acc) * 0.5f, 2));
-        float MagVariance = TimeGap / 5 + Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.1f, 2));
+        float MagVariance = Mathf.Max(TimeGap / 1,Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.05f, 2)));
         state.Variance = state.Variance * AccVariance / (state.Variance + AccVariance);
         state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);
 
         var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, AccVariance / (AccVariance + MagVariance));
+
         var quaMinRate = GyrVariance / (GyrVariance + Mathf.Max(AccVariance, MagVariance));
         var quaMaxRate = GyrVariance / (GyrVariance + Mathf.Min(AccVariance, MagVariance));
-
         Quaternion quaFirst = Quaternion.Slerp(quaGyr, quaAccMag, quaMinRate).normalized;
 
         float quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
@@ -227,6 +299,7 @@ class AimHandler
         return state.Qua;
     }
 
+
     //转换读取的数据，无符号->有符号
     float TwoByteToFloat(byte b1, byte b2) 
     {
@@ -235,7 +308,7 @@ class AimHandler
         return (float) shortNum; 
     }
 
-    o0MagneticCalibrater MagCalibrater;
+    o0MagneticCalibraterSimple MagCalibrater;
     o0Vector3Filter MagFilter = new o0Vector3Filter();
     long msOld = 0;
     public AimHandler(Transform controlObj, Button SetIdentity, Button CalibrationButton, Text MagScaleText)
@@ -247,7 +320,7 @@ class AimHandler
         
         SetIdentity.onClick.AddListener(DoIdentity);
         
-        MagCalibrater = new o0MagneticCalibrater();
+        MagCalibrater = new o0MagneticCalibraterSimple();
         string caliraterDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
         if (caliraterDataStr.Length > 0)
         {
@@ -283,6 +356,7 @@ class AimHandler
 
     public void OnDataReceived(byte[] bytes)
     {
+        Debug.Log("瞄准模块数据长度" + bytes.Length);
         if (bytes.Length != 26)
         {
             if (bytes[3] == 125)
@@ -295,7 +369,7 @@ class AimHandler
             return;
         if (bytes[16] == 0 && bytes[17] == 0 && bytes[18] == 0 && bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0)
             return;
-
+        
         float ax = -TwoByteToFloat(bytes[4], bytes[5]);
         float ay = TwoByteToFloat(bytes[6], bytes[7]);
         float az = -TwoByteToFloat(bytes[8], bytes[9]);
@@ -358,18 +432,11 @@ class AimHandler
         }
         if (doIdentity)
         {
-            if (Quaternion.Angle(newRotation, baseRotation) < 2) 
-            {
-                if (!hasAutoIdentity)
-                {
-                    controlObj.transform.localRotation = newRotation;
-                }
-                doIdentity = false;
-                hasAutoIdentity = true;
-            } else {
-                AccIdentity = AccOld;
-                MagIdentity = MagOld;
-            }
+            AccIdentity = AccOld;
+            MagIdentity = MagOld;
+            States.Last().Qua = controlObj.transform.localRotation = Quaternion.identity;
+            doIdentity = false;
+            hasAutoIdentity = true;
         }
     }
 

Assets/BowArrow/Scripts/Bluetooth/BluetoothShoot.cs

@@ -83,6 +83,7 @@ public class BluetoothShoot : MonoBehaviour
             bluetoothHelper.OnCharacteristicChanged += (helper, value, characteristic) =>
             {
                 byte[] bytes = value;
+                Log(String.Join(",", bytes));
                 BluetoothClient.UploadData(1, bytes);
                 if (ShootCheck.ins != null)
                 {

Assets/BowArrow/Scripts/Bluetooth/ShootCheck.cs

@@ -11,6 +11,7 @@ public class ShootCheck : MonoBehaviour
     bool hasReachShootThreshold = false;
     bool locked = false;
     int hitCount = 0;
+    float rangeAcc; //最新几帧的平均值
     [SerializeField] Text text;
     public float shootSpeed;
     public static ShootCheck ins; 
@@ -28,6 +29,7 @@ public class ShootCheck : MonoBehaviour
 
     float[] ays = new float[4];
     public void OnDataReceived(byte[] bytes) {
+        Debug.Log("射击模块数据长度" + bytes.Length);
         ays[0] = ToAcceleratedSpeed(bytes[7], bytes[8]);
         ays[1] = ToAcceleratedSpeed(bytes[17], bytes[18]);
         ays[2] = ToAcceleratedSpeed(bytes[27], bytes[28]);
@@ -64,8 +66,7 @@ public class ShootCheck : MonoBehaviour
         return (int)shortNum;
     }
 
-    int afterReachShootFrameCount = 0;
-    int steadyFrameCount = 6;
+    int validFrameCount = 0;
     bool check(float acc)
     {
         DebugLine.show(acc);
@@ -81,12 +82,13 @@ public class ShootCheck : MonoBehaviour
             return false;
         }
         if (hasReachShootThreshold) {
+            validFrameCount++;
             if (acc <= gravity) {
                 hitCount++;
                 shootSpeed = maxAcc - gravity;
-                Log("第" + hitCount + "次识别射箭\n过滤正轴重力" + gravity.ToString("#0.000") + "后\n所得最大加速度峰值" + (maxAcc - gravity).ToString("#0.000") + "\n有效帧数" + afterReachShootFrameCount);
+                Log("第" + hitCount + "次识别射箭\n过滤正轴重力" + gravity.ToString("#0.000") + "后\n所得最大加速度峰值" + (maxAcc - gravity).ToString("#0.000") + "\n有效帧数" + validFrameCount);
                 maxAcc = 0;
-                afterReachShootFrameCount = 0;
+                validFrameCount = 0;
                 hasReachShootThreshold = false;
                 Dolock();
                 Invoke("Unlock", 0.8f);
@@ -94,11 +96,10 @@ public class ShootCheck : MonoBehaviour
             } else if (acc > maxAcc) {
                 maxAcc = acc;
             }
-            afterReachShootFrameCount++;
-            if (afterReachShootFrameCount >= 20)
+            if (validFrameCount > 20)
             {
                 maxAcc = 0;
-                afterReachShootFrameCount = 0;
+                validFrameCount = 0;
                 hasReachShootThreshold = false;
                 Log("不符合短时间爆发加速");
             }
@@ -106,33 +107,12 @@ public class ShootCheck : MonoBehaviour
         }
         if (dataCount > steadyFrameCount)
         {
-            float totalAcc = 0;
-            for (int i = 0; i < steadyFrameCount; i++)
-            {
-                totalAcc += accList[i];
-            }
-            float rangeAcc = totalAcc / steadyFrameCount;
-            float squareAcc = 0;
-            for (int i = 0; i < steadyFrameCount; i++)
-            {
-                squareAcc += (float) Mathf.Pow(accList[i] - rangeAcc, 2);
-            }
-            squareAcc /= steadyFrameCount;
-            if (squareAcc < 0.00012)
+            if (isSteady())
             {
                 gravity = Mathf.Clamp(rangeAcc, -0.981f, 0.981f);
             } 
-            if (acc - gravity > 3)
+            if (acc - gravity > 2)
             {
-                // for (int i = 1; i < dataCount && i < 15; i++)
-                // {
-                //     if (accList[i] - gravity < - 1)
-                //     {
-                //         Dolock();
-                //         Invoke("Unlock", 0.2f);
-                //         return false;
-                //     }
-                // }
                 hasReachShootThreshold = true;
                 maxAcc = acc;
             }
@@ -140,6 +120,23 @@ public class ShootCheck : MonoBehaviour
         return false;
     }
 
+    int steadyFrameCount = 6;
+    bool isSteady() {
+        float totalAcc = 0;
+        for (int i = 0; i < steadyFrameCount; i++)
+        {
+            totalAcc += accList[i];
+        }
+        rangeAcc = totalAcc / steadyFrameCount;
+        float squareAcc = 0;
+        for (int i = 0; i < steadyFrameCount; i++)
+        {
+            squareAcc += (float) Mathf.Pow(accList[i] - rangeAcc, 2);
+        }
+        squareAcc /= steadyFrameCount;
+        return squareAcc < 0.00012;
+    }
+
     void Dolock() 
     {
         this.locked = true;

Assets/BowArrow/Scripts/Bluetooth/o09DOF.cs

@@ -22,6 +22,97 @@ public class o0Vector3Filter
         return state;
     }
 }
+public class o0MagneticCalibraterSimple//默认在无磁干扰环境下，有磁干扰则无法保证效果
+{
+    public Vector3 _Center = Vector3.zero;
+    public Vector3 _Radius = new Vector3(2, 2, 2);
+
+    public o0Project.Vector3f Center
+    {
+        get
+        {
+            return new o0Project.Vector3f(_Center.x, _Center.y, _Center.z);
+        }
+        set
+        {
+            _Center = new Vector3(value.x, value.y, value.z);
+        }
+    }
+    public o0Project.Vector3f Radius
+    {
+        get
+        {
+            return new o0Project.Vector3f(_Radius.x, _Radius.y, _Radius.z);
+        }
+        set
+        {
+            _Radius = new Vector3(value.x, value.y, value.z);
+        }
+    }
+
+    public o0MagneticCalibraterSimple()
+    {
+        //Calibration = true;
+    }
+    public o0MagneticCalibraterSimple(o0Project.Vector3f Center, o0Project.Vector3f Radius)
+    {
+        this.Center = Center;
+        this.Radius = Radius;
+    }
+
+    Vector3 Min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
+    Vector3 Max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
+    public bool Calibration
+    {
+        get
+        {
+            return Min != default && Max != default;
+        }
+        set
+        {
+            if (value == true)
+            {
+                Min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
+                Max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
+            }
+            else
+            {
+                Min = default;
+                Max = default;
+            }
+        }
+    }
+    public System.Random r = new System.Random();
+    public Vector3 Update(Vector3 v)
+    {
+        if (v.magnitude > 30)
+            Debug.Log(v);
+        if (Calibration)
+        {
+            if (Min.x > v.x)
+                Min.x = v.x;
+            if (Min.y > v.y)
+                Min.y = v.y;
+            if (Min.z > v.z)
+                Min.z = v.z;
+            if (Max.x < v.x)
+                Max.x = v.x;
+            if (Max.y < v.y)
+                Max.y = v.y;
+            if (Max.z < v.z)
+                Max.z = v.z;
+            _Center = (Max + Min) / 2;
+            _Radius = (Max - Min) / 2;
+        }
+        v -= _Center;
+        v = new Vector3(v.x / _Radius.x, v.y / _Radius.y, v.z / _Radius.z);
+        return v;
+    }
+    public float CalibratCompletionPercentage()
+    {
+        return 0;
+    }
+}
 public class o0MagneticCalibrater//默认在无磁干扰环境下，有磁干扰则无法保证效果
 {
     public Vector3 _Center = Vector3.zero;

Assets/BowArrow/Scripts/Game/ArmBow.cs

@@ -133,7 +133,7 @@ public class ArmBow : MonoBehaviour
         // arrowComp.calculateSpeed(rayHitPoint);
         // Arrow.speed = BaseSpeedSlider.ins.getValue() * DebugBowPower.ins.getPowerPercent();
         // Arrow.speed = BaseSpeedSlider.ins.getValue();
-        Arrow.speed = ShootCheck.ins.shootSpeed * BaseSpeedSlider.ins.getValue();
+        Arrow.speed = Mathf.Pow(ShootCheck.ins.shootSpeed, 2.5f);
 
         arrowCopy.SetActive(true);  
         arrow.SetActive(false);

Assets/DebugShoot/BluetoothDispatcher.cs

@@ -1,4 +1,5 @@
-using System.Collections;
+using System;
+using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
 
@@ -14,7 +15,7 @@ public class BluetoothDispatcher : MonoBehaviour
 
     void Dispatch(byte sign, byte[] data)
     {
-        Debug.Log(sign + "---" + data);
+        Debug.Log(sign + ", LEN " + data.Length);
         if (sign == 0 && aim != null)
         {
             aim(data);

Assets/DebugShoot/DebugBluetooth.unity

@@ -290,7 +290,7 @@ GameObject:
   - component: {fileID: 417067451}
   - component: {fileID: 417067450}
   m_Layer: 0
-  m_Name: GameObject
+  m_Name: DebugScripts
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
@@ -348,7 +348,7 @@ Transform:
   m_LocalScale: {x: 1, y: 1, z: 1}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 2
+  m_RootOrder: 5
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!1 &557747841
 GameObject:
@@ -591,7 +591,7 @@ RectTransform:
   - {fileID: 407940832}
   - {fileID: 656646620}
   m_Father: {fileID: 0}
-  m_RootOrder: 3
+  m_RootOrder: 4
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
   m_AnchorMin: {x: 0.5, y: 0.5}
   m_AnchorMax: {x: 0.5, y: 0.5}
@@ -1325,7 +1325,7 @@ Transform:
   m_LocalScale: {x: 1, y: 1, z: 1}
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 5
+  m_RootOrder: 3
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!1 &1632686057
 GameObject:
@@ -1503,7 +1503,7 @@ RectTransform:
   - {fileID: 2081173760}
   - {fileID: 948485337}
   m_Father: {fileID: 0}
-  m_RootOrder: 4
+  m_RootOrder: 2
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
   m_AnchorMin: {x: 0, y: 0}
   m_AnchorMax: {x: 0, y: 0}