lvjincheng_chengbao/Assets/BLE9Axis/Scripts/Bluetooth/AimHandler.cs

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

/* 瞄准处理器 */
public class AimHandler : MonoBehaviour
{
    private Quaternion _gameRotation = Quaternion.identity;
    private Quaternion gameRotation
    {
        get
        {
            return _gameRotation;
        }
        set
        {
            _gameRotation = value;
            onGameRotationUpdate?.Invoke(Quaternion.LookRotation(_gameRotation * Vector3.forward));
        }
    }
    public Action<Quaternion> onGameRotationUpdate;
    
    long TimeGap = default;
    Vector3 Acc = default;
    Vector3 Gyr = default;
    Vector3 Mag = default;
    public o09Axis _9Axis = new o09Axis();

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

    [o0.Serialize]
    MagnetometerAutoCalibrater MagCalibrater;
    o0GyrCalibrater GyrCalibrater;

    //陀螺仪校准进度记录
    [NonSerialized] public int gyrCalibrateCompleteCount = 0;
    [NonSerialized] public int gyrCalibrateTotalCount = 2000;

    [NonSerialized] public long msOld = 0;

    public static AimHandler ins; 

    void Start()
    {
        ins = this;
        _9Axis.LoadIdentity();
        InitGyr(null);
        InitMag(null);
    }

    public void InitGyr(string record) {
        GyrCalibrater = new o0GyrCalibrater();
        try {
            if (string.IsNullOrEmpty(record))
            {
                record = PlayerPrefs.GetString("gyr-calibrate-record");
            }
            if (!string.IsNullOrEmpty(record)) {
                var res = JsonConvert.DeserializeObject<o0GyrCalibrater>(record);
                if (res != null) GyrCalibrater = res;
            }
        } catch(Exception) {}
    }

    public void InitMag(string record) {
        MagCalibrater = new MagnetometerAutoCalibrater();
        try {
            if (string.IsNullOrEmpty(record))
            {
                record = PlayerPrefs.GetString("mag-calibrate-record");
            }
            if (!string.IsNullOrEmpty(record)) {
                Json.FromJson<MagnetometerAutoCalibrater>(record, ref MagCalibrater);
            }
        } catch (System.Exception) {}
        magComplete = MagCalibrater.Complete;
    }

    public void ResetGyr() {
        GyrCalibrater._Average = Vector3.zero;
        PlayerPrefs.DeleteKey("gyr-calibrate-record");
    }

    public void ResetMag() {
        MagCalibrater = new MagnetometerAutoCalibrater();
        PlayerPrefs.DeleteKey("mag-calibrate-record");
    }

    public bool IsGyrCompleted() {
        return !GyrCalibrater._Average.Equals(Vector3.zero);
    }

    public bool IsMagCompleted() {
        return MagCalibrater.Complete;
    }

    public IEnumerator SaveGyr() {
        yield return null;
        string record = JsonConvert.SerializeObject(GyrCalibrater);
        PlayerPrefs.SetString("gyr-calibrate-record", record);
    }

    public IEnumerator SaveMag() {
        yield return null;
        string record = MagCalibrater.ToJson();
        PlayerPrefs.SetString("mag-calibrate-record", record);
    }

