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

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

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 MagCalibrationButton = default;
    [SerializeField] Button GyrCalibrationButton = default;
    [SerializeField] Text MagScaleText = default;
    [SerializeField] Text GyrScaleText = default;

    AimHandler aimHandler = null;

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

    void Start()
    {
        aimHandler = new AimHandler(controlObj, SetIdentity, MagCalibrationButton, GyrCalibrationButton, MagScaleText, GyrScaleText);
        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;
                // Log(String.Join(",", bytes));
                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;
        }
    }

    public void SetControlObject(Transform obj)
    {
        this.aimHandler.controlObj = obj;
        if (obj != null)
        {
            this.aimHandler.InitAutoIdentity();
        }
    }
}

class AimHandler
{
    public Transform controlObj;
    Button SetIdentity = default;
    Button MagCalibrationButton = default;
    Button GyrCalibrationButton = default;
    Text MagScaleText = default;
    Text GyrScaleText = default;

    long TimeGap = default;
    Vector3 Acc = default;
    Vector3 Gyr = default;
    Vector3 Mag = default;
    o09Axis _9Axis = new o09Axis();
    o0SigmoidIntegrationFilterQuaternion filter = new o0SigmoidIntegrationFilterQuaternion(0.2f);

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

    // o0MagneticCalibraterSimple MagCalibrater;
    o0MagneticCalibraterEllipsoidFitting MagCalibrater;
    o0GyrCalibrater GyrCalibrater;
    long msOld = 0;

    public AimHandler(
        Transform controlObj, 
        Button SetIdentity, 
        Button MagCalibrationButton, 
        Button GyrCalibrationButton, 
        Text MagScaleText,
        Text GyrScaleText
    ) {
        this.controlObj = controlObj;
        this.SetIdentity = SetIdentity;
        this.MagCalibrationButton = MagCalibrationButton;
        this.GyrCalibrationButton = GyrCalibrationButton;
        this.MagScaleText = MagScaleText;
        this.GyrScaleText = GyrScaleText;
        
        if (SetIdentity != null) 
        {
            SetIdentity.onClick.AddListener(DoIdentity);
        }
        
        // MagCalibrater = new o0MagneticCalibraterSimple();
        // string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
        // if (magDataStr.Length > 0)
        // {
        //     string[] dataStrs = magDataStr.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]));
        //     }
        // }
        // if (MagCalibrationButton != null)
        // {
        //     MagCalibrationButton.onClick.AddListener(delegate {

        //         // if (MagCalibrater.Calibration)
        //         if (MagCalibrater.Calibration)
        //         {
        //             Debug.Log(MagCalibrater.Calibration);
        //             MagCalibrater.Calibration = false;
        //             Debug.Log(MagCalibrater.Calibration);
        //             MagCalibrationButton.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
        //         {
        //             Debug.Log(MagCalibrater.Calibration);
        //             MagCalibrater.Calibration = true;

        //             Debug.Log(MagCalibrater.Calibration);
        //             MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
        //         }
        //     });
        // }
        try {
            string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
            MagCalibrater = JsonConvert.DeserializeObject<o0MagneticCalibraterEllipsoidFitting>(magDataStr);
        } catch(Exception) {
            MagCalibrater = null;
        }
        if (MagCalibrater == null) 
        {
            MagCalibrater = new o0MagneticCalibraterEllipsoidFitting();
        }
        if (MagCalibrationButton != null)
        {
            MagCalibrationButton.onClick.AddListener(delegate {
                if (MagCalibrater.Calibration)
                {
                    MagCalibrater.Calibration = false;
                    MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
                    PlayerPrefs.SetString("o0MagneticCalibrater", JsonConvert.SerializeObject(MagCalibrater));
                }
                else
                {
                    MagCalibrater.Calibration = true;
                    MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
                }
            });
        }

        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 != null)
        {
            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 = "停止陀螺仪校准";
                }
            });
        }
    }

    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);

        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 != null && 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)
        {
            Mag = MagCalibrater.Update(Mag);
            if (MagScaleText != null)
            {
                MagScaleText.text = MagCalibrater._Radius.ToString();
            }
        }

        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 = _9Axis.Update(Acc * 10, Gyr, Mag, TimeGap);
        // if(_9Axis.States.Count() < 10){
        //     newRotation = _9Axis.States.Last().Qua;
        // }
        // else
        // {
        //     int MuliteLerpCount = 10;
        //     for (var i = 0;i< MuliteLerpCount;++i)
        //     {
        //         newRotation = Quaternion.Slerp(newRotation, _9Axis.States[_9Axis.States.Count() - MuliteLerpCount + i].Qua, 1/(i+1));
        //     }
        // }


        receiveDataCount++;
        if (!hasAutoIdentity && receiveDataCount == 5) {
            doIdentity = true;
        }
    }   

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

    public void Update()
    {
        if (hasAutoIdentity && controlObj != null)
        {
            // controlObj.localRotation = Quaternion.Lerp(controlObj.localRotation, newRotation, Time.deltaTime * 6);
            // controlObj.localRotation = newRotation;
            Quaternion nowRotation = controlObj.localRotation;
            filter.Update(ref nowRotation, newRotation);
            controlObj.localRotation = nowRotation;
            // GameObject.Find("Canvas/RPY_LOG").GetComponent<Text>().text = 
            //     "roll: " + controlObj.localEulerAngles.x +
            //     "\npitch: " + controlObj.localEulerAngles.y +
            //     "\nyaw: " + controlObj.localEulerAngles.z;
            GameObject.Find("Canvas/RPY_LOG").GetComponent<Text>().text = 
                "x: " + _9Axis.States.Last().Qua.eulerAngles.x +
                "\ny: " + _9Axis.States.Last().Qua.eulerAngles.y +
                "\nz: " + _9Axis.States.Last().Qua.eulerAngles.z;
                
        }
        if (doIdentity)
        {
            _9Axis.SetIdentityAccordingToRecords();
            if (controlObj != null)
            {
                controlObj.localRotation = Quaternion.identity;
            }
            doIdentity = false;
            hasAutoIdentity = true;
        }
    }

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

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