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

using ArduinoBluetoothAPI;
using System;
using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using UnityEngine.UI;

public class BluetoothAim : MonoBehaviour
{
    BluetoothHelper bluetoothHelper;
    BluetoothHelperCharacteristic characteristicWrite;
    BluetoothHelperService bluetoothService;
    string deviceName = "";
    bool canConnect = true;

    [SerializeField] string targetDeviceName = "Bbow_20210501";
    [SerializeField] Text textUI;
    [SerializeField] Transform controlObj = default;
    [SerializeField] Button SetIdentity = default;
    [SerializeField] Button CalibrationButton = default;
    [SerializeField] Text MagScaleText = default;

    AimHandler aimHandler = null;

    public static bool scanLock = false; //防止同时扫描冲突

    void Start()
    {
        aimHandler = new AimHandler(controlObj, SetIdentity, CalibrationButton, MagScaleText);
        BluetoothDispatcher.aim = aimHandler.OnDataReceived;
    }

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

    void FixedUpdate()
    {
        Connect();
    }

    void Update()
    {
        aimHandler.Update();   
    }

    void Connect()
    {
        if (BluetoothShoot.scanLock)
        {
            return;
        }
        if (!canConnect)
        {
            return;
        }
        scanLock = true;
        canConnect = false;
        try
        {
            BluetoothHelper.BLE = true;
            bluetoothHelper = BluetoothHelper.GetNewInstance();

            bluetoothHelper.OnConnected += (BluetoothHelper helper) =>
            {
                Log("连接成功\n" + helper.getDeviceName());
                foreach (BluetoothHelperService service in helper.getGattServices())
                {
                
                    if (service.getName().ToLower().StartsWith("0000fff0"))
                    {
                        bluetoothService = service;

                        foreach (BluetoothHelperCharacteristic characteristic in service.getCharacteristics())
                        {
                            if (characteristic.getName().ToLower().StartsWith("0000fff2"))
                            {
                                characteristicWrite = characteristic;
                            }
                            else if (characteristic.getName().ToLower().StartsWith("0000fff1"))
                            {

                                BluetoothHelperCharacteristic ch = new BluetoothHelperCharacteristic(characteristic.getName());
                                ch.setService(bluetoothService.getName());
                                bluetoothHelper.Subscribe(ch);
                            }
                        }
                    }
                }
                Invoke("OpenReceiveData", 1);
            };
            bluetoothHelper.OnConnectionFailed += (BluetoothHelper helper) =>
            {
                canConnect = true;
                Log("连接失败\n" + helper.getDeviceName());
            };
            bluetoothHelper.OnCharacteristicChanged += (helper, value, characteristic) =>
            {
                byte[] bytes = value;
                BluetoothClient.UploadData(0, bytes);
                aimHandler.OnDataReceived(bytes);
            };
            bluetoothHelper.OnScanEnded += (BluetoothHelper helper, LinkedList<BluetoothDevice> nearbyDevices) =>
            {
                scanLock = false;
                foreach (BluetoothDevice device in nearbyDevices)
                {
                    if (device.DeviceName == targetDeviceName)
                    {
                        deviceName = device.DeviceName;
                        bluetoothHelper.setDeviceName(deviceName);
                        bluetoothHelper.Connect();
                        Log("发现设备\n" + device.DeviceName);
                        return;
                    }
                }
                canConnect = true;
                Log("没有发现设备");
            };

            bluetoothHelper.ScanNearbyDevices();
            Log("正在扫描设备");
        }
        catch (Exception e)
        {
            Debug.Log(e.Message);
            canConnect = true;
            Log("请打开蓝牙");
        }
    }

    void OpenReceiveData()
    {
        BluetoothHelperCharacteristic ch = new BluetoothHelperCharacteristic(characteristicWrite.getName());
        ch.setService(bluetoothService.getName());
        bluetoothHelper.WriteCharacteristic(ch, "3");
        Log("瞄准模块准备完成\n" + deviceName);
    }

    void Log(string text)
    {
        if (textUI != null) 
        {
            textUI.text = text;
        }
    }
}

class AimHandler
{
    Transform controlObj;
    Button SetIdentity = default;
    Button CalibrationButton = default;
    Text MagScaleText = default;

    Vector3 Acc = default;
    Vector3 Gyr = default;
    Vector3 Mag = default;
    long TimeGap = default;

    static Vector3 AccIdentity = new Vector3(0, -1, 0);
    static Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
    public class State
    {
        public long TimeGap;
        public Vector3 Acc = AccIdentity;
        public Vector3 Gyr;
        public Vector3 Mag = MagIdentity;
        public Quaternion Qua;
        public float Variance = 1;
    }
    List<State> States = new List<State>();

    Vector3 AccOld;
    Vector3 GyrOld;
    Vector3 MagOld;
    long TimeGapOld;

