yichael_smartbow/Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

using System;
using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using UnityEngine.UI;
using Newtonsoft.Json;

public class AimHandler : MonoBehaviour
{
    Transform controlObj {
        get {
            if (CameraToLook.ins) {
                return CameraToLook.ins.transform;
            }
            return null;
        }
    }
    [SerializeField] Button SetIdentityButton;
    [SerializeField] Button MagCalibrationButton;
    [SerializeField] Button GyrCalibrationButton;
    [SerializeField] Text MagScaleText;
    [SerializeField] Text GyrScaleText;

    //椭圆对象
    [SerializeField] Ellipse ellipseScript;
    [SerializeField] GameObject AccObj;
    [SerializeField] GameObject MagObj;

    [SerializeField] GameObject AccMesh;
    [SerializeField] GameObject GryMesh;
    [SerializeField] GameObject MagMesh;
    [SerializeField] GameObject AMesh;
    [SerializeField] Transform DebugTexts;
    [SerializeField] Transform DrawImage;
    
    long TimeGap = default;
    Vector3 Acc = default;
    Vector3 Gyr = default;
    Vector3 Mag = default;
    o09Axis _9Axis = new o09Axis();

    Vector3 cMaxVector = new Vector3(0,0,0);
    Vector3 cMinVector = new Vector3(0, 0, 0);

    o0MagneticCalibraterEllipsoidFitting MagCalibrater;
    o0GyrCalibrater GyrCalibrater;

    //陀螺仪校准进度记录
    public int gyrCalibrateCompleteCount = 0;
    public int gyrCalibrateTotalCount = 300;

    long msOld = 0;

    public static AimHandler ins; 

    void Start()
    {
        ins = this;
        BluetoothDispatcher.aim = OnDataReceived;

        //初始化
        _9Axis.LoadIdentity();

        _9Axis.AccMesh = AccMesh;
        _9Axis.GryMesh = GryMesh;
        _9Axis.MagMesh = MagMesh;

        for (var i = 0; i < 9; ++i)
        {
            _9Axis.Tester.Add(DrawImage.Find(i.ToString()).gameObject.AddComponent<o0UIRawImageTester>());
        }

        for (var i = 0; i < 15; ++i)
        {
            _9Axis.TextTester.Add(DebugTexts.transform.Find("Text" + i.ToString()).gameObject.GetComponent<Text>());
        }

        if (SetIdentityButton) 
        {
            SetIdentityButton.onClick.AddListener(DoIdentity);
        }
        
        try 
        {  
            string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
            MagCalibrater = JsonConvert.DeserializeObject<o0MagneticCalibraterEllipsoidFitting>(magDataStr);
        } 
        catch(Exception) 
        {
            MagCalibrater = null;
        }

        if (MagCalibrater == null) 
        {
            MagCalibrater = new o0MagneticCalibraterEllipsoidFitting();
        }
        if (MagCalibrationButton)
        {
            MagCalibrationButton.onClick.AddListener(delegate {
                CalibrateMag(!MagCalibrater.Calibration);
            });
        }

        try {
            string gyrDataStr = PlayerPrefs.GetString("o0GyrCalibrater");
            GyrCalibrater = JsonConvert.DeserializeObject<o0GyrCalibrater>(gyrDataStr);
            if (GyrCalibrater._Average != Vector3.zero) GyrScaleText.text = "已校准";
        } catch(Exception) {
            GyrCalibrater = null;
        }
        if (GyrCalibrater == null) 
        {
            GyrCalibrater = new o0GyrCalibrater();
        }
        if (GyrCalibrationButton)
        {
            GyrCalibrationButton.onClick.AddListener(delegate() {
                CalibrateGyr(!GyrCalibrater.Calibration);
            });
        }
    }

    public void CalibrateGyr(bool calibration) {
        try {
            GyrCalibrater.Calibration = calibration;
            if (calibration)
            {
                GyrCalibrationButton.GetComponentInChildren<Text>().text = "停止陀螺仪校准";
            }
            else
            {
                GyrCalibrationButton.GetComponentInChildren<Text>().text = "开始陀螺仪校准";
                PlayerPrefs.SetString("o0GyrCalibrater", JsonConvert.SerializeObject(GyrCalibrater));            
            }
        } catch (Exception e) { Debug.LogError(e.Message); }
    }

