兼容多种轴算法

Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

@@ -1,17 +1,11 @@
 using System;
 using UnityEngine;
 using System.Collections;
-using System.Collections.Generic;
-using System.Linq;
-using UnityEngine.UI;
-using Newtonsoft.Json;
-using o0._9Axis;
-using o0;
 
 /* 瞄准处理器 */
 public class AimHandler : MonoBehaviour
 {
-    Transform controlObj {
+    Transform m_controlObj {
         get {
             if (CameraToLook.ins) {
                 return CameraToLook.ins.transform;
@@ -19,41 +13,23 @@ public class AimHandler : MonoBehaviour
             return null;
         }
     }
-    [SerializeField] Button SetIdentityButton;
-    [SerializeField] Button MagCalibrationButton;
-    [SerializeField] Button GyrCalibrationButton;
-    [SerializeField] Text MagScaleText;
-    [SerializeField] Text GyrScaleText;
 
-    //椭圆对象
-    [SerializeField] Ellipse ellipseScript;
-    [SerializeField] GameObject AccObj;
-    [SerializeField] GameObject MagObj;
-
-    [SerializeField] GameObject AccMesh;
-    [SerializeField] GameObject GryMesh;
-    [SerializeField] GameObject MagMesh;
-    [SerializeField] GameObject AMesh;
-    [SerializeField] Transform DebugTexts;
-    [SerializeField] Transform DrawImage;
-    
-    long TimeGap = default;
-    Vector3 Acc = default;
-    Vector3 Gyr = default;
-    Vector3 Mag = default;
-    public o09Axis _9Axis = new o09Axis();
-
-    Vector3 cMaxVector = new Vector3(0,0,0);
-    Vector3 cMinVector = new Vector3(0, 0, 0);
+    public int DeviceType 
+    {
+        get 
+        {
+            if (m_axisHandler.GetType() == typeof(Axis9Handler)) return 9;
+            if (m_axisHandler.GetType() == typeof(Axis663Handler)) return 663;
+            return 0;
+        }
+    }
 
-    [o0.Serialize]
-    MagnetometerAutoCalibrater MagCalibrater;
-    o0GyrCalibrater GyrCalibrater;
+    private AxisBaseHandler m_axisHandler;
 
     //陀螺仪校准进度记录
     [NonSerialized] public int gyrCalibrateCompleteCount = 0;
     [NonSerialized] public int gyrCalibrateTotalCount = 2000;
-
+    //时差记录（新版算法不需要）
     [NonSerialized] public long msOld = 0;
 
     public static AimHandler ins; 
@@ -63,205 +39,39 @@ public class AimHandler : MonoBehaviour
         ins = this;
         BluetoothDispatcher.aim = OnDataReceived;
 
-        //初始化
-        _9Axis.LoadIdentity();
-
-        _9Axis.AccMesh = AccMesh;
-        _9Axis.GryMesh = GryMesh;
-        _9Axis.MagMesh = MagMesh;
-
-        for (var i = 0; i < 9; ++i)
-        {
-            _9Axis.Tester.Add(DrawImage.Find(i.ToString()).gameObject.AddComponent<o0UIRawImageTester>());
-        }
+        m_axisHandler = new Axis9NopackHandler(this);
+        m_axisHandler.Init();
+    }
 
-        for (var i = 0; i < 15; ++i)
+    [NonSerialized] public Quaternion newRotation = Quaternion.identity;
+    [NonSerialized] public bool lerpForRotation = true;
+    [NonSerialized] public float lerpTimeRate = 7;
+    public void Update()
+    {
+        if (m_controlObj && !m_ban9AxisCalculate)
         {
-            _9Axis.TextTester.Add(DebugTexts.transform.Find("Text" + i.ToString()).gameObject.GetComponent<Text>());
+            if (lerpForRotation)
+                m_controlObj.localRotation = Quaternion.Lerp(m_controlObj.localRotation, newRotation, Time.deltaTime * lerpTimeRate);   
+            else 
+                m_controlObj.localRotation = newRotation;
         }
-
-        if (SetIdentityButton) 
+        if (IsMagCompleted()) 
         {
-            SetIdentityButton.onClick.AddListener(DoIdentity);
-        }
-
-        InitGyr(null);
-        InitMag(null);
-    }
-
-    public void InitGyr(string record) {
-        try {
-            if (!string.IsNullOrEmpty(record)) {
-                var res = JsonConvert.DeserializeObject<o0GyrCalibrater>(record);
-                if (res != null) GyrCalibrater = res;
-            } else {
-                GyrCalibrater = new o0GyrCalibrater();
-            }
-        } catch(Exception) {}
-    }
-
-    public void InitMag(string record) {
-        try {
-            if (!string.IsNullOrEmpty(record)) {
-                MagCalibrater = JsonConvert.DeserializeObject<MagnetometerAutoCalibrater>(record, new MagJsonConverter());
-            } else {
-                MagCalibrater = new MagnetometerAutoCalibrater();
-            }
-        } catch (System.Exception e) {
-            Debug.LogError("地磁计反序列化出错");
-            Debug.LogError(e.Message);
-            Debug.LogError(e.StackTrace);
-        }
-        magComplete = MagCalibrater.Complete;
-    }
-
-    public void ResetGyr() {
-        GyrCalibrater._Average = Vector3.zero;
-    }
-
-    public void ResetMag() {
-        MagCalibrater = new MagnetometerAutoCalibrater();
-    }
-
-    public bool IsGyrCompleted() {
-        return !GyrCalibrater._Average.Equals(Vector3.zero);
-    }
-
-    public bool IsMagCompleted() {
-        return MagCalibrater.Complete;
-    }
-
-    public IEnumerator SaveGyr() {
-        yield return null;
-        string mac = LoginMgr.myUserInfo.mac;
-        string record = JsonConvert.SerializeObject(GyrCalibrater);
-        UserPlayer.ins.call("userComp.saveMacCalibrate", 0, mac, record);
-    }
-
-    public IEnumerator SaveMag() {
-        yield return null;
-        string mac = LoginMgr.myUserInfo.mac;
-        string record = JsonConvert.SerializeObject(MagCalibrater, new JsonConverter[]{new MagJsonConverter()});
-        UserPlayer.ins.call("userComp.saveMacCalibrate", 1, mac, record);
-    }
-
-    public void CalibrateGyr(bool calibration) {
-        try {
-            GyrCalibrater.Calibration = calibration;
-            if (calibration)
-            {
-                GyrCalibrationButton.GetComponentInChildren<Text>().text = "停止陀螺仪校准";
-            }
-            else
+            if (!m_magCompleted) 
             {
-                GyrCalibrationButton.GetComponentInChildren<Text>().text = "开始陀螺仪校准";
+                StartCoroutine(SaveMag());
+                PopupMgr.ins.ShowTipTop(TextAutoLanguage2.GetTextByKey("tip_mag-calibrate_success"));
             }
-        } catch (Exception e) { Debug.LogError(e.Message); }
-    }
-
-    [NonSerialized] public bool isCalibrateMagPerfect = false;
-     public void CalibrateMag(bool calibration) {
-        // try {
-        //     if (calibration)
-        //     {
-        //         PointCorrector.ins = new PointCorrector(this.ellipseScript, this.MagCalibrater);
-        //         MagCalibrater.Calibration = calibration;
-        //         MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
-        //         this.cMaxVector = new Vector3(float.MinValue, float.MinValue, float.MinValue);
-        //         this.cMinVector = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
-        //         this.ellipseScript.ellipseTran.gameObject.SetActive(false);
-        //     }
-        //     else
-        //     {
-        //         PointCorrector.ins = null;
-        //         List<Vector3> list = MagCalibrater.getRecords();
-
-        //         //停止校准时候，看看数组值
-        //         float maxDistance = 0f,ratio = 1f;
-        //         Vector3 maxVector3 = new Vector3(0,0,0);
-        //         List<Vector3> endRecords = new List<Vector3>();
-        //         foreach (Vector3 i in list)
-        //         {
-        //             Vector3 v = i - MagCalibrater._Center;
-        //             if (Math.Abs(v.magnitude) > maxDistance)
-        //             {
-        //                 maxVector3 = v;
-        //                 maxDistance = Math.Abs(v.magnitude);
-        //                 if(Math.Abs(v.magnitude) < Math.Abs(MagCalibrater._Radius.magnitude))
-        //                     ratio = Math.Abs(v.magnitude) / Math.Abs(MagCalibrater._Radius.magnitude);
-        //                 else
-        //                     ratio = Math.Abs(MagCalibrater._Radius.magnitude) / Math.Abs(v.magnitude);
-        //             }
-        //         }
-        //         Debug.LogWarning(maxDistance + " == " + Math.Abs(MagCalibrater._Radius.magnitude) + " == " + MagCalibrater._Radius + " = " + maxVector3);
-        //         //如果比例效果不理想。可以设置为ratio=0.5f 
-        //         foreach (Vector3 i in list)
-        //         {
-        //             //- MagCalibrater._Center
-        //             Vector3 v = i ;
-        //             v *= ratio;
-        //             if(endRecords.Count>3000)
-        //             {
-        //                 endRecords.RemoveAt(0);
-        //             }
-        //             endRecords.Add(v);
-        //         }
-        //         this.ellipseScript.ClearAndUpdatePointArray();
-        //         // this.ellipseScript.DrawPointCloud(endRecords);
-
-        //         MagCalibrater.Calibration = calibration;
-
-        //         this.ellipseScript.ellipseTran.gameObject.SetActive(true);
-        //         //绘制椭圆形
-        //         if (MagCalibrater._Radius != this.ellipseScript.ellipseTran.localScale)
-        //         {
-        //             this.ellipseScript.setEllipseLocalScaleAndCenter(MagCalibrater._Radius, MagCalibrater._Center * ratio);
-        //             //设置绘制图像相机的对应位置
-        //             this.ellipseScript.setCameraPos(MagCalibrater._Center * ratio);
-        //             this.ellipseScript.setCameraSize(MagCalibrater._Radius);
-        //         }
-
-        //         MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
-        //         PlayerPrefs.SetString("o0MagneticCalibrater", JsonConvert.SerializeObject(MagCalibrater));
-
-        //         #region 看校准是否完美，求Records方差
-        //         if (list == null || list.Count == 0) isCalibrateMagPerfect = false;
-        //         else
-        //         {
-        //             double sumV = 0;
-        //             double[] listArray = new double[list.Count];
-        //             for (int i = 0; i < list.Count; i++)
-        //             {
-        //                 listArray[i] = MagCalibrater.Update(list[i]).magnitude;
-        //                 sumV += listArray[i];
-        //             }
-        //             double avarageV = sumV / listArray.Length;
-        //             double varianceV = 0;
-        //             foreach (double vvv in listArray)
-        //             {
-        //                 varianceV += Math.Pow(vvv - avarageV, 2);
-        //             }
-        //             varianceV /= listArray.Length;
-        //             isCalibrateMagPerfect = varianceV < 0.001;
-        //             PopupMgr.ins.ShowTip(TextAutoLanguage2.GetTextByKey("tip_mag-calibrate-variance-equal") + varianceV);
-        //         }
-        //         #endregion
-        //     }
-        // } catch (Exception e) { Debug.LogError(e.Message); }
-    }
-
-    //转换读取的数据，无符号->有符号
-    float TwoByteToFloat(byte b1, byte b2) 
-    {
-        ushort twoByte = (ushort) (b1 * 256 + b2);
-        short shortNum = (short) twoByte;
-        return (float) shortNum; 
+            m_magCompleted = true;
+        } else {
+            m_magCompleted = false;
+        }
     }
 
     public void OnDataReceived(byte[] bytes)
     {
         // Debug.Log("瞄准模块数据长度" + bytes.Length);
-        if (bytes.Length != 27)
+        if (bytes.Length != 27 && bytes.Length != 39)
         {
             if (bytes.Length == 2) {
                 if (bytes[0] == 0x66 && bytes[1] == 0x31) {
@@ -288,227 +98,42 @@ public class AimHandler : MonoBehaviour
             }
             return;
         }
-        // if (DeviceBatteryView.ins) {
-        //     DeviceBatteryView.ins.labelTemperature.text = (TwoByteToFloat(bytes[5], bytes[6]) / 100).ToString("#0.00") + "℃";
-        // }
-        if (bytes[7] == 0 && bytes[8] == 0 && bytes[9] == 0 && bytes[10] == 0 && bytes[11] == 0 && bytes[12] == 0)
-            return;
-        if (bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0 && bytes[22] == 0 && bytes[23] == 0 && bytes[24] == 0)
-            return;
-
-       //if (ban9AxisCalculate) //如果箭射出后禁止九轴计算，就缓存最新几帧九轴数据
-       //{
-       //    
-       //    if (cached9AxisFrames.Count < 2)
-       //    {
-       //        cached9AxisFrames.Enqueue(bytes);
-       //    }
-       //    else
-       //    {
-       //        cached9AxisFrames.Dequeue();
-       //        cached9AxisFrames.Enqueue(bytes);
-       //    }
-       //    return;
-       //}
-
-        float ax = TwoByteToFloat(bytes[7], bytes[8]);
-        float ay = TwoByteToFloat(bytes[9], bytes[10]);
-        float az = TwoByteToFloat(bytes[11], bytes[12]);
-
-        float roll = TwoByteToFloat(bytes[13], bytes[14]);
-        float pitch = TwoByteToFloat(bytes[15], bytes[16]);
-        float yaw = TwoByteToFloat(bytes[17], bytes[18]);
-
-        float x = TwoByteToFloat(bytes[19], bytes[20]);
-        float y = TwoByteToFloat(bytes[21], bytes[22]);
-        float z = TwoByteToFloat(bytes[23], bytes[24]);
-
-        float mxr = TwoByteToFloat(bytes[20], bytes[19]);
-        float myr = TwoByteToFloat(bytes[22], bytes[21]);
-        float mzr = TwoByteToFloat(bytes[24], bytes[23]);
-
-        if (CommonConfig.devicePlan == 3) {
-            Acc = new Vector3(az, ay, ax) / 32768 * 16;
-            Gyr = new Vector3(-yaw, -pitch, -roll) / 32768 * 2;
-            Mag = new Vector3(z, y, -x) / 32768 * 256; //最新版
-        } else if (CommonConfig.devicePlan == 0) {
-            Acc = new Vector3(-az, ay, -ax) / 32768 * 16;
-            Gyr = new Vector3(yaw, -pitch, roll) / 32768 * 2;
-            Mag = new Vector3(-z, y, x) / 32768 * 256; //旧版
-        } else if (CommonConfig.devicePlan == 1) {
-            Acc = new Vector3(ax, ay, az) / 32768 * 16;
-            Gyr = new Vector3(roll, pitch, yaw) / 32768 * 2;
-            Mag = new Vector3(z, x, -y) / 32768 * 256;//第一个6+3硬件
-        } else if (CommonConfig.devicePlan == 2) {
-            Acc = new Vector3(-az, ax, -ay) / 32768 * 16;
-            Gyr = new Vector3(-yaw, roll, -pitch) / 32768 * 2;
-            Mag = new Vector3(mzr, mxr, -myr) / 32768 * 256;//第二个6+3硬件
-        } 
-
-        AccObj.transform.GetChild(0).localPosition = Acc;
-
-        Gyr = GyrCalibrater.Update(Gyr);
-
-        if (GyrCalibrater.Calibration) 
-        {
-            if (gyrCalibrateCompleteCount < gyrCalibrateTotalCount) {
-                gyrCalibrateCompleteCount++;
-            }
-        }
-        // if (GyrScaleText && GyrCalibrater.Calibration) 
-        // {   
-        //     // _9Axis.getGyrOld()，states缓存列表没数据时，会报错
-        //     GyrScaleText.text = "" + (_9Axis.getGyrOld() * 1000000).ToString();
-        // }
-
-        // if(Mag.x > -128 && Mag.y > -128 && Mag.z > -128 && Mag.x < 128 && Mag.y < 128 && Mag.z < 128)
-        // {   
-        //     //绘制地磁计点
-        //     if (MagCalibrater.Calibration)
-        //     {
-        //         this.ellipseScript.AddAndUpdatePointArray(Mag);
-
-        //         if (Mag.x > cMaxVector.x) cMaxVector.x = Mag.x;
-        //         if (Mag.y > cMaxVector.y) cMaxVector.y = Mag.y;
-        //         if (Mag.z > cMaxVector.z) cMaxVector.z = Mag.z;
-        //         if (Mag.x < cMinVector.x) cMinVector.x = Mag.x;
-        //         if (Mag.y < cMinVector.y) cMinVector.y = Mag.y;
-        //         if (Mag.z < cMinVector.z) cMinVector.z = Mag.z;
-
-        //         Vector3 centerPoint = (this.cMaxVector + this.cMinVector) / 2;
-
-        //         //设置绘制图像相机的对应位置
-        //         this.ellipseScript.setCameraPos(centerPoint);
-        //         this.ellipseScript.setCameraSize(this.cMaxVector - this.cMinVector);
-
-        //         PointCorrector.ins?.Update(centerPoint);
-        //     }
-        //     Mag = MagCalibrater.Update(Mag);
-
-        //     if (MagScaleText)
-        //     {
-        //         MagScaleText.text = MagCalibrater._Radius.ToString();
-        //     }
-           
-        // }
-        mag0o = UnityVectorTo0o(Mag);
-        try {
-            if (!MagCalibrater.Update(mag0o)) return;
-        } catch(System.Exception) {
-            ResetMag();
-            PopupMgr.ins.ShowTipTop("磁场干扰请远离电子设备");
-            return;
-        }
-        mag0o = MagCalibrater.EllipsoidFitting.Map(mag0o);
-        Mag = o0VectorToUnity(mag0o);
-        MagObj.transform.GetChild(0).localPosition = Mag;
-
-        var ms = (((long)bytes[1]) *60 + bytes[2])*1000 + (long)TwoByteToFloat(bytes[3], bytes[4]);
-        if(msOld == default)
-        {
-            msOld = ms;
-            return;
-        }
-        TimeGap = ms - msOld;
-        msOld = ms;
-
-        GapMs = TimeGap;
-        gyr0o = UnityVectorTo0o(Gyr);
-        acc0o = UnityVectorTo0o(Acc);
         
-        distanceToAxis.Update(gyr0o, acc0o, mag0o, GapMs);
-        acc0o = distanceToAxis.AccCorrection(gyr0o, acc0o, GapMs);/**///轴心偏离矫正
-        Acc = o0VectorToUnity(acc0o);
-
-        var GyrOperator = new Quaternion();
-        GyrOperator.eulerAngles = Gyr * TimeGap;
-        GryMesh.transform.localRotation *= GyrOperator;
-
-        AMesh.transform.localRotation = newRotation = _9Axis.update(Acc, Gyr, Mag, TimeGap);
+        m_axisHandler.Update(bytes);
 
         if (BowQuatDebug.ins) BowQuatDebug.ins.ShowModuleQuat(newRotation.eulerAngles);
 
-        if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked) {
-            SB_EventSystem.ins.MoveSimulateMouse(newRotation);
-        }
+        if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked)  SB_EventSystem.ins.MoveSimulateMouse(newRotation);
     }
 