    Quaternion o06DOFUpdate(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
    {
        var Acc = this.AccOld;
        var Gyr = this.GyrOld;
        var Mag = this.MagOld;
        float TimeGap = (TimeGapOld + this.TimeGapOld) / 2;
        this.AccOld = AccOld;
        this.GyrOld = GyrOld;
        this.MagOld = MagOld;
        this.TimeGapOld = TimeGapOld;
        
        var Last = States.LastOrDefault() ?? new State();
        States.Add(new State());
        if (States.Count > 10)
            States.RemoveAt(0);
        var state = States.Last();
        state.Acc = Acc;
        state.Gyr = Gyr;
        state.Mag = Mag;

        var LastQuaternion = Last.Qua;

        var newQua = new Quaternion();
        newQua.eulerAngles = Gyr * TimeGap;
        var quaGyr = LastQuaternion * newQua;

        float AccLengthToAngle = 90;//1倍引力差相当于多少度方差
        float MagLengthToAngle = 90;//1倍磁力差相当于多少度方差

        float GyrVariance = state.Variance + TimeGap/100 + (Gyr * TimeGap).magnitude * 0.02f;
        float AccVariance = TimeGap / 5 + Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2)+ Mathf.Pow(Vector3.Angle(Acc,Last.Acc) * 0.5f, 2));
        float MagVariance = TimeGap / 5 + Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.1f, 2));
        state.Variance = state.Variance * AccVariance / (state.Variance + AccVariance);
        state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);

        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, AccVariance / (AccVariance + MagVariance));
        var quaMinRate = GyrVariance / (GyrVariance + Mathf.Max(AccVariance, MagVariance));
        var quaMaxRate = GyrVariance / (GyrVariance + Mathf.Min(AccVariance, MagVariance));

        Quaternion quaFirst = Quaternion.Slerp(quaGyr, quaAccMag, quaMinRate).normalized;

        float quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
        state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, quaSecondRate): o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, quaSecondRate);

        return state.Qua;
    }

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

    o0MagneticCalibrater MagCalibrater;
    o0Vector3Filter MagFilter = new o0Vector3Filter();
    long msOld = 0;
    public AimHandler(Transform controlObj, Button SetIdentity, Button CalibrationButton, Text MagScaleText)
    {
        this.controlObj = controlObj;
        this.SetIdentity = SetIdentity;
        this.CalibrationButton = CalibrationButton;
        this.MagScaleText = MagScaleText;
        
        SetIdentity.onClick.AddListener(DoIdentity);
        
        MagCalibrater = new o0MagneticCalibrater();
        string caliraterDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
        if (caliraterDataStr.Length > 0)
        {
            string[] dataStrs = caliraterDataStr.Split(',');
            if (dataStrs.Length == 6)
            {
                MagCalibrater._Center =  new Vector3(float.Parse(dataStrs[0]), float.Parse(dataStrs[1]), float.Parse(dataStrs[2]));
                MagCalibrater._Radius =  new Vector3(float.Parse(dataStrs[3]), float.Parse(dataStrs[4]), float.Parse(dataStrs[5]));
            }
        }
        CalibrationButton.onClick.AddListener(delegate {
            if (MagCalibrater.Calibration)
            {
                MagCalibrater.Calibration = false;
                CalibrationButton.GetComponentInChildren<Text>().text = "开始磁场校准";
                float[] dataFloats = new float[6];
                dataFloats[0] = MagCalibrater.Center.x;
                dataFloats[1] = MagCalibrater.Center.y;
                dataFloats[2] = MagCalibrater.Center.z;
                dataFloats[3] = MagCalibrater.Radius.x;
                dataFloats[4] = MagCalibrater.Radius.y;
                dataFloats[5] = MagCalibrater.Radius.z;
                string dataStr = String.Join(",", dataFloats);
                PlayerPrefs.SetString("o0MagneticCalibrater", dataStr);
            }
            else
            {
                MagCalibrater.Calibration = true;
                CalibrationButton.GetComponentInChildren<Text>().text = "停止磁场校准";
            }
        });
    }

    public void OnDataReceived(byte[] bytes)
    {
        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);

        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;
        var gyr = new Vector3(roll, pitch, yaw) / 1000 * 20;// timeGap;

        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)
        {
            Mag = MagCalibrater.Update(Mag);
            MagScaleText.text = MagCalibrater._Radius.ToString() + MagCalibrater.CalibratCompletionPercentage() + "%";
        }

        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;

        newRotation = o06DOFUpdate(Acc * 10, Gyr, Mag, TimeGap);
        
        receiveDataCount++;
        if (!hasAutoIdentity && receiveDataCount == 5) {
            doIdentity = true;
        }
    }   

    void DoIdentity()
    {
        if (hasAutoIdentity)
        {
            doIdentity = true;
            Debug.Log("reset identity");
        }
    }

    public void Update()
    {
        if (hasAutoIdentity)
        {
            controlObj.transform.localRotation = Quaternion.Lerp(controlObj.transform.localRotation, newRotation, Time.deltaTime * 6);
        }
        if (doIdentity)
        {
            if (Quaternion.Angle(newRotation, baseRotation) < 2) 
            {
                if (!hasAutoIdentity)
                {
                    controlObj.transform.localRotation = newRotation;
                }
                doIdentity = false;
                hasAutoIdentity = true;
            } else {
                AccIdentity = AccOld;
                MagIdentity = MagOld;
            }
        }
    }

    int receiveDataCount = 0;
    bool doIdentity = false;
    bool hasAutoIdentity = false;
    Quaternion newRotation;
    Quaternion baseRotation = new Quaternion(0, 0, 0, 1);
}