     public void CalibrateMag(bool calibration) {
        try {
            if (calibration)
            {
                MagCalibrater.Calibration = calibration;
                MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
                this.cMaxVector = new Vector3(0, 0, 0);
                this.cMinVector = new Vector3(0, 0, 0);
                this.ellipseScript.ellipseTran.gameObject.SetActive(false);
            }
            else
            {
                List<Vector3> list = MagCalibrater.getRecords();
                //停止校准时候，看看数组值
                float maxDistance = 0f,ratio = 1f;
                Vector3 maxVector3 = new Vector3(0,0,0);
                List<Vector3> endRecords = new List<Vector3>();
                foreach (Vector3 i in list)
                {
                    Vector3 v = i - MagCalibrater._Center;
                    if (Math.Abs(v.magnitude) > maxDistance)
                    {
                        maxVector3 = v;
                        maxDistance = Math.Abs(v.magnitude);
                        if(Math.Abs(v.magnitude) < Math.Abs(MagCalibrater._Radius.magnitude))
                            ratio = Math.Abs(v.magnitude) / Math.Abs(MagCalibrater._Radius.magnitude);
                        else
                            ratio = Math.Abs(MagCalibrater._Radius.magnitude) / Math.Abs(v.magnitude);
                    }
                }
                Debug.LogWarning(maxDistance + " == " + Math.Abs(MagCalibrater._Radius.magnitude) + " == " + MagCalibrater._Radius + " = " + maxVector3);
                //如果比例效果不理想。可以设置为ratio=0.5f 
                foreach (Vector3 i in list)
                {
                    //- MagCalibrater._Center
                    Vector3 v = i ;
                    v *= ratio;
                    if(endRecords.Count>3000)
                    {
                        endRecords.RemoveAt(0);
                    }
                    endRecords.Add(v);
                }
                this.ellipseScript.ClearAndUpdatePointArray();
                this.ellipseScript.DrawPointCloud(endRecords);

                MagCalibrater.Calibration = calibration;

                this.ellipseScript.ellipseTran.gameObject.SetActive(true);
                //绘制椭圆形
                if (MagCalibrater._Radius != this.ellipseScript.ellipseTran.localScale)
                {
                    this.ellipseScript.setEllipseLocalScaleAndCenter(MagCalibrater._Radius, MagCalibrater._Center* ratio);
                    //设置绘制图像相机的对应位置
                    this.ellipseScript.setCameraPos(MagCalibrater._Center * 0.5f);
                }

                MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
                PlayerPrefs.SetString("o0MagneticCalibrater", JsonConvert.SerializeObject(MagCalibrater));
            }
        } catch (Exception e) { Debug.LogError(e.Message); }
    }

    //转换读取的数据，无符号->有符号
    float TwoByteToFloat(byte b1, byte b2) 
    {
        ushort twoByte = (ushort) (b1 * 256 + b2);
        short shortNum = (short) twoByte;
        return (float) shortNum; 
    }

