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("重置到初始状态");
}
///
/// 转换成四元数
///
///
///
///
public Quaternion tranUpdateData(SixData sixData) {
return update(sixData.Acc, sixData.Gryo);
}
protected double LastMS;
///
/// 解析处理一个6轴数据
///
///
///
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;
}
}
///
/// 解析转换的6轴格式
///
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;
}
}
}