yichael_smartbow/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, 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);
    }
}