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

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, -1, 2);/**///双陀螺仪 usb向上 pcb向右 电池向右

    public Axis663Handler(AimHandler aimHandler) : base(aimHandler) {}

    public override void Init()
    {
        CommonConfig.devicePlan = 4;
        _663Axis = new o0.Bow.o0663Axis(GyrByteIndex, GyrByteIndex, AccByteIndex, AccByteIndex, MagByteIndex);
        _663Axis.Attitude = new o0.IMU._663AxisPreProcessor(GyrByteIndex, GyrByteIndex, AccByteIndex, AccByteIndex, MagByteIndex);
        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.SetNewRotation(Qua);
    }

    public override void DoIdentity()
    {
        SetIdentityAndSave();
        m_aimHandler.SetNewRotation(_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);
    }

    #region 视角归位-恢复和保存
    private void LoadIdentity() 
    {
        try {
            string accStr = PlayerPrefs.GetString("AccIdentity0", "");
            if (accStr.Length > 0) {
                double[] arr = JsonConvert.DeserializeObject<double[]>(accStr);
                o0.Bow.o0663Axis.AccIdentity = new o0.Geometry.Vector<double>(arr);
            }
            string magStr = PlayerPrefs.GetString("MagIdentity0", "");
            if (magStr.Length > 0) {
                double[] arr = JsonConvert.DeserializeObject<double[]>(magStr);
                o0.Bow.o0663Axis.MagIdentity = new o0.Geometry.Vector<double>(arr);
            }
        } catch (System.Exception e) { 
            Debug.LogError(e.Message);
            Debug.LogError(e.StackTrace); 
        }
    }

    private void SetIdentityAndSave()
    {
        _663Axis.SetIdentity();
        //save
        var a = o0.Bow.o0663Axis.AccIdentity;
        var m = o0.Bow.o0663Axis.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
}