smart-bow-sdk-dll/SmartBowSDK/Axis9Handler_SDK.cs

using System;
using UnityEngine;
using Newtonsoft.Json;
using System.Reflection;

namespace SmartBowSDK
{
    public class Axis9Handler_SDK : AxisBaseHandler_SDK
    {
        private o0.Bow.o09AxisAfterXiaMenFromDll_SDK _9Axis;
        private AttitudeJson_SDK attitudeJson = new AttitudeJson_SDK();

        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 Axis9Handler_SDK(AimHandler_SDK aimHandler) : base(aimHandler) { }

        public override void Init()
        {
            _9Axis = new o0.Bow.o09AxisAfterXiaMenFromDll_SDK(GyrByteIndex, AccByteIndex, MagByteIndex);
            LoadIdentity();
        }

        public override void Update(byte[] bytes)
        {
            UpdateCheckGyrCompleted();

            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);
            }
            m_aimHandler.SetNewRotation(Qua);

            UpdateCheckMagCompleted();
        }

        private void UpdateCheckGyrCompleted()
        {
            if (_9Axis.Attitude.GyrCalibrate && GetGyrProgress() >= 1)
            {
                StopGyrCalibration();
                SaveCalibrateRecord();
            }
        }

        private bool _magCompleted;
        private void UpdateCheckMagCompleted()
        {
            bool magCompleted = IsMagCompleted();
            if (magCompleted && !_magCompleted)
                SaveCalibrateRecord();
            _magCompleted = magCompleted;
        }
        private void CorrectMagCompleted()
        {
            _magCompleted = IsMagCompleted();
        }

        public override void DoIdentity()
        {
            if (_9Axis.States.Count == 0) return; //没数据时触发视角归位会产生NAN，从而导致算法崩坏
            SetIdentityAndSave();
            m_aimHandler.SetNewRotation(_9Axis.getLastState().Qua);
        }

        public override void NotifyAxisOnShot()
        {
            _9Axis.OnShot(100, 100, 100000);
        }

        public override void StartGyrCalibration()
        {
            _9Axis.Attitude.GyrCalibrate = true;
        }

        public override void StopGyrCalibration()
        {
            _9Axis.Attitude.GyrCalibrate = false;
        }

        public override bool IsGyrCalibrating()
        {
            return _9Axis.Attitude.GyrCalibrate;
        }

        public override float GetGyrProgress()
        {
            return _9Axis.Attitude.GyrCalibrater.Count / 2000f;
        }

        public override bool IsGyrCompleted()
        {
            return _9Axis.Attitude.GyrCalibrater.Count >= 2000;
        }

        public override void StartMagCalibration()
        {
            _9Axis.Attitude.MagCalibrater = new o0.IMU.MagnetometerAutoCalibrater(0.001d);
        }

        public override bool IsMagCompleted()
        {
            return _9Axis.Attitude.MagCalibrater.Complete;
        }

        public override void SaveCalibrateRecord()
        {
            try
            {
                SmartBowLogger.Log(this, "开始序列化九轴数据");
                string record = attitudeJson.Stringify(_9Axis.Attitude);
                if (!string.IsNullOrEmpty(record))
                {
                    SmartBowLogger.Log(this, $"九轴数据序列化完成(长度{record.Length})\nGyrMeanLen:{_9Axis.Attitude.GyrCalibrater.Mean.Length}\nMagVariance:{_9Axis.Attitude.MagCalibrater.Variance}");
                    if (!IsGyrCompleted())
                    {
                        SmartBowLogger.LogWarning(this, "陀螺仪未校准，因校准数据不完整无法上传！");
                        return;
                    }
                    if (!IsMagCompleted())
                    {
                        SmartBowLogger.LogWarning(this, "地磁计未校准，因校准数据不完整无法上传！");
                        return;
                    }
                    string macAddress = m_aimHandler.smartBowHelper.bluetoothAim.macAddress;
                    if (macAddress == null)
                    {
                        Debug.Log("MacAddress为null，无法上传校准记录！");
                        return;
                    }
                    m_aimHandler.smartBowHelper.smartBowNetwork.SaveCalibrateRecord(macAddress, record);
                }
            }
            catch (Exception e)
            {
                Debug.LogError(e);
            }
        }

        public override void ResumeCalibrateRecord(string record)
        {
            try
            {
                SmartBowLogger.Log(this, $"成功加载服务端的九轴数据(长度{record.Length})");
                _9Axis.Attitude = attitudeJson.Parse<o0.IMU._9AxisPreProcessor>(record);
                SmartBowLogger.Log(this, $"九轴数据恢复成功\nGyrMeanLen:{_9Axis.Attitude.GyrCalibrater.Mean.Length}\nMagVariance:{_9Axis.Attitude.MagCalibrater.Variance}");
                if (!IsAxisRight())
                {
                    _9Axis.Attitude = new o0.IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
                    SmartBowLogger.LogWarning(this, "跟保存的轴向不相同，重置校准记录！");
                }
            }
            catch (Exception e)
            {
                Debug.LogError(e);
            }
            CorrectMagCompleted();
        }

        //判断轴向是否正确
        private bool IsAxisRight()
        {
            object o = _9Axis.Attitude;
            Type t = o.GetType();
            var _gyr = t.GetField("GyrByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
            var _acc = t.GetField("AccByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
            var _mag = t.GetField("MagByteIndex", BindingFlags.Instance | BindingFlags.NonPublic).GetValue(o);
            bool gyrEqual = GyrByteIndex.Equals(_gyr);
            bool accEqual = AccByteIndex.Equals(_acc);
            bool magEqual = MagByteIndex.Equals(_mag);
            return gyrEqual && accEqual && magEqual;
        }

        private void LoadIdentity()
        {
            try
            {
                string accStr = PlayerPrefs.GetString("AccIdentity0", "");
                if (accStr.Length > 0)
                {
                    double[] arr = JsonConvert.DeserializeObject<double[]>(accStr);
                    var v = new o0.Geometry.Vector<double>(arr);
                    if (!double.IsNaN(v.x) && !double.IsInfinity(v.x)
                        && !double.IsNaN(v.y) && !double.IsInfinity(v.y)
                        && !double.IsNaN(v.z) && !double.IsInfinity(v.z)
                    )
                    {
                        _9Axis.AccIdentity = v;
                    }
                }
                string magStr = PlayerPrefs.GetString("MagIdentity0", "");
                if (magStr.Length > 0)
                {
                    double[] arr = JsonConvert.DeserializeObject<double[]>(magStr);
                    var v = new o0.Geometry.Vector<double>(arr);
                    if (!double.IsNaN(v.x) && !double.IsInfinity(v.x)
                        && !double.IsNaN(v.y) && !double.IsInfinity(v.y)
                        && !double.IsNaN(v.z) && !double.IsInfinity(v.z)
                    )
                    {
                        _9Axis.MagIdentity = v;
                    }
                }
            }
            catch (Exception e)
            {
                Debug.LogError(e);
            }
        }

        private void SetIdentityAndSave()
        {
            _9Axis.SetIdentity();
            var a = _9Axis.AccIdentity;
            var m = _9Axis.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
            }));
        }
    }
}