    public void CalibrateGyr(bool calibration) {
        try {
            GyrCalibrater.Calibration = calibration;
        } 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) {
                if (bytes[0] == 0x66 && bytes[1] == 0x31) {
                    //if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked) {
                        //视角回正
                        DoIdentity();
                        //鼠标居中自然会居中
                    //} else {
                    //    AutoResetView.DoIdentity();
                    //}
                } else if (bytes[0] == 0x66 && bytes[1] == 0x32) {
                    //if (SB_EventSystem.ins) {
                    //    //唤起/隐藏虚拟鼠标
                    //    SB_EventSystem.ins.AwakenSimulateMouse(); 
                    //}
                } else if (bytes[1] == 10) {
                    //显示电量
                    //DeviceBatteryView.ins.RenderBattery(1, bytes[0]); 
                }
            } else if (bytes[0] == 0x5b) {
                //红外射击检测
                ShootCheck.ins.ShootByInfrared(bytes); 
            }
            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]);

        float roll = TwoByteToFloat(bytes[13], bytes[14]);
        float pitch = TwoByteToFloat(bytes[15], bytes[16]);
        float yaw = TwoByteToFloat(bytes[17], bytes[18]);

        float x = TwoByteToFloat(bytes[19], bytes[20]);
        float y = TwoByteToFloat(bytes[21], bytes[22]);
        float z = TwoByteToFloat(bytes[23], bytes[24]);

        float mxr = TwoByteToFloat(bytes[20], bytes[19]);
        float myr = TwoByteToFloat(bytes[22], bytes[21]);
        float mzr = TwoByteToFloat(bytes[24], bytes[23]);

        if (BluetoothAim.devicePlan == 3)
        {
            Acc = new Vector3(az, ay, ax) / 32768 * 16;
            Gyr = new Vector3(-yaw, -pitch, -roll) / 32768 * 2;
            Mag = new Vector3(z, y, -x) / 32768 * 256; //最新版
        }
        else if (BluetoothAim.devicePlan == 0)
        {
            Acc = new Vector3(-az, ay, -ax) / 32768 * 16;
            Gyr = new Vector3(yaw, -pitch, roll) / 32768 * 2;
            Mag = new Vector3(-z, y, x) / 32768 * 256; //旧版
        }
        else if (BluetoothAim.devicePlan == 1)
        {
            Acc = new Vector3(ax, ay, az) / 32768 * 16;
            Gyr = new Vector3(roll, pitch, yaw) / 32768 * 2;
            Mag = new Vector3(z, x, -y) / 32768 * 256;//第一个6+3硬件
        }
        else if (BluetoothAim.devicePlan == 2)
        {
            Acc = new Vector3(-az, ax, -ay) / 32768 * 16;
            Gyr = new Vector3(-yaw, roll, -pitch) / 32768 * 2;
            Mag = new Vector3(mzr, mxr, -myr) / 32768 * 256;//第二个6+3硬件
        }

        Gyr = GyrCalibrater.Update(Gyr);

        if (GyrCalibrater.Calibration) 
        {
            if (gyrCalibrateCompleteCount < gyrCalibrateTotalCount) {
                gyrCalibrateCompleteCount++;
            }
        }

        mag0o = UnityVectorTo0o(Mag);
        try {
            if (!MagCalibrater.Update(mag0o)) return;
        } catch(System.Exception) {
            ResetMag();
            PopupTip.Show("磁场干扰请远离电子设备");
            return;
        }
        mag0o = MagCalibrater.EllipsoidFitting.Map(mag0o);
        Mag = o0VectorToUnity(mag0o);

        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;

        GapMs = TimeGap;
        gyr0o = UnityVectorTo0o(Gyr);
        acc0o = UnityVectorTo0o(Acc);
        
        distanceToAxis.Update(gyr0o, acc0o, mag0o, GapMs);
        acc0o = distanceToAxis.AccCorrection(gyr0o, acc0o, GapMs);/**///轴心偏离矫正
        Acc = o0VectorToUnity(acc0o);

        newRotation = _9Axis.update(Acc, Gyr, Mag, TimeGap);
        //if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked) {
        //    SB_EventSystem.ins.MoveSimulateMouse(newRotation);
        //} #undetermined
    }

    o0.Bow.DistanceToAxis distanceToAxis = new o0.Bow.DistanceToAxis();
    double GapMs;
    o0.Geometry.Vector<double> gyr0o;
    o0.Geometry.Vector<double> acc0o;
    o0.Geometry.Vector<double> mag0o;
    o0.Geometry.Vector<double> UnityVectorTo0o(Vector3 src) {
        return new o0.Geometry.Vector<double>(double.Parse(src.x.ToString()), double.Parse(src.y.ToString()), double.Parse(src.z.ToString()));
    }
    Vector3 o0VectorToUnity(o0.Geometry.Vector<double> src) {
        return new Vector3(float.Parse(src.x.ToString()), float.Parse(src.y.ToString()), float.Parse(src.z.ToString()));
    }

    
    public void Update()
    {
        gameRotation = Quaternion.Lerp(gameRotation, newRotation, Time.deltaTime * 7);
        _magCompleteTemp = IsMagCompleted();
        if (!magComplete && _magCompleteTemp) {
            StartCoroutine(SaveMag());
            PopupTip.Show("地磁计校准完成");
        }
        magComplete = _magCompleteTemp;
    }

    bool _magCompleteTemp;
    bool magComplete;
    Quaternion newRotation = Quaternion.identity;

    public void DoIdentity()
    {
        _9Axis.SetIdentityAndSave();
        Quaternion qua = _9Axis.getLastState().Qua;
        newRotation = qua;
        gameRotation = qua;
    }
}