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

using System;
using UnityEngine;
using System.Collections;
using Newtonsoft.Json;
using o0;
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);
        _9Axis.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.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;
        Debug.Log("陀螺仪校准结果主动重置！");
    }

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

    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 = attitudeJson.Stringify(_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
        {
            _9Axis.Attitude = attitudeJson.Parse<o0.IMU._9AxisPreProcessor>(record);
            Debug.Log("9轴反序列化完成");
            PrintAxisInfo();
            if (!IsAxisRight())
            {
                _9Axis.Attitude = new o0.IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
                Debug.Log("跟保存的轴向不相同，重置校准记录！");
                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());
    }
}