-    o0.Bow.DistanceToAxis distanceToAxis = new o0.Bow.DistanceToAxis();
-    double GapMs;
-    o0.Geometry.Vector<double> gyr0o;
-    o0.Geometry.Vector<double> acc0o;
-    o0.Geometry.Vector<double> mag0o;
-    o0.Geometry.Vector<double> UnityVectorTo0o(Vector3 src) {
-        return new o0.Geometry.Vector<double>(double.Parse(src.x.ToString()), double.Parse(src.y.ToString()), double.Parse(src.z.ToString()));
-    }
-    Vector3 o0VectorToUnity(o0.Geometry.Vector<double> src) {
-        return new Vector3(float.Parse(src.x.ToString()), float.Parse(src.y.ToString()), float.Parse(src.z.ToString()));
-    }
+    private bool m_magCompleted;
+    public void CorrectMagCompleted(bool value) { m_magCompleted = value; }
 
-    [NonSerialized] public bool lerpForRotation = true;
-    [NonSerialized] public float lerpTimeRate = 7;
-    public void Update()
+    private bool m_ban9AxisCalculate = false;
+    public void Ban9AxisCalculate(bool ban) 
     {
-        if (controlObj && !ban9AxisCalculate)
+        m_ban9AxisCalculate = ban;
+        if (!ban) 
         {
-            if (lerpForRotation)
-            {
-                controlObj.localRotation = Quaternion.Lerp(controlObj.localRotation, newRotation, Time.deltaTime * lerpTimeRate);   
-            } 
-            else 
-            {
-                controlObj.localRotation = newRotation;
-            }
-        }
-        _magCompleteTemp = IsMagCompleted();
-        if (!magComplete && _magCompleteTemp) {
-            StartCoroutine(SaveMag());
-            PopupMgr.ins.ShowTipTop(TextAutoLanguage2.GetTextByKey("tip_mag-calibrate_success"));
-        }
-        magComplete = _magCompleteTemp;
-    }
-
-    bool _magCompleteTemp;
-    bool magComplete;
-
-    private bool ban9AxisCalculate = false;
-    private Queue<byte[]> cached9AxisFrames = new Queue<byte[]>();
-    public void Ban9AxisCalculate(bool ban) {
-        ban9AxisCalculate = ban;
-        if (!ban) {
             msOld = default;
-            // try
-            // {
-            //     if (StatesBackDebug.ins) _9Axis.DeleteStatesFromTail(StatesBackDebug.ins.val);
-            // }
-            // catch (Exception) { }
-            //恢复九轴计算时，把缓存的最新几帧计算了
-            // bool isFirstFrame = true;
-            while (cached9AxisFrames.Count > 0)
-            {
-                try
-                {
-                    OnDataReceived(cached9AxisFrames.Dequeue());
-                    //最初的一帧是用来顶掉msOld的，不会进state数组，因此不需要设置方差
-                    // if (!isFirstFrame) {
-                    //     _9Axis.SetAccMagVariance(10000);
-                    // }
-                    // isFirstFrame = false;
-                }
-                catch (Exception) { }
-            }
-            //立马应用到控制物体中
-            if (controlObj) controlObj.localRotation = newRotation;
+            if (m_controlObj) m_controlObj.localRotation = newRotation;
         }
     }
 
