smart-bow-sdk-dll/SmartBowSDK/o06DOF.cs

using ArduinoBluetoothAPI;
using System;
using UnityEngine;


public class o06DOF
{

    /////////////////////g       degree/ms         
    //void Update(Vector3 Acc, Vector3 Gyr, Vector3 Mag, long TimeGap)
    //{
    //    if (TimeGap <= 0)
    //        return;
    //}

    public class SixAxisFusion
    {
        Vector3 acceleratorOffset;
        Vector3 gyroscopeOffset;
        float gForce;
        float gyroscopeToDegree;
        //float acceleratorRateAsOffset;

        float overallGVectorCredibility = 0;
        Vector3 previousAccelerator;
        public Vector3 acceleratorGlobal;
        public Quaternion quaternion = new Quaternion(0, 0, 0, 1);
        public Vector3 accelerationNG//with No G force
        {
            get
            {
                var newQuat = new Quaternion(quaternion.x, quaternion.y, quaternion.z, -quaternion.w);
                return previousAccelerator - newQuat * new Vector3(0, -gForce, 0);
            }
        }
        public Vector3 speed = Vector3.zero;
        public Vector3 locus = Vector3.zero;
        public Vector3 rotation = Vector3.zero;
        public SixAxisFusion(Vector3 acceleratorOffset, Vector3 gyroscopeOffset, float gForce, float gyroscopeToDegree)
        {
            this.acceleratorOffset = acceleratorOffset;// 反向校正
            this.gyroscopeOffset = gyroscopeOffset;// 反向校正
            this.gForce = gForce;
            this.gyroscopeToDegree = gyroscopeToDegree;
            //this.acceleratorRateAsOffset = acceleratorRateAsOffset;

            previousAccelerator = new Vector3(0, -gForce, 0);
            acceleratorGlobal = new Vector3(0, -gForce, 0);
        }
        Quaternion update(Vector3 accelerator, Vector3 gyroscope)
        {
            accelerator += acceleratorOffset;
            gyroscope += gyroscopeOffset;
            gyroscope *= gyroscopeToDegree;

            var newQua = new Quaternion();
            newQua.eulerAngles = gyroscope;
            // 根据陀螺仪计算新的四元数旋转
            //var newQua = Quaternion.Euler(gyroscope); // 使用 Euler 角度来生成四元数
            quaternion = quaternion * newQua;//一定要这样乘

            float gForceCredibility = 1 / (1 + Mathf.Pow((Mathf.Abs(Mathf.Sqrt(accelerator.sqrMagnitude) - gForce) / gForce * 16), 1.0f));
            float gDegreeCredibility = 1 / (1 + Mathf.Pow(Vector3.Angle(accelerator, newQua * previousAccelerator), 1.0f));
            overallGVectorCredibility = overallGVectorCredibility * 0.3f + gForceCredibility * gDegreeCredibility * 0.7f;
            //Debug.Log(Mathf.Pow(overallGVectorCredibility, 0.1f));

            float rotationUnCredibility = Mathf.Pow(1 - 1 / (1 + gyroscope.magnitude), 5);
            //Debug.Log(rotationUnCredibility);

            acceleratorGlobal = quaternion * accelerator;
            Vector3 downVector = new Vector3(0, -1, 0);
            float angle = Vector3.Angle(acceleratorGlobal, downVector);
            //Vector3 gVector = Vector3.RotateTowards(acceleratorGlobal, downVector, angle / 180 * Mathf.PI * Mathf.Pow(overallGVectorCredibility, 1f) * rotationUnCredibility, 1);//偏移量,从陀螺仪数据往加速计数据的偏移量
            Vector3 gVector = Vector3.RotateTowards(acceleratorGlobal, downVector, angle / 180 * Mathf.PI * Mathf.Pow(overallGVectorCredibility, 1f) * 0.1f, 1);//偏移量,从陀螺仪数据往加速计数据的偏移量
                                                                                                                                                                //Vector3 gVector = Vector3.RotateTowards(acceleratorGlobal, downVector, angle / 180 * Mathf.PI * 1, 1);//偏移量,从陀螺仪数据往加速计数据的偏移量

            newQua = new Quaternion();
            newQua.SetFromToRotation(quaternion * accelerator, gVector);
            // 使用 SetFromToRotation 进行四元数校正
           // newQua = Quaternion.FromToRotation(quaternion * accelerator, gVector);
            quaternion = newQua * quaternion;//一定要反过来乘

            previousAccelerator = accelerator;
            rotation = gyroscope;
            speed = speed * 0.95f + accelerationNG;
            locus = locus * 0.95f + speed;
            return quaternion;
        }

