smart-bow-infrared/Assets/BowArrow/Scripts/Bluetooth/New/Axis9NopackHandler.cs

using System;
using System.Collections;
using UnityEngine;
using o0._9Axis;
using Newtonsoft.Json;

public class Axis9NopackHandler : AxisBaseHandler
{
    o09Axis _9Axis;
    MagnetometerAutoCalibrater MagCalibrater;
    o0GyrCalibrater GyrCalibrater;

    public Axis9NopackHandler(AimHandler aimHandler) : base(aimHandler) {}

    public override void Init()
    {
        CommonConfig.devicePlan = 3;
        _9Axis = new o09Axis();
        _9Axis.LoadIdentity();
        InitGyr(null);
        InitMag(null);
    }

    public long msOld = 0;
    long TimeGap = default;
    Vector3 Acc = default;
    Vector3 Gyr = default;
    Vector3 Mag = default;
    public override void Update(byte[] bytes)
    {
        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 (CommonConfig.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 (CommonConfig.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 (CommonConfig.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 (CommonConfig.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 (m_aimHandler.gyrCalibrateCompleteCount < m_aimHandler.gyrCalibrateTotalCount) 
            {
                m_aimHandler.gyrCalibrateCompleteCount++;
            }
        }
        
        mag0o = UnityVectorTo0o(Mag);
        try {
            if (!MagCalibrater.Update(mag0o)) return;
        } catch(System.Exception) {
            ResetMag();
            PopupMgr.ins.ShowTipTop("磁场干扰请远离电子设备");
            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);

        m_aimHandler.SetNewRotation(_9Axis.update(Acc, Gyr, Mag, TimeGap));
    }

    public override void DoIdentity()
    {
        _9Axis.SetIdentityAndSave();
        m_aimHandler.SetNewRotation(_9Axis.getLastState().Qua);
    }

    public override void NotifyAxisOnShot()
    {
        _9Axis.axisCSBridge.o09AxisCS.OnShot(100, 100, 100000);
    }

    public override void CalibrateGyr(bool calibration) {
        GyrCalibrater.Calibration = calibration;
    }

    public override void InitGyr(string record) {
        try {
            if (!string.IsNullOrEmpty(record)) {
                var res = JsonConvert.DeserializeObject<o0GyrCalibrater>(record);
                if (res != null) GyrCalibrater = res;
            } else {
                GyrCalibrater = new o0GyrCalibrater();
            }
        } catch(Exception) {}
    }

    public override void InitMag(string record) {
        try {
            if (!string.IsNullOrEmpty(record)) {
                MagCalibrater = JsonConvert.DeserializeObject<MagnetometerAutoCalibrater>(record, new MagJsonConverter());
            } else {
                MagCalibrater = new MagnetometerAutoCalibrater();
            }
        } catch (System.Exception e) {
            Debug.LogError("地磁计反序列化出错");
            Debug.LogError(e.Message);
            Debug.LogError(e.StackTrace);
        }
        m_aimHandler.CorrectMagCompleted(MagCalibrater.Complete);
    }

    public override void ResetGyr() {
        GyrCalibrater._Average = Vector3.zero;
    }

    public override void ResetMag() {
        MagCalibrater = new MagnetometerAutoCalibrater();
    }

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

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

    public override IEnumerator SaveGyr() {
        yield return null;
        string mac = LoginMgr.myUserInfo.mac;
        string record = JsonConvert.SerializeObject(GyrCalibrater);
        UserPlayer.ins.call("userComp.saveMacCalibrate", 0, mac, record);
    }

    public override IEnumerator SaveMag() {
        yield return null;
        string mac = LoginMgr.myUserInfo.mac;
        string record = JsonConvert.SerializeObject(MagCalibrater, new JsonConverter[]{new MagJsonConverter()});
        UserPlayer.ins.call("userComp.saveMacCalibrate", 1, mac, record);
    }

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

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