-    [NonSerialized] public Quaternion newRotation = Quaternion.identity;
-
     public void DoIdentity()
     {
-        _9Axis.SetIdentityAndSave();
-        Quaternion qua = _9Axis.getLastState().Qua;
-        newRotation = qua;
-        if (controlObj) controlObj.localRotation = qua;
-    }
+        m_axisHandler.DoIdentity();
+        if (m_controlObj) m_controlObj.localRotation = newRotation;
+    }
+    public void NotifyAxisOnShot() { m_axisHandler.NotifyAxisOnShot(); }
+    public void CalibrateGyr(bool calibration) { m_axisHandler.CalibrateGyr(calibration); }
+    public void InitGyr(string record) { m_axisHandler.InitGyr(record); }
+    public void InitMag(string record) { m_axisHandler.InitMag(record); }
+    public void ResetGyr() { m_axisHandler.ResetGyr(); }
+    public void ResetMag() { m_axisHandler.ResetMag(); }
+    public bool IsGyrCompleted() { return m_axisHandler.IsGyrCompleted(); }
+    public bool IsMagCompleted() { return m_axisHandler.IsMagCompleted(); }
+    public IEnumerator SaveGyr() { return m_axisHandler.SaveGyr(); }
+    public IEnumerator SaveMag() { return m_axisHandler.SaveMag(); }
+    public void ResumeCalibrateRecord(string record) { m_axisHandler.ResumeCalibrateRecord(record); }
 }

Assets/BowArrow/Scripts/Bluetooth/BluetoothAim.cs

@@ -564,7 +564,7 @@ public class BluetoothAim : MonoBehaviour
         if (mac != null) mac = mac.Trim();
         if (CheckIsMacValid(mac)) {
             LoginMgr.myUserInfo.mac = mac;
-            SaveMac(mac);
+            UserComp.Instance.saveMac();
         }
     }
 
@@ -579,30 +579,4 @@ public class BluetoothAim : MonoBehaviour
         }
         return true;
     }
-
-    void SaveMac(string mac) {
-        Action<Newtonsoft.Json.Linq.JToken> cb = (Newtonsoft.Json.Linq.JToken o) => {
-            string gyrStr = o.Value<string>("gyr");
-            string magStr = o.Value<string>("mag");
-            AimHandler.ins.InitGyr(gyrStr);
-            AimHandler.ins.InitMag(magStr);
-        };
-        UserPlayer.ins.call("userComp.saveMac", new object[]{mac}, cb);
-    }
-
-    // #region 测试mac获取服务端校准记录
-    // void Awake()
-    // {
-    //     StartCoroutine(qqq());
-    // }
-
-    // System.Collections.IEnumerator qqq() {
-    //     while (LoginMgr.myUserInfo.id == 0) {
-    //         yield return null;
-    //     }
-    //     SaveMac("123");
-    //     StartCoroutine(AimHandler.ins.SaveGyr());
-    //     StartCoroutine(AimHandler.ins.SaveMag());
-    // }
-    // #endregion
 }

Assets/BowArrow/Scripts/Bluetooth/New.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 572ae60186cda0943b4bf01f20e53552
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

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

