smart-bow-infrared/Assets/BowArrow/Scripts/Bluetooth/New/Axis9Handler.cs

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

public class Axis9Handler : AxisBaseHandler
{
    private o0.Bow.o09AxisAfterXiaMenFromDll _9Axis;
    private AttitudeJson attitudeJson = new AttitudeJson();
    /*
    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 Axis9Handler(AimHandler aimHandler) : base(aimHandler) {}

    public override void Init()
    {
        CommonConfig.devicePlan = 3;
        _9Axis = new o0.Bow.o09AxisAfterXiaMenFromDll(GyrByteIndex, AccByteIndex, MagByteIndex);
        _9Axis.Attitude = new o0.IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
        LoadIdentity();
        Debug.Log("9轴启动成功（首次初始化）");
        PrintAxisInfo();
    }

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

    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 CalibrateGyr(bool calibration) {
        try {
            _9Axis.Attitude.GyrCalibrate = calibration;
        } catch (Exception) {}
    }

    public override void ResetGyr() {
        _9Axis.Attitude.GyrCalibrate = true;
        _9Axis.Attitude.GyrCalibrate = false;
        Debug.Log("陀螺仪校准结果主动重置！");
    }

    public override void ResetMag() {
        _SaveOldMag();
        _9Axis.Attitude.MagCalibrater = new o0.IMU.MagnetometerAutoCalibrater(0.001d);
        Debug.Log("地磁计校准结果主动重置！");
    }

    
    // private string _oldEllipsoidFitting = "{\"Center\":[-3.0443925790756605,-3.6882526269162423,-0.9424965068421579],\"CorrectMatrixArray\":[[0.2584454016577151,0.00410177628600846,0.005727574526798527],[0.00410177628600846,0.2639115667519193,-0.03866026824619133],[0.005727574526798523,-0.03866026824619133,0.23912157876854007]],\"Radius\":[4.764270274783962,3.854047926765827,3.4233050788114037]}";
    // private double _oldVariance = 0.000362;
    private void _SaveOldMag()
    {
        // if (!_9Axis.Attitude.MagCalibrater.Complete) return;
        // _oldVariance = _9Axis.Attitude.MagCalibrater.Variance;
        // _oldEllipsoidFitting = attitudeJson.Stringify(_9Axis.Attitude.MagCalibrater.EllipsoidFitting);
    }

    //应用不精准地磁数据，为让客户不地磁校准成功也能动
    public override void ApplyImpreciseMag()
    {
        // if (_9Axis.Attitude.MagCalibrater.Complete) return;
        // _9Axis.Attitude.MagCalibrater.Lock = true;
        // _9Axis.Attitude.MagCalibrater.Init();
        // _9Axis.Attitude.MagCalibrater.EllipsoidFitting = attitudeJson.Parse<o0.IMU.EllipsoidFitting>(_oldEllipsoidFitting);
        // _9Axis.Attitude.MagCalibrater.Variance = _oldVariance;
    }

    public override bool IsGyrCompleted() {
        return _9Axis.Attitude.GyrCalibrater.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
        {
            SideTipView.ShowTip("开始序列化九轴数据", Color.white);
            string record = attitudeJson.Stringify(_9Axis.Attitude);
            if (!string.IsNullOrEmpty(record))
            {
                SideTipView.ShowTip($"九轴数据序列化完成(长度{record.Length})\nGyrMeanLen:{_9Axis.Attitude.GyrCalibrater.Mean.Length}\nMagVariance:{_9Axis.Attitude.MagCalibrater.Variance}", Color.white);
                Debug.Log("9轴数据序列化成功");
                if (!IsGyrCompleted()) {
                    SideTipView.ShowTip("陀螺仪未校准，因校准数据不完整无法上传！", Color.yellow);
                    return;
                }
                if (!IsMagCompleted()) {
                    SideTipView.ShowTip("地磁计未校准，因校准数据不完整无法上传！", Color.yellow);
                    return;
                }
                UserComp.Instance.saveCalibrateRecord(record);
                SideTipView.ShowTip("正在上传九轴数据", Color.white);
            }
        }
        catch (Exception e)
        {
            Debug.LogError(e.Message);
            Debug.LogError(e.StackTrace);
        }
    }

    public override void ResumeCalibrateRecord(string record) 
    {
        try
        {
            SideTipView.ShowTip($"成功加载服务端的九轴数据(长度{record.Length})", Color.white);
            _9Axis.Attitude = attitudeJson.Parse<o0.IMU._9AxisPreProcessor>(record);
            Debug.Log("9轴反序列化完成");
            SideTipView.ShowTip($"九轴数据恢复成功\nGyrMeanLen:{_9Axis.Attitude.GyrCalibrater.Mean.Length}\nMagVariance:{_9Axis.Attitude.MagCalibrater.Variance}", Color.white);
            PrintAxisInfo();
            if (!IsAxisRight())
            {
                _9Axis.Attitude = new o0.IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
                Debug.Log("跟保存的轴向不相同，重置校准记录！");
                SideTipView.ShowTip("跟保存的轴向不相同，重置校准记录！", Color.yellow);
                PrintAxisInfo();
            }
            Debug.Log("9轴数据恢复结果: " + _9Axis.Attitude.MagCalibrater.Complete);
        }
        catch (Exception e)
        {
            Debug.LogError(e.Message);
            Debug.LogError(e.StackTrace);
        }
        m_aimHandler.CorrectMagCompleted(_9Axis.Attitude.MagCalibrater.Complete);
    }

    //判断轴向是否正确
    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;
    }

    //打印当前轴向
    public void PrintAxisInfo()
    {
        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);
        Debug.Log($"当前轴向：gyr:{_gyr.ToString()} acc:{_acc.ToString()} mag:{_mag.ToString()}");
        Debug.Log("当前轴向是否正确：" + IsAxisRight());
    }

    #region 视角归位-恢复和保存
    private void LoadIdentity() 
    {
        try {
            string accStr = PlayerPrefs.GetString("AccIdentity0", "");
            if (accStr.Length > 0) {
                double[] arr = JsonConvert.DeserializeObject<double[]>(accStr);
                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)
                ) {
                    o0.Bow.o09AxisAfterXiaMenFromDll.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)
                ) {
                    o0.Bow.o09AxisAfterXiaMenFromDll.MagIdentity = v;
                }
            }
        } catch (System.Exception e) { 
            Debug.LogError(e.Message);
            Debug.LogError(e.StackTrace); 
        }
    }

    private void SetIdentityAndSave()
    {
        _9Axis.SetIdentity();
        //save
        var a = o0.Bow.o09AxisAfterXiaMenFromDll.AccIdentity;
        var m = o0.Bow.o09AxisAfterXiaMenFromDll.MagIdentity;
        PlayerPrefs.SetString("AccIdentity0", JsonConvert.SerializeObject(new double[]{
            a.x, a.y, a.z
        }));
        PlayerPrefs.SetString("MagIdentity0",JsonConvert.SerializeObject(new double[]{
            m.x, m.y, m.z
        }));
    }
    #endregion
}