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 {
            CameraToLook ctl = CameraToLook.ins;
            if (ctl && ctl.isNew) 
            {
                ctl.isNew = false;
                InitAutoIdentity();
            }
            if (ctl) return ctl.transform;
            else return null;      
        }
    }
    [SerializeField] Button SetIdentityButton;
    [SerializeField] Button MagCalibrationButton;
    [SerializeField] Button GyrCalibrationButton;
    [SerializeField] Text MagScaleText;
    [SerializeField] Text GyrScaleText;

    //椭圆对象
    [SerializeField] Ellipse ellipseScript;
    [SerializeField] Toggle ellipseToggle;
    [SerializeField] GameObject AccObj;
    [SerializeField] GameObject MagObj;
    [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;
    long msOld = 0;

    public static AimHandler ins; 

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

        //Init
        ellipseToggle.onValueChanged.AddListener(delegate(bool value) {
             Debug.Log(value);
        });

        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 {
                if (MagCalibrater.Calibration)
                {
                    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);

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

                    MagCalibrater.Calibration = false;
                    MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
                    PlayerPrefs.SetString("o0MagneticCalibrater", JsonConvert.SerializeObject(MagCalibrater));

                }
                else
                {
                    MagCalibrater.Calibration = true;
                    MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";

                    this.cMaxVector = new Vector3(0, 0, 0);
                    this.cMinVector = new Vector3(0, 0, 0);
                }
            });
        }

        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 {
                if (GyrCalibrater.Calibration)
                {
                    GyrCalibrater.Calibration = false;
                    GyrCalibrationButton.GetComponentInChildren<Text>().text = "开始陀螺仪校准";
                    PlayerPrefs.SetString("o0GyrCalibrater", JsonConvert.SerializeObject(GyrCalibrater));

                }
                else
                {
                    GyrCalibrater.Calibration = true;
                    GyrCalibrationButton.GetComponentInChildren<Text>().text = "停止陀螺仪校准";
                }
            });
        }
    }

    void OnDestroy()
    {
        ins = null;
        BluetoothDispatcher.aim = null;
    }

    //转换读取的数据，无符号->有符号
    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 != 26)
        {
            if (bytes[3] == 125)
            {
                DoIdentity();
            }
            return;
        }
        if (bytes[4] == 0 && bytes[5] == 0 && bytes[6] == 0 && bytes[7] == 0 && bytes[8] == 0 && bytes[9] == 0)
            return;
        if (bytes[16] == 0 && bytes[17] == 0 && bytes[18] == 0 && bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0)
            return;
        
        float ax = -TwoByteToFloat(bytes[4], bytes[5]);
        float ay = TwoByteToFloat(bytes[6], bytes[7]);
        float az = -TwoByteToFloat(bytes[8], bytes[9]);
        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[10], bytes[11]);
        float pitch = TwoByteToFloat(bytes[12], bytes[13]);
        float yaw = TwoByteToFloat(bytes[14], bytes[15]);
        roll = -roll / 32768 * 2000;
        pitch = pitch / 32768 * 2000;
        yaw = -yaw / 32768 * 2000;
        Gyr = new Vector3(roll, pitch, yaw) / 1000;
        Gyr = GyrCalibrater.Update(Gyr);
        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[16], bytes[17]);
        float y = TwoByteToFloat(bytes[18], bytes[19]);
        float z = -TwoByteToFloat(bytes[20], bytes[21]);
        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[22]) *60 + bytes[23])*1000 + (long)TwoByteToFloat(bytes[1], bytes[2]);
        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

        checkAutoIdentity();
    }   

    public void Update()
    {
        if (controlObj && hasAutoIdentity)
        {
            // 最终结果进行lerp
            controlObj.localRotation = Quaternion.Lerp(controlObj.localRotation, newRotation, Time.deltaTime * 8);
            // controlObj.localRotation = newRotation;          
        }
    }

    int receiveDataCount = 0;
    bool hasAutoIdentity = false;
    Quaternion newRotation;

    void InitAutoIdentity() 
    {
        receiveDataCount = 0;
        hasAutoIdentity = false;
    }

    void checkAutoIdentity() 
    {
        receiveDataCount++;
        if (!hasAutoIdentity && receiveDataCount == 5) {
            hasAutoIdentity = true;
            DoIdentity();
        }
    }

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