@@ -0,0 +1,139 @@
+using System;
+using UnityEngine;
+using System.Collections;
+using Newtonsoft.Json;
+using o0;
+using System.Reflection;
+
+public class Axis663Handler : AxisBaseHandler
+{
+    private o0.Bow.o0663Axis _663Axis;
+
+    /*
+    public o0.Geometry.Vector<int> GyrByteIndex = new o0.Geometry.Vector<int>(3, -2, 1);
+    public o0.Geometry.Vector<int> AccByteIndex = new o0.Geometry.Vector<int>(3, -2, 1);
+    public o0.Geometry.Vector<int> MagByteIndex = new o0.Geometry.Vector<int>(-3, 2, 1);/**///9轴 usb向上 pcb向左 电池向右
+    /*
+    public o0.Geometry.Vector<int> GyrByteIndex = new o0.Geometry.Vector<int>(-3, -2, -1);
+    public o0.Geometry.Vector<int> AccByteIndex = new o0.Geometry.Vector<int>(-3, -2, -1);
+    public o0.Geometry.Vector<int> MagByteIndex = new o0.Geometry.Vector<int>(3, 2, -1);/**///9轴 usb向上 pcb向右 电池向左
+    public o0.Geometry.Vector<int> GyrByteIndex = new o0.Geometry.Vector<int>(-3, -2, -1);
+    public o0.Geometry.Vector<int> AccByteIndex = new o0.Geometry.Vector<int>(-3, -2, -1);
+    public o0.Geometry.Vector<int> MagByteIndex = new o0.Geometry.Vector<int>(-3, -1, 2);/**///双陀螺仪 usb向上 pcb向右 电池向右
+
+    public Axis663Handler(AimHandler aimHandler) : base(aimHandler) {}
+
+    public override void Init()
+    {
+        _663Axis = new o0.Bow.o0663Axis(GyrByteIndex, GyrByteIndex, AccByteIndex, AccByteIndex, MagByteIndex);
+        _663Axis.Attitude = new o0.IMU._663AxisPreProcessor(GyrByteIndex, GyrByteIndex, AccByteIndex, AccByteIndex, MagByteIndex);
+        _663Axis.LoadIdentity();
+    }
+
+    public override void Update(byte[] bytes)
+    {
+        if (_663Axis.Attitude.GyrCalibrate)
+        {
+            if (m_aimHandler.gyrCalibrateCompleteCount < m_aimHandler.gyrCalibrateTotalCount)
+            {
+                m_aimHandler.gyrCalibrateCompleteCount++;
+            }
+        }
+
+        o0.Geometry.Quaternion Qua = o0.Geometry.Quaternion.Identity;
+        try
+        {
+            lock (_663Axis)
+                Qua = _663Axis.Update(new byte[] { bytes[13], bytes[14], bytes[15], bytes[16], bytes[17], bytes[18] }, 
+                    new byte[] { bytes[25], bytes[26], bytes[27], bytes[28], bytes[29], bytes[30] },
+                    new byte[] { bytes[7], bytes[8], bytes[9], bytes[10], bytes[11], bytes[12] },
+                    new byte[] { bytes[19], bytes[20], bytes[21], bytes[22], bytes[23], bytes[24] },
+                    new byte[] { bytes[31], bytes[32], bytes[33], bytes[34], bytes[35], bytes[36] },
+                    bytes[1], bytes[2], bytes[3], bytes[4]);/**///双陀螺仪
+        }
+        catch (Exception e) {
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace);
+        }
+        m_aimHandler.newRotation = o0.Bow.Extension.ToUnityQuaternion(Qua);
+    }
+
+    public override void DoIdentity()
+    {
+        _663Axis.SetIdentityAndSave();
+        m_aimHandler.newRotation = o0.Bow.Extension.ToUnityQuaternion(_663Axis.getLastState().Qua);
+    }
+
+    public override void NotifyAxisOnShot()
+    {
+        _663Axis.OnShot(100, 100, 100000);
+    }
+
+    public override void CalibrateGyr(bool calibration) {
+        try {
+            _663Axis.Attitude.GyrCalibrate = calibration;
+        } catch (Exception) {}
+    }
+
+    public override void ResetGyr() {
+        _663Axis.Attitude.GyrCalibrate = true;
+        _663Axis.Attitude.GyrCalibrate = false;
+    }
+
+    public override void ResetMag() {
+        _663Axis.Attitude.MagCalibrater = new o0.IMU.MagnetometerAutoCalibrater();
+    }
+
+    public override bool IsGyrCompleted() {
+        FieldInfo fieldInfo = _663Axis.Attitude.GetType().GetField("GyrCalibrater1", BindingFlags.Instance | BindingFlags.NonPublic);
+        var mm = fieldInfo.GetValue(_663Axis.Attitude) as MeanMaintainer<o0.Geometry.Vector<double>>;
+        return mm.Count > 0;
+    }
+
+    public override bool IsMagCompleted() {
+        return _663Axis.Attitude.MagCalibrater.Complete;
+    }
+
+    public override IEnumerator SaveGyr() {
+        yield return null;
+        SaveCalibrateRecord();
+    }
+
+    public override IEnumerator SaveMag() {
+        yield return null;
+        SaveCalibrateRecord();
+    }
+
+    private void SaveCalibrateRecord() 
+    {
+        try
+        {
+            string record = JsonConvert.SerializeObject(_663Axis.Attitude);
+            if (!string.IsNullOrEmpty(record))
+            {
+                Debug.Log("663轴数据序列化成功");
+                UserComp.Instance.saveCalibrateRecord(record);
+            }
+        }
+        catch (Exception e)
+        {
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace);
+        }
+    }
+
+    public override void ResumeCalibrateRecord(string record) 
+    {
+        try
+        {
+            Json.FromJson<o0.IMU._663AxisPreProcessor>(record, ref _663Axis.Attitude);
+            Debug.Log("663轴数据恢复成功" + ", MagComplete" + _663Axis.Attitude.MagCalibrater.Complete);
+        }
+        catch (Exception e)
+        {
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace);
+        }
+        m_aimHandler.CorrectMagCompleted(_663Axis.Attitude.MagCalibrater.Complete);
+    }
+}

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

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 08537ba6c15768e4085237e986eaf407
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

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

@@ -0,0 +1,137 @@
+using System;
+using UnityEngine;
+using System.Collections;
+using Newtonsoft.Json;
+using o0;
+using System.Reflection;
+
+public class Axis9Handler : AxisBaseHandler
+{
+    private o0.Bow.o09AxisAfterXiaMenFromDll _9Axis;
+
+    /*
+    public o0.Geometry.Vector<int> GyrByteIndex = new o0.Geometry.Vector<int>(3, -2, 1);
+    public o0.Geometry.Vector<int> AccByteIndex = new o0.Geometry.Vector<int>(3, -2, 1);
+    public o0.Geometry.Vector<int> MagByteIndex = new o0.Geometry.Vector<int>(-3, 2, 1);/**///9轴 usb向上 pcb向左 电池向右
+    public o0.Geometry.Vector<int> GyrByteIndex = new o0.Geometry.Vector<int>(-3, -2, -1);
+    public o0.Geometry.Vector<int> AccByteIndex = new o0.Geometry.Vector<int>(-3, -2, -1);
+    public o0.Geometry.Vector<int> MagByteIndex = new o0.Geometry.Vector<int>(3, 2, -1);/**///9轴 usb向上 pcb向右 电池向左
+    /*
+    public o0.Geometry.Vector<int> GyrByteIndex = new o0.Geometry.Vector<int>(-3, -2, -1);
+    public o0.Geometry.Vector<int> AccByteIndex = new o0.Geometry.Vector<int>(-3, -2, -1);
+    public o0.Geometry.Vector<int> MagByteIndex = new o0.Geometry.Vector<int>(-3, -1, 2);/**///双陀螺仪 usb向上 pcb向右 电池向右
+
+    public Axis9Handler(AimHandler aimHandler) : base(aimHandler) {}
+
+    public override void Init()
+    {
+        _9Axis = new o0.Bow.o09AxisAfterXiaMenFromDll(GyrByteIndex, AccByteIndex, MagByteIndex);
+        _9Axis.Attitude = new o0.IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
+        _9Axis.LoadIdentity();
+    }
+
+    public override void Update(byte[] bytes)
+    {
+        if (_9Axis.Attitude.GyrCalibrate)
+        {
+            if (m_aimHandler.gyrCalibrateCompleteCount < m_aimHandler.gyrCalibrateTotalCount)
+            {
+                m_aimHandler.gyrCalibrateCompleteCount++;
+            }
+        }
+
+        o0.Geometry.Quaternion Qua = o0.Geometry.Quaternion.Identity;
+        try
+        {
+            lock (_9Axis)
+                Qua = _9Axis.Update(new byte[] { bytes[13], bytes[14], bytes[15], bytes[16], bytes[17], bytes[18] },
+                    new byte[] { bytes[7], bytes[8], bytes[9], bytes[10], bytes[11], bytes[12] },
+                    new byte[] { bytes[19], bytes[20], bytes[21], bytes[22], bytes[23], bytes[24] },
+                    bytes[1], bytes[2], bytes[3], bytes[4]);/**///9轴
+        }
+        catch (Exception e) {
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace);
+        }
+        m_aimHandler.newRotation = o0.Bow.Extension.ToUnityQuaternion(Qua);
+    }
+
+    public override void DoIdentity()
+    {
+        _9Axis.SetIdentityAndSave();
+        m_aimHandler.newRotation = o0.Bow.Extension.ToUnityQuaternion(_9Axis.getLastState().Qua);
+    }
+
+    public override void NotifyAxisOnShot()
+    {
+        _9Axis.OnShot(100, 100, 100000);
+    }
+
+    public override void CalibrateGyr(bool calibration) {
+        try {
+            _9Axis.Attitude.GyrCalibrate = calibration;
+        } catch (Exception) {}
+    }
+
+    public override void ResetGyr() {
+        _9Axis.Attitude.GyrCalibrate = true;
+        _9Axis.Attitude.GyrCalibrate = false;
+    }
+
+    public override void ResetMag() {
+        _9Axis.Attitude.MagCalibrater = new o0.IMU.MagnetometerAutoCalibrater();
+    }
+
+    public override bool IsGyrCompleted() {
+        FieldInfo fieldInfo = _9Axis.Attitude.GetType().GetField("GyrCalibrater", BindingFlags.Instance | BindingFlags.NonPublic);
+        var mm = fieldInfo.GetValue(_9Axis.Attitude) as MeanMaintainer<o0.Geometry.Vector<double>>;
+        return mm.Count > 0;
+    }
+
+    public override bool IsMagCompleted() {
+        return _9Axis.Attitude.MagCalibrater.Complete;
+    }
+
+    public override IEnumerator SaveGyr() {
+        yield return null;
+        SaveCalibrateRecord();
+    }
+
+    public override IEnumerator SaveMag() {
+        yield return null;
+        SaveCalibrateRecord();
+    }
+
+    private void SaveCalibrateRecord() 
+    {
+        try
+        {
+            string record = JsonConvert.SerializeObject(_9Axis.Attitude);
+            if (!string.IsNullOrEmpty(record))
+            {
+                Debug.Log("9轴数据序列化成功");
+                UserComp.Instance.saveCalibrateRecord(record);
+            }
+        }
+        catch (Exception e)
+        {
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace);
+        }
+    }
+
+    public override void ResumeCalibrateRecord(string record) 
+    {
+        try
+        {
+            Json.FromJson<o0.IMU._9AxisPreProcessor>(record, ref _9Axis.Attitude);
+            Debug.Log("9轴数据恢复成功" + ", MagComplete" + _9Axis.Attitude.MagCalibrater.Complete);
+        }
+        catch (Exception e)
+        {
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace);
+        }
+        m_aimHandler.CorrectMagCompleted(_9Axis.Attitude.MagCalibrater.Complete);
+    }
+}

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

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 8941c7a0e68315d448828f9c95c15e9a
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

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