        public void ResetToInitialRotation()
        {
            quaternion = Quaternion.identity;
            speed = Vector3.zero;
            locus = Vector3.zero;
            rotation = Vector3.zero;

            previousAccelerator = new Vector3(0, -gForce, 0);
            acceleratorGlobal = new Vector3(0, -gForce, 0);

            Debug.Log("重置到初始状态");
        }
        /// <summary>
        /// 转换成四元数
        /// </summary>
        /// <param name="accelerator"></param>
        /// <param name="gyroscope"></param>
        /// <returns></returns>
        public Quaternion tranUpdateData(SixData sixData) {
            return update(sixData.Acc, sixData.Gryo);
        }


        protected double LastMS;
        /// <summary>
        /// 解析处理一个6轴数据
        /// </summary>
        /// <param name="bytes"></param>
        /// <returns></returns>
        public SixData getSixAxisByBytes(byte[] bytes)
        {
            // 获取时间信息
            int min = bytes[16];
            int sec = bytes[17];
            float millis = SNum(bytes[1], bytes[2]);
            var CurrentMS = (((double)min) * 60 + sec) * 1000 + millis;
            if (LastMS == default)
            {
                LastMS = CurrentMS;
            }
            // 计算时间差，以秒为单位
            var GapMS = CurrentMS - LastMS;
            LastMS = CurrentMS;
            double deltaTime = GapMS / 1000.0;  // 转换为秒
            string handle = bytes[3].ToString();
            float ax = (SNum(bytes[4], bytes[5]) / 32768 * 16);
            float ay = (SNum(bytes[6], bytes[7]) / 32768 * 16);
            float az = (SNum(bytes[8], bytes[9]) / 32768 * 16);
            float gx = (SNum(bytes[10], bytes[11]) / 32768 * 2000); // 度/秒
            float gy = (SNum(bytes[12], bytes[13]) / 32768 * 2000); // 度/秒
            float gz = (SNum(bytes[14], bytes[15]) / 32768 * 2000); // 度/秒
            string[] parseDataAcc = { ax.ToString("#0.000"), ay.ToString("#0.000"), az.ToString("#0.000") };
            string[] parseDataGro = { gx.ToString("#0.000"), gy.ToString("#0.000"), gz.ToString("#0.000") };
            string[] parseDataTime = { min.ToString(), sec.ToString(), millis.ToString() };

            // 计算每帧旋转的角度（角速度 * 时间）
            float rotationX = gx * (float)deltaTime;  // X轴转动的度数
            float rotationY = gy * (float)deltaTime;  // Y轴转动的度数
            float rotationZ = gz * (float)deltaTime;  // Z轴转动的度数
                                                      //Quaternion quaternion = update(new Vector3(ax, ay, az), new Vector3(rotationX, rotationY, rotationZ));
                                                      //+ ":Quat:" + quaternion 
                                                      // Debug.Log("数据：Acc[" + string.Join(", ", parseDataAcc) + "] Gryo:" + string.Join(", ", parseDataGro) + "]. Time:" + string.Join(", ", parseDataTime) + ",Euler:" + quaternion.eulerAngles);
            return new SixData(new Vector3(ax, ay, az), new Vector3(rotationX, rotationY, rotationZ), min, sec, millis);
        }
        float SNum(byte b1, byte b2)
        {
            ushort twoByte = (ushort)(b1 * 256 + b2);
            short shortNum = (short)twoByte;
            return (float)shortNum;
        }
    }

   
    /// <summary>
    /// 解析转换的6轴格式
    /// </summary>
    public class SixData
    {
        public Vector3 Acc;
        public Vector3 Gryo;
        public int Min;
        public int Sec;
        public float Millis;

        public SixData(Vector3 acc, Vector3 gyro, int min, int sec, float millis)
        {
            Acc = acc;
            Gryo = gyro;
            Min = min;
            Sec = sec;
            Millis = millis;
        }
    }
}