    public void OnDataReceived(byte[] bytes)
    {
        // Debug.Log("瞄准模块数据长度" + bytes.Length);
        if (bytes.Length != 27)
        {
            if (bytes.Length == 2) 
            {
                DeviceBatteryView.ins.RenderBattery(1, bytes[0]);
                return;
            }
            if (bytes.Length > 3 && bytes[3] == 125)
            {
                DoIdentity();
            }
            return;
        }
        if (bytes[7] == 0 && bytes[8] == 0 && bytes[9] == 0 && bytes[10] == 0 && bytes[11] == 0 && bytes[12] == 0)
            return;
        if (bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0 && bytes[22] == 0 && bytes[23] == 0 && bytes[24] == 0)
            return;
        
        float ax = -TwoByteToFloat(bytes[7], bytes[8]);
        float ay = TwoByteToFloat(bytes[9], bytes[10]);
        float az = -TwoByteToFloat(bytes[11], bytes[12]);
        ax = ax / 32768 * 16;
        ay = ay / 32768 * 16;
        az = az / 32768 * 16;
        Acc = new Vector3(ax, ay, az);
        AccObj.transform.GetChild(0).localPosition = Acc;


        float roll = TwoByteToFloat(bytes[13], bytes[14]);
        float pitch = TwoByteToFloat(bytes[15], bytes[16]);
        float yaw = TwoByteToFloat(bytes[17], bytes[18]);
        roll = -roll / 32768 * 2000;
        pitch = pitch / 32768 * 2000;
        yaw = -yaw / 32768 * 2000;
        Gyr = new Vector3(roll, pitch, yaw) / 1000;
        Gyr = GyrCalibrater.Update(Gyr);
        if (GyrCalibrater.Calibration) 
        {
            if (gyrCalibrateCompleteCount < gyrCalibrateTotalCount) {
                gyrCalibrateCompleteCount++;
            }
        }
        if (GyrScaleText && GyrCalibrater.Calibration) 
        {    
            // GyrScaleText.text = GyrCalibrater._Average.x + "\n" + GyrCalibrater._Average.y + "\n" + GyrCalibrater._Average.z;
            // GyrScaleText.text = "Gyr*1000,000:" + (_9Axis.GyrOld * 1000000).ToString();
            GyrScaleText.text = "" + (_9Axis.GyrOld * 1000000).ToString();
        }

        float x = TwoByteToFloat(bytes[19], bytes[20]);
        float y = TwoByteToFloat(bytes[21], bytes[22]);
        float z = -TwoByteToFloat(bytes[23], bytes[24]);
        var mag = new Vector3(x, y, z);
        Mag = mag / 32768 * 256;
    

        if(Mag.x > -128 && Mag.y > -128 && Mag.z > -128 && Mag.x < 128 && Mag.y < 128 && Mag.z < 128)
        {   
            //绘制地磁计点
            if (MagCalibrater.Calibration)
            {
                this.ellipseScript.AddAndUpdatePointArray(Mag);

                if (Mag.magnitude > this.cMaxVector.magnitude)
                {
                    this.cMaxVector = Mag;
                }
                else if (Mag.magnitude < this.cMinVector.magnitude) {
                    this.cMinVector = Mag;
                }

                Vector3 _center = this.cMaxVector - this.cMinVector;
                Debug.LogWarning(_center + " == "+ _center.magnitude);
                //设置绘制图像相机的对应位置
                this.ellipseScript.setCameraPos(_center/2);
            }
            Mag = MagCalibrater.Update(Mag);

            if (MagScaleText)
            {
                MagScaleText.text = MagCalibrater._Radius.ToString();
            }
           
        }
        MagObj.transform.GetChild(0).localPosition = Mag;

        var ms = (((long)bytes[1]) *60 + bytes[2])*1000 + (long)TwoByteToFloat(bytes[3], bytes[4]);
        if(msOld == default)
        {
            msOld = ms;
            return;
        }
        TimeGap = ms - msOld;
        msOld = ms;

        AMesh.transform.localRotation = newRotation = _9Axis.Update(Acc * 10, Gyr, Mag, TimeGap);

        // 记录一些旋转角---start
        if (ArmBow.ins) {
            for (int i = ArmBow.ins.recordRotations.Length - 1; i > 0 ; i--)
            {
                ArmBow.ins.recordRotations[i] = ArmBow.ins.recordRotations[i - 1];
            }
            ArmBow.ins.recordRotations[0] = newRotation;
            ArmBow.ins.recordCount++;
        }
        // 记录一些旋转角---end
    }   

    public bool lerpForRotation = true;
    public void Update()
    {
        if (controlObj)
        {
            if (lerpForRotation)
            {
                controlObj.localRotation = Quaternion.Lerp(controlObj.localRotation, newRotation, Time.deltaTime * 8);   
            } 
            else 
            {
                controlObj.localRotation = newRotation;
            }
        }
    }

    Quaternion newRotation = Quaternion.identity;

    public void DoIdentity()
    {
        _9Axis.SetIdentity();
        if (controlObj) controlObj.localRotation = _9Axis.States.Last().Qua;
    }
}