@@ -0,0 +1,199 @@
+using System;
+using System.Collections;
+using UnityEngine;
+using o0._9Axis;
+using Newtonsoft.Json;
+
+public class Axis9NopackHandler : AxisBaseHandler
+{
+    o09Axis _9Axis = new o09Axis();
+    MagnetometerAutoCalibrater MagCalibrater;
+    o0GyrCalibrater GyrCalibrater;
+
+    public Axis9NopackHandler(AimHandler aimHandler) : base(aimHandler) {}
+
+    public override void Init()
+    {
+        _9Axis.LoadIdentity();
+        InitGyr(null);
+        InitMag(null);
+    }
+
+    long TimeGap = default;
+    Vector3 Acc = default;
+    Vector3 Gyr = default;
+    Vector3 Mag = default;
+    public override void Update(byte[] bytes)
+    {
+        if (bytes[7] == 0 && bytes[8] == 0 && bytes[9] == 0 && bytes[10] == 0 && bytes[11] == 0 && bytes[12] == 0)
+            return;
+        if (bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0 && bytes[22] == 0 && bytes[23] == 0 && bytes[24] == 0)
+            return;
+
+        float ax = TwoByteToFloat(bytes[7], bytes[8]);
+        float ay = TwoByteToFloat(bytes[9], bytes[10]);
+        float az = TwoByteToFloat(bytes[11], bytes[12]);
+
+        float roll = TwoByteToFloat(bytes[13], bytes[14]);
+        float pitch = TwoByteToFloat(bytes[15], bytes[16]);
+        float yaw = TwoByteToFloat(bytes[17], bytes[18]);
+
+        float x = TwoByteToFloat(bytes[19], bytes[20]);
+        float y = TwoByteToFloat(bytes[21], bytes[22]);
+        float z = TwoByteToFloat(bytes[23], bytes[24]);
+
+        float mxr = TwoByteToFloat(bytes[20], bytes[19]);
+        float myr = TwoByteToFloat(bytes[22], bytes[21]);
+        float mzr = TwoByteToFloat(bytes[24], bytes[23]);
+
+        if (CommonConfig.devicePlan == 3) {
+            Acc = new Vector3(az, ay, ax) / 32768 * 16;
+            Gyr = new Vector3(-yaw, -pitch, -roll) / 32768 * 2;
+            Mag = new Vector3(z, y, -x) / 32768 * 256; //最新版
+        } else if (CommonConfig.devicePlan == 0) {
+            Acc = new Vector3(-az, ay, -ax) / 32768 * 16;
+            Gyr = new Vector3(yaw, -pitch, roll) / 32768 * 2;
+            Mag = new Vector3(-z, y, x) / 32768 * 256; //旧版
+        } else if (CommonConfig.devicePlan == 1) {
+            Acc = new Vector3(ax, ay, az) / 32768 * 16;
+            Gyr = new Vector3(roll, pitch, yaw) / 32768 * 2;
+            Mag = new Vector3(z, x, -y) / 32768 * 256;//第一个6+3硬件
+        } else if (CommonConfig.devicePlan == 2) {
+            Acc = new Vector3(-az, ax, -ay) / 32768 * 16;
+            Gyr = new Vector3(-yaw, roll, -pitch) / 32768 * 2;
+            Mag = new Vector3(mzr, mxr, -myr) / 32768 * 256;//第二个6+3硬件
+        } 
+
+        Gyr = GyrCalibrater.Update(Gyr);
+
+        if (GyrCalibrater.Calibration) 
+        {
+            if (m_aimHandler.gyrCalibrateCompleteCount < m_aimHandler.gyrCalibrateTotalCount) 
+            {
+                m_aimHandler.gyrCalibrateCompleteCount++;
+            }
+        }
+        
+        mag0o = UnityVectorTo0o(Mag);
+        try {
+            if (!MagCalibrater.Update(mag0o)) return;
+        } catch(System.Exception) {
+            ResetMag();
+            PopupMgr.ins.ShowTipTop("磁场干扰请远离电子设备");
+            return;
+        }
+        mag0o = MagCalibrater.EllipsoidFitting.Map(mag0o);
+        Mag = o0VectorToUnity(mag0o);
+
+        var ms = (((long)bytes[1]) *60 + bytes[2])*1000 + (long)TwoByteToFloat(bytes[3], bytes[4]);
+        if(m_aimHandler.msOld == default)
+        {
+            m_aimHandler.msOld = ms;
+            return;
+        }
+        TimeGap = ms - m_aimHandler.msOld;
+        m_aimHandler.msOld = ms;
+
+        GapMs = TimeGap;
+        gyr0o = UnityVectorTo0o(Gyr);
+        acc0o = UnityVectorTo0o(Acc);
+        
+        distanceToAxis.Update(gyr0o, acc0o, mag0o, GapMs);
+        acc0o = distanceToAxis.AccCorrection(gyr0o, acc0o, GapMs);/**///轴心偏离矫正
+        Acc = o0VectorToUnity(acc0o);
+
+        m_aimHandler.newRotation = _9Axis.update(Acc, Gyr, Mag, TimeGap);
+    }
+
+    public override void DoIdentity()
+    {
+        _9Axis.SetIdentityAndSave();
+        m_aimHandler.newRotation =  _9Axis.getLastState().Qua;;
+    }
+
+    public override void NotifyAxisOnShot()
+    {
+        _9Axis.axisCSBridge.o09AxisCS.OnShot(100, 100, 100000);
+    }
+
+    public override void CalibrateGyr(bool calibration) {
+        GyrCalibrater.Calibration = calibration;
+    }
+
+    public override void InitGyr(string record) {
+        try {
+            if (!string.IsNullOrEmpty(record)) {
+                var res = JsonConvert.DeserializeObject<o0GyrCalibrater>(record);
+                if (res != null) GyrCalibrater = res;
+            } else {
+                GyrCalibrater = new o0GyrCalibrater();
+            }
+        } catch(Exception) {}
+    }
+
+    public override void InitMag(string record) {
+        try {
+            if (!string.IsNullOrEmpty(record)) {
+                MagCalibrater = JsonConvert.DeserializeObject<MagnetometerAutoCalibrater>(record, new MagJsonConverter());
+            } else {
+                MagCalibrater = new MagnetometerAutoCalibrater();
+            }
+        } catch (System.Exception e) {
+            Debug.LogError("地磁计反序列化出错");
+            Debug.LogError(e.Message);
+            Debug.LogError(e.StackTrace);
+        }
+        m_aimHandler.CorrectMagCompleted(MagCalibrater.Complete);
+    }
+
+    public override void ResetGyr() {
+        GyrCalibrater._Average = Vector3.zero;
+    }
+
+    public override void ResetMag() {
+        MagCalibrater = new MagnetometerAutoCalibrater();
+    }
+
+    public override bool IsGyrCompleted() {
+        return !GyrCalibrater._Average.Equals(Vector3.zero);
+    }
+
+    public override bool IsMagCompleted() {
+        return MagCalibrater.Complete;
+    }
+
+    public override IEnumerator SaveGyr() {
+        yield return null;
+        string mac = LoginMgr.myUserInfo.mac;
+        string record = JsonConvert.SerializeObject(GyrCalibrater);
+        UserPlayer.ins.call("userComp.saveMacCalibrate", 0, mac, record);
+    }
+
+    public override IEnumerator SaveMag() {
+        yield return null;
+        string mac = LoginMgr.myUserInfo.mac;
+        string record = JsonConvert.SerializeObject(MagCalibrater, new JsonConverter[]{new MagJsonConverter()});
+        UserPlayer.ins.call("userComp.saveMacCalibrate", 1, mac, record);
+    }
+
+    float TwoByteToFloat(byte b1, byte b2) 
+    {
+        ushort twoByte = (ushort) (b1 * 256 + b2);
+        short shortNum = (short) twoByte;
+        return (float) shortNum; 
+    }
+
+    o0.Bow.DistanceToAxis distanceToAxis = new o0.Bow.DistanceToAxis();
+    double GapMs;
+    o0.Geometry.Vector<double> gyr0o;
+    o0.Geometry.Vector<double> acc0o;
+    o0.Geometry.Vector<double> mag0o;
+    o0.Geometry.Vector<double> UnityVectorTo0o(Vector3 src) 
+    {
+        return new o0.Geometry.Vector<double>(double.Parse(src.x.ToString()), double.Parse(src.y.ToString()), double.Parse(src.z.ToString()));
+    }
+    Vector3 o0VectorToUnity(o0.Geometry.Vector<double> src) 
+    {
+        return new Vector3(float.Parse(src.x.ToString()), float.Parse(src.y.ToString()), float.Parse(src.z.ToString()));
+    }
+}

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

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 88a85284bf4ad1e4c937715eb549bb46
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

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

