yichael_smartbow/Assets/BowArrow/Scenes/NewBehaviourScript.cs

using ArduinoBluetoothAPI;
using System;
using UnityEngine;
using UnityEngine.UI;

public class NewBehaviourScript : MonoBehaviour
{
    BluetoothHelper bluetoothHelper;
    private bool btnCanClick = true;
    [SerializeField] GameObject controlTarget;
    [SerializeField] Text text;

    long lastTimestampGyr = 0;
    long Timestamp = default;
    long TimeGap = default;
    Vector3 acc = new Vector3();
    Vector3 gyr = new Vector3();
    Vector3 mag = new Vector3();

    o09DOF cal = new o09DOF();

    long ConvertDateTimeToInt(System.DateTime time)
    {
        System.DateTime startTime = TimeZone.CurrentTimeZone.ToLocalTime(new System.DateTime(1970, 1, 1, 0, 0, 0, 0));
        long t = (time.Ticks - startTime.Ticks) / 10000;
        return t;
    }

    void Start() 
    {
        try {
            BluetoothHelper.BLE = false;
            bluetoothHelper = BluetoothHelper.GetNewInstance("HC-06");
            bluetoothHelper.setFixedLengthBasedStream(11);

            bluetoothHelper.OnConnected += (BluetoothHelper helper) =>
            {
                Log("连接成功");
                bluetoothHelper.StartListening();
                Log("开始采集信息\n" + bluetoothHelper.getDeviceName());
            };
            bluetoothHelper.OnConnectionFailed += (BluetoothHelper helper) =>
            {
                btnCanClick = true;
                Log("连接失败");
            };
            bluetoothHelper.OnDataReceived += (BluetoothHelper helper) =>
            {
                byte[] bytes = helper.ReadBytes();
                if (bytes[1] == 81) {
                    if (Timestamp == default)
                    {
                        Timestamp = ConvertDateTimeToInt(DateTime.Now);
                    }
                    else
                    {
                        long temp = Timestamp;
                        Timestamp = ConvertDateTimeToInt(DateTime.Now);
                        TimeGap = Timestamp - temp;//ms
                    }
                    acc.x = TwoByteToFloat(bytes[3], bytes[2]) / 32768f * 16f;
                    acc.y = TwoByteToFloat(bytes[5], bytes[4]) / 32768f * 16f;
                    acc.z = TwoByteToFloat(bytes[7], bytes[6]) / 32768f * 16f;
                    ClearLog();
                    Log("acc " + acc.x.ToString("#0.00") + " " + acc.y.ToString("#0.00") + " " + acc.z.ToString("#0.00"));
                }
                if (bytes[1] == 82) {
                    float timeGap;
                    if (lastTimestampGyr == default)
                    {
                        lastTimestampGyr = ConvertDateTimeToInt(DateTime.Now);
                        return;
                    }
                    else
                    {
                        long temp = lastTimestampGyr;
                        lastTimestampGyr = ConvertDateTimeToInt(DateTime.Now);
                        timeGap = (lastTimestampGyr - temp) / 1000f;
                    }
                    gyr.x = -TwoByteToFloat(bytes[3], bytes[2]) / 32768f * 2f;
                    gyr.y = TwoByteToFloat(bytes[5], bytes[4]) / 32768f * 2f;
                    gyr.z = -TwoByteToFloat(bytes[7], bytes[6]) / 32768f * 2f;
                    Log("gyr " + gyr.x.ToString("#0.00") + " " + gyr.y.ToString("#0.00") + " " + gyr.z.ToString("#0.00"));
                }
                if (bytes[1] == 84) {
                    mag.x = TwoByteToFloat(bytes[3], bytes[2]);
                    mag.y = TwoByteToFloat(bytes[5], bytes[4]);
                    mag.z = TwoByteToFloat(bytes[7], bytes[6]);
                    Log("mag " + mag.x.ToString("#0.00") + " " + mag.y.ToString("#0.00") + " " + mag.z.ToString("#0.00"));

                    Quaternion qua = cal.o06DOFUpdate(acc * 10f, gyr, mag, TimeGap);
                    controlTarget.transform.localRotation = qua;
                }
            };
        } catch (Exception e) {
            Debug.Log(e.Message);
            Log("请打开蓝牙");
        }
        this.Invoke("Connect", 1);
    }

    void OnDestroy() 
    {
        if (bluetoothHelper != null) {
            bluetoothHelper.Disconnect();
        }
    }

    void Update()
    {
        if (Input.GetKeyDown(KeyCode.Space))
        {
            cal.Calibrate();
            Debug.Log("校准");
        }
    }

    public void Connect()
    {
        if (!btnCanClick) {
            return;
        }
        btnCanClick = false;
        try {
            if (bluetoothHelper != null) {
                bluetoothHelper.Connect();
                Log("正在扫描设备");
            } else {
                btnCanClick = true;
                Log("蓝牙初始化失败");
            }
        } catch (Exception e) {
            btnCanClick = true;
            Debug.Log(e.Message);
            Log("请先配对蓝牙");
        }
    }

    float TwoByteToFloat(byte b1, byte b2) 
    {
        ushort twoByte = (ushort) (b1 * 256 + b2);
        short shortNum = (short) twoByte;
        return (float) shortNum; 
    }

    void Log(string text)
    {
        if (this.text == null) return;
        this.text.text += text + "\n";
    }

    void ClearLog() 
    {
        if (this.text == null) return;
        this.text.text = "";
    }
}