@@ -0,0 +1,24 @@
+using System.Collections;
+
+public abstract class AxisBaseHandler
+{
+    protected AimHandler m_aimHandler;
+    public AxisBaseHandler(AimHandler aimHandler)
+    {
+        m_aimHandler = aimHandler;
+    }
+    public abstract void Init();
+    public abstract void Update(byte[] bytes);
+    public abstract void DoIdentity();
+    public abstract void NotifyAxisOnShot();
+    public abstract void CalibrateGyr(bool calibration);
+    public virtual void InitGyr(string record) {}
+    public virtual void InitMag(string record) {}
+    public abstract void ResetGyr();
+    public abstract void ResetMag();
+    public abstract bool IsGyrCompleted();
+    public abstract bool IsMagCompleted();
+    public abstract IEnumerator SaveGyr();
+    public abstract IEnumerator SaveMag();
+    public virtual void ResumeCalibrateRecord(string record) {}
+}

Assets/BowArrow/Scripts/Bluetooth/New/AxisBaseHandler.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 1c42dcc3da598f541b69848383ec2605
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

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

@@ -0,0 +1,291 @@
+using o0.Geometry;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Newtonsoft.Json;
+
+namespace o0.Bow
+{
+    public class o0663Axis
+    {
+        public IMU._663AxisPreProcessor Attitude;
+
+        public static List<UnityEngine.Quaternion> QuaTest = new List<UnityEngine.Quaternion>();
+        public o0663Axis(Vector<int> Gyr1ByteIndex = default, Vector<int> Gyr2ByteIndex = default, Vector<int> Acc1ByteIndex = default, Vector<int> Acc2ByteIndex = default, Vector<int> MagByteIndex = default)
+        {
+            Attitude = new IMU._663AxisPreProcessor(Gyr1ByteIndex, Gyr2ByteIndex, Acc1ByteIndex, Acc2ByteIndex, MagByteIndex);
+
+            QuaTest.Add(UnityEngine.Quaternion.identity);
+            QuaTest.Add(UnityEngine.Quaternion.identity);
+        }
+
+        static public Vector<double> AccIdentity = new Vector<double>(0, -1, 0);
+        static public Vector<double> MagIdentity = new Vector<double>(-1, 2, 0).Normalized;
+        public class State
+        {
+            public double TimeGap;
+            public Vector<double> Acc = AccIdentity;
+            public Vector<double> AccSmooth = AccIdentity;
+
+            public Vector<double> Gyr;
+            public Vector<double> Mag = MagIdentity;
+            public Vector<double> MagSmooth = MagIdentity;
+
+            public double GapMS;
+
+            public Geometry.Quaternion Qua = Geometry.Quaternion.Identity;
+            public Geometry.Quaternion QuaSmooth = Geometry.Quaternion.Identity;
+            public double Variance = 1;
+            public double GyrVariance = 1;
+            public double AccVariance = 1;
+            public double MagVariance = 1;
+            public Geometry.Quaternion QuaAccMag = Geometry.Quaternion.Identity;
+            public int QuaAccMagCount = 0;
+            public double AccMagVariance = 1;
+            public double TotalVariance = 1;
+        }
+
+
+        public List<State> States = new List<State>();
+        public int StatesMaxCount = 1000;
+
+        public float x;
+        public float y;
+        public float z;
+
+        long TimeGapOld;
+
+        double lastv = default;
+        public Geometry.Quaternion Update(IEnumerable<byte> gyr1Byte, IEnumerable<byte> gyr2Byte, IEnumerable<byte> acc1Byte, IEnumerable<byte> acc2Byte, IEnumerable<byte> magByte,
+            byte min, byte sec, byte ms1, byte ms2)
+        {
+            if(lastv != Attitude.MagCalibrater?.Variance)
+            {
+                lastv = Attitude.MagCalibrater?.Variance ?? default;
+                Debug.Log(lastv);
+            }
+            var (Gyr, Acc, Mag, TimeGap) = Attitude.Update(gyr1Byte, gyr2Byte, acc1Byte, acc2Byte, magByte, min, sec, ms1, ms2);
+
+
+            var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
+
+
+            var Last = States.LastOrDefault() ?? new State();
+            States.Add(new State());
+            if (States.Count > StatesMaxCount)
+                States.RemoveAt(0);
+            var state = States.Last();
+            state.Acc = Acc;
+
+
+            //Debug.Log(Gyr.magnitude);
+
+            state.Gyr = Gyr;
+            state.Mag = Mag;/**/
+            state.TimeGap = TimeGap;
+            if (States.Count <= 1)
+                return Geometry.Quaternion.Identity;
+            return Process9Axis(Last, state);
+        }
+        int ShakeFrame;
+        int AccVarianceInput;
+        /// //////////////////向前追溯多少帧//向后多少帧计算抖动
+        public void OnShot(int TrackBack, int ShakeFrame, int AccVarianceInput)
+        {
+            this.AccVarianceInput = AccVarianceInput;
+            TrackBack = Math.Min(TrackBack, States.Count);
+            var startI = States.Count - TrackBack;
+            State Last = default;
+            foreach (var i in TrackBack.Range())
+            {
+                var state = States[startI + i];
+
+                if (Last != default)
+                    Process9Axis(Last, state);
+                Last = state;
+            }
+
+            this.ShakeFrame = ShakeFrame;
+            Debug.Log("OnShot");/**/
+        }
+        public double diff = 0.001;
+        public Geometry.Quaternion Process9Axis(State Last, State state)
+        {
+            var Acc = state.Acc;
+            var Gyr = state.Gyr;
+            var Mag = state.Mag;
+            double TimeGap = state.TimeGap;
+
+            o0UIRawImageTester.UpdateAllOffset();
+
+
+            var LastQuaternion = Last.Qua;
+
+
+
+            var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
+            var quaGyr = LastQuaternion * GyrOperator;
+
+
+            //TestVector.Update9AxisRotation(GyrOperator, 1);
+            //TestVector.SetAcc(Acc / 10, 1);
+            //TestVector.SetMag(Mag, 1);
+            var accTest = Geometry.Quaternion.FromToRotation(Last.Acc, Acc).Inversed;
+            var magTest = Geometry.Quaternion.FromToRotation(Last.Mag, Mag).Inversed;
+            //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).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 MagVariance = 10 + Math.Pow(Math.Abs(state.Mag.Length - 1) / 1 * MagLengthToAngle, 4);/**/
+
+
+
+            if (double.IsInfinity(GyrVariance))
+                GyrVariance = double.MinValue;
+            if (double.IsNaN(GyrVariance))
+                GyrVariance = double.MinValue;
+            if (double.IsNaN(AccVariance))
+                AccVariance = double.MinValue;
+            if (double.IsNaN(MagVariance))
+                MagVariance = double.MinValue;
+
+            state.Variance = GyrVariance;
+            state.Variance = state.Variance * (AccVariance + MagVariance) / (state.Variance + (AccVariance + MagVariance));
+
+
+            if (double.IsNaN(state.Variance))
+                state.Variance = double.MinValue;
+
+            var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.Acc, state.Mag, (float)(AccVariance / (AccVariance + MagVariance)));
+            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))
+                quaFirst = Last.Qua;
+
+            var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
+
+            state.Qua = AccVariance < MagVariance ? Geometry.Quaternion.FormQuaternion(quaFirst, AccIdentity, state.Acc, (float)quaSecondRate) : Geometry.Quaternion.FormQuaternion(quaFirst, MagIdentity, state.Mag, (float)quaSecondRate);
+
+
+
+
+            ///////////////////////////////////////////////////////////////
+
+            if (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
+
+
+            //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[1] = o0Project.o0.FormQuaternion(QuaTest[1] * GyrOperator, MagIdentity, state.MagSmooth, 1);
+            //Debug.Log(o09AxisCS.QuaTest[0]);
+
+            if (ShakeFrame > 0)
+            {
+                --ShakeFrame;
+                if (ShakeFrame == 0)
+                    AccVarianceInput = default;
+            }
+            return state.QuaSmooth;
+        }
+
+        public void Init()
+        {
+            States.Last().AccVariance = 1000;
+            States.Last().GyrVariance = 1000;
+            States.Last().MagVariance = 1000;
+            States.Last().AccMagVariance = 1000;
+            States.Last().TotalVariance = 1000;
+        }
+        public Geometry.Quaternion SetIdentity()
+        {
+            //UnityEngine.Quaternion qua = default;
+
+            //int averageCount = Math.Min(States.Count, 5);
+            int averageCount = Math.Min(States.Count, 50);
+            Vector<double> aveAcc = Vector<double>.Zero;
+            Vector<double> aveMag = Vector<double>.Zero;
+            for (var i = States.Count - averageCount; i < States.Count; ++i)
+            {
+                aveAcc += States[i].Acc;
+                aveMag += States[i].Mag;
+            }
+            aveAcc /= averageCount;
+            aveMag /= averageCount;
+            //AccIdentity = AccOld;
+            //MagIdentity = MagOld;
+            AccIdentity = aveAcc;
+            MagIdentity = aveMag;
+            //qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,AccIdentity, 1);
+            //AccIdentity=qua*AccIdentity;
+            //MagIdentity = qua*MagIdentity;
+            States.Last().Qua = Geometry.Quaternion.Identity;
+            States.Last().QuaSmooth = Geometry.Quaternion.Identity;
+            //States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
+            States.Last().Variance = 0.0000001;
+            States.Last().AccVariance = 0.0000001;
+            States.Last().GyrVariance = 0.0000001;
+            States.Last().MagVariance = 0.0000001;
+            States.Last().QuaAccMag = Geometry.Quaternion.Identity;
+            States.Last().QuaAccMagCount = 0;
+            States.Last().AccMagVariance = 0.0000001;
+            States.Last().TotalVariance = 0.0000001;
+
+
+            return States.Last().Qua;
+        }
+
+        public State getLastState()
+        {
+            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/o0663Axis.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 3ad31f80d19d0d84db22490a519ff50d
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

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

@@ -0,0 +1,296 @@
+using o0.Geometry;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Newtonsoft.Json;
+
+
+namespace o0.Bow
+{
+    public class o09AxisAfterXiaMenFromDll
+    {
+        public IMU._9AxisPreProcessor Attitude;
+        public IMU.HardwareVariance GyrHardwareVariance;
+        public IMU.HardwareVariance AccHardwareVariance;
+        public IMU.HardwareVariance MagHardwareVariance;
+
+        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);
+            GyrHardwareVariance = new IMU.HardwareVariance();
+            AccHardwareVariance = new IMU.HardwareVariance();
+            MagHardwareVariance = new IMU.HardwareVariance();
+
+            QuaTest.Add(UnityEngine.Quaternion.identity);
+            QuaTest.Add(UnityEngine.Quaternion.identity);
+        }
+
+        static public Vector<double> AccIdentity = new Vector<double>(0, -1, 0);
+        static public Vector<double> MagIdentity = new Vector<double>(-1, 2, 0).Normalized;
+        public class State
+        {
+            public double TimeGap;
+            public Vector<double> Acc = AccIdentity;
+            public Vector<double> AccSmooth = AccIdentity;
+
+            public Vector<double> Gyr;
+            public Vector<double> Mag = MagIdentity;
+            public Vector<double> MagSmooth = MagIdentity;
+
+            public double GapMS;
+
+            public Geometry.Quaternion Qua = Geometry.Quaternion.Identity;
+            public Geometry.Quaternion QuaSmooth = Geometry.Quaternion.Identity;
+            public double Variance = 1;
+            public double GyrVariance = 1;
+            public double AccVariance = 1;
+            public double MagVariance = 1;
+            public Geometry.Quaternion QuaAccMag = Geometry.Quaternion.Identity;
+            public int QuaAccMagCount = 0;
+            public double AccMagVariance = 1;
+            public double TotalVariance = 1;
+        }
+
+
+        public List<State> States = new List<State>();
+        public int StatesMaxCount = 1000;
+
+
+        public Geometry.Quaternion Update(IEnumerable<byte> gyrByte, IEnumerable<byte> accByte, IEnumerable<byte> magByte,
+            byte min, byte sec, byte ms1, byte ms2)
+        {
+            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)
+                Debug.Log("GyrHardwareVariance: " + Buffer.Length);
+            Buffer = AccHardwareVariance.Update(Acc);
+            if (Buffer != default)
+                Debug.Log("AccHardwareVariance: " + Buffer.Length);
+            Buffer = MagHardwareVariance.Update(Mag);
+            if (Buffer != default)
+                Debug.Log("MagHardwareVariance: " + Buffer.Length);
+
+            var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
+
+
+            var Last = States.LastOrDefault() ?? new State();
+            States.Add(new State());
+            if (States.Count > StatesMaxCount)
+                States.RemoveAt(0);
+            var state = States.Last();
+            state.Acc = Acc;
+
+
+            //Debug.Log(Gyr.magnitude);
+
+            state.Gyr = Gyr;
+            state.Mag = Mag;/**/
+            state.TimeGap = TimeGap;
+            if (States.Count <= 1)
+                return Geometry.Quaternion.Identity;
+            return Process9Axis(Last, state);
+        }
+        int ShakeFrame;
+        int AccVarianceInput;
+        /// //////////////////向前追溯多少帧//向后多少帧计算抖动
+        public void OnShot(int TrackBack, int ShakeFrame, int AccVarianceInput)
+        {
+            this.AccVarianceInput = AccVarianceInput;
+            TrackBack = Math.Min(TrackBack, States.Count);
+            var startI = States.Count - TrackBack;
+            State Last = default;
+            foreach (var i in TrackBack.Range())
+            {
+                var state = States[startI + i];
+
+                if (Last != default)
+                    Process9Axis(Last, state);
+                Last = state;
+            }
+
+            this.ShakeFrame = ShakeFrame;
+            Debug.Log("OnShot");/**/
+        }
+        public double diff = 0.001;
+        public Geometry.Quaternion Process9Axis(State Last, State state)
+        {
+            var Acc = state.Acc;
+            var Gyr = state.Gyr;
+            var Mag = state.Mag;
+            double TimeGap = state.TimeGap;
+
+            o0UIRawImageTester.UpdateAllOffset();
+
+
+            var LastQuaternion = Last.Qua;
+
+
+
+            var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
+            var quaGyr = LastQuaternion * GyrOperator;
+
+
+            //TestVector.Update9AxisRotation(GyrOperator, 1);
+            //TestVector.SetAcc(Acc / 10, 1);
+            //TestVector.SetMag(Mag, 1);
+            var accTest = Geometry.Quaternion.FromToRotation(Last.Acc, Acc).Inversed;
+            var magTest = Geometry.Quaternion.FromToRotation(Last.Mag, Mag).Inversed;
+            //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).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 MagVariance = 10 + Math.Pow(Math.Abs(state.Mag.Length - 1) / 1 * MagLengthToAngle, 4);/**/
+
+
+
+            if (double.IsInfinity(GyrVariance))
+                GyrVariance = double.MinValue;
+            if (double.IsNaN(GyrVariance))
+                GyrVariance = double.MinValue;
+            if (double.IsNaN(AccVariance))
+                AccVariance = double.MinValue;
+            if (double.IsNaN(MagVariance))
+                MagVariance = double.MinValue;
+
+            state.Variance = GyrVariance;
+            state.Variance = state.Variance * (AccVariance + MagVariance) / (state.Variance + (AccVariance + MagVariance));
+
+
+            if (double.IsNaN(state.Variance))
+                state.Variance = double.MinValue;
+
+            var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.Acc, state.Mag, (float)(AccVariance / (AccVariance + MagVariance)));
+            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))
+                quaFirst = Last.Qua;
+
+            var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
+
+            state.Qua = AccVariance < MagVariance ? Geometry.Quaternion.FormQuaternion(quaFirst, AccIdentity, state.Acc, (float)quaSecondRate) : Geometry.Quaternion.FormQuaternion(quaFirst, MagIdentity, state.Mag, (float)quaSecondRate);
+
+
+
+
+            ///////////////////////////////////////////////////////////////
+
+            if (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
+
+
+            //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[1] = o0Project.o0.FormQuaternion(QuaTest[1] * GyrOperator, MagIdentity, state.MagSmooth, 1);
+            //Debug.Log(o09AxisCS.QuaTest[0]);
+
+            if (ShakeFrame > 0)
+            {
+                --ShakeFrame;
+                if (ShakeFrame == 0)
+                    AccVarianceInput = default;
+            }
+            return state.QuaSmooth;
+        }
+
+        public void Init()
+        {
+            States.Last().AccVariance = 1000;
+            States.Last().GyrVariance = 1000;
+            States.Last().MagVariance = 1000;
+            States.Last().AccMagVariance = 1000;
+            States.Last().TotalVariance = 1000;
+        }
+        public Geometry.Quaternion SetIdentity()
+        {
+            //UnityEngine.Quaternion qua = default;
+
+            //int averageCount = Math.Min(States.Count, 5);
+            int averageCount = Math.Min(States.Count, 50);
+            Vector<double> aveAcc = Vector<double>.Zero;
+            Vector<double> aveMag = Vector<double>.Zero;
+            for (var i = States.Count - averageCount; i < States.Count; ++i)
+            {
+                aveAcc += States[i].Acc;
+                aveMag += States[i].Mag;
+            }
+            aveAcc /= averageCount;
+            aveMag /= averageCount;
+            //AccIdentity = AccOld;
+            //MagIdentity = MagOld;
+            AccIdentity = aveAcc;
+            MagIdentity = aveMag;
+            //qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,AccIdentity, 1);
+            //AccIdentity=qua*AccIdentity;
+            //MagIdentity = qua*MagIdentity;
+            States.Last().Qua = Geometry.Quaternion.Identity;
+            States.Last().QuaSmooth = Geometry.Quaternion.Identity;
+            //States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
+            States.Last().Variance = 0.0000001;
+            States.Last().AccVariance = 0.0000001;
+            States.Last().GyrVariance = 0.0000001;
+            States.Last().MagVariance = 0.0000001;
+            States.Last().QuaAccMag = Geometry.Quaternion.Identity;
+            States.Last().QuaAccMagCount = 0;
+            States.Last().AccMagVariance = 0.0000001;
+            States.Last().TotalVariance = 0.0000001;
+
+
+            return States.Last().Qua;
+        }
+
+        public State getLastState()
+        {
+            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.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 28debb66f5f749748a4c462c09e1051f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/BowArrow/Scripts/Bluetooth/ShootCheck.cs

@@ -357,7 +357,7 @@ public class ShootCheck : MonoBehaviour
             ShootCheck.ins.webSocket.Send(logTxt);
         } catch (Exception) {}
         //调用游戏中的射箭接口
-        AimHandler.ins._9Axis.axisCSBridge.o09AxisCS.OnShot(100, 100, 100000);
+        AimHandler.ins.NotifyAxisOnShot();
         if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked) {
             SB_EventSystem.ins.ClickMouse(); 
         } else if (ArmBow.ins) {

Assets/BowArrow/Scripts/Bluetooth/o0Bow.cs

@@ -0,0 +1,18 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using o0.Geometry;
+using o0.Num;
+
+namespace o0.Bow
+{
+    public static partial class Extension
+    {
+        public static UnityEngine.Quaternion ToUnityQuaternion(this Quaternion q) => new UnityEngine.Quaternion((float)q.x, (float)q.y, (float)q.z, (float)q.w);
+        public static Quaternion Too0Quaternion(this UnityEngine.Quaternion q) => new Quaternion(q.x, q.y, q.z, q.w);
+        public static UnityEngine.Vector3 ToUnityVector(this Vector<double> v) => new UnityEngine.Vector3((float)v.x, (float)v.y, (float)v.z);
+        public static Vector<double> Too0Vector(this UnityEngine.Vector3 v) => new Vector<double>(v.x, v.y, v.z);
+    }
+}

Assets/BowArrow/Scripts/Bluetooth/o0Bow.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: df58c862ccb8bef4cb2f17e0693c9d42
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/BowArrow/Scripts/Bluetooth/o0Lib/o0NetIMU.dll.meta

@@ -0,0 +1,33 @@
+fileFormatVersion: 2
+guid: f1ec028e3951e4b42b3517d17ffc7077
+PluginImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  iconMap: {}
+  executionOrder: {}
+  defineConstraints: []
+  isPreloaded: 0
+  isOverridable: 0
+  isExplicitlyReferenced: 0
+  validateReferences: 1
+  platformData:
+  - first:
+      Any: 
+    second:
+      enabled: 1
+      settings: {}
+  - first:
+      Editor: Editor
+    second:
+      enabled: 0
+      settings:
+        DefaultValueInitialized: true
+  - first:
+      Windows Store Apps: WindowsStoreApps
+    second:
+      enabled: 0
+      settings:
+        CPU: AnyCPU
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/BowArrow/Scripts/Network/SocketComp/UserComp.cs

@@ -1,6 +1,5 @@
-using System.Collections;
-using System.Collections.Generic;
-using UnityEngine;
+using System;
+
 /* Socket组件-用户 */
 public class UserComp : JCUnityLib.Singleton<UserComp>
 {
@@ -11,4 +10,37 @@ public class UserComp : JCUnityLib.Singleton<UserComp>
     public void saveUserInfo(UserInfo userInfo) {
         UserPlayer.ins.call("userComp.saveUserInfo", userInfo);
     }
+
+    public void saveMac()
+    {
+        string mac = LoginMgr.myUserInfo.mac;
+        if (string.IsNullOrEmpty(mac)) return;
+        int type = AimHandler.ins.DeviceType;
+        if (type > 0) {
+            UserPlayer.ins.call("userComp.saveMac2", mac, type);
+        } else {
+            Action<Newtonsoft.Json.Linq.JToken> cb = (Newtonsoft.Json.Linq.JToken o) => {
+                string gyrStr = o.Value<string>("gyr");
+                string magStr = o.Value<string>("mag");
+                AimHandler.ins.InitGyr(gyrStr);
+                AimHandler.ins.InitMag(magStr);
+            };
+            UserPlayer.ins.call("userComp.saveMac", new object[]{mac}, cb);
+        }
+    } 
+
+    public void saveCalibrateRecord(string record) {
+        string mac = LoginMgr.myUserInfo.mac;
+        if (string.IsNullOrEmpty(mac)) return;
+        int type = AimHandler.ins.DeviceType;
+        if (type > 0) {
+            UserPlayer.ins.call("userComp.saveCalibrateRecord", type, record, mac);
+        }
+    }
+
+    #region 被服务端调用的函数
+    public void onResumeCalibrateRecord(string record) {
+        AimHandler.ins.ResumeCalibrateRecord(record);
+    }
+    #endregion
 }

Assets/BowArrow/Scripts/Network/UserPlayer.cs

@@ -11,6 +11,7 @@ public class UserPlayer : JCEntity
     public UserPlayer() {
         ins = this;
         components.Add("PKComp", PKComp.Instance);
+        components.Add("UserComp", UserComp.Instance);
     }
     public static void ConnectServer() {
         if (ins != null) return;