/******************** (C) COPYRIGHT 2023 Geek************************************ * File Name : SensorImpl.cpp * Current Version : V1.0 * Author : zhanli * Date of Issued : 2023.09.10 * Comments : GeekIMU 传感器实现模块 ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #include "windows.h" #include "SensorImpl.h" #include "stdio.h" #include "quaternion.h" #include "math.h" #include "UsbHid_CAPI.h" #include "MyCls.h" extern "C" FILE* fpdata; Quaternion GlobalQ; BOOL GlobalQ_Updating; float LocX,LocY,LocZ; float Yaw,Pitch,Roll; extern FILE* Getfpdata(); extern int global_calibration_flag; extern int global_Show; UINT8 global_key[4] = { 0 }; float gxc=0, gyc=0, gzc=0; int gyro_cali_count = 0; #define GYRO_CALIBRATION_SAMPLE_NUMBER 3000 float gx0=0, gy0=0, gz0=0; BOOL GlobalMag_Updating = false; Vector3 GlobalMag = {0}; static bool HmdStandByState; Quaternion CQ_DM = { 1, 0, 0, 0 }; bool DM_Corrected = false; void MagFilter(float *mx,float *my,float *mz,float sita); void GyroComputeDrift(float *gx, float *gy, float *gz); void LowPassFilterYaw(float *z, float alpha); bool IsHmdIntoStandBy(float *Pitch, float *Roll, float dTime); /**---------------------------------------------------------------------- * Function : UnpackSensor * Description : Oculus 数据解包 * Date : 2023/09/10 logzhan : 重新整理代码 *---------------------------------------------------------------------**/ void UnpackSensor(const UCHAR* buffer, long* x, long* y, long* z) { struct {INT32 x:21;} s; *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3); *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) | ((buffer[5] & 0xC0) >> 6); *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1); } Vector3 GetMagData() { Vector3 v = { 0 }; static Vector3 VLast = { 0 }; if (!GlobalMag_Updating) { VLast = GlobalMag; } v = VLast; return v; } void GetControllerSensor(unsigned char usb_input_buffer[]) { static UINT8 updatecnt = 0; INT16 MagX,MagY,MagZ; long GX,GY,GZ; long AX,AY,AZ; UINT NowTime; float ax, ay, az, gx, gy, gz, mx, my, mz, mx1, my1, mz1;; static float mxlast=0,mylast=0,mzlast=0; static int Dtime = 0; static UINT LastTime = 0; float q0,q1,q2,q3; NowTime = *(UINT16* )(usb_input_buffer+2); if(LastTime != 0) { if(NowTime >= LastTime) { Dtime = NowTime - LastTime; LastTime = NowTime; }else { Dtime = (65535 - LastTime) + NowTime; LastTime = NowTime; } }else { Dtime = 1; LastTime = NowTime; } if (global_Show==5) { //clrscr(); back2zero(); printf("Hex数据\n\n"); for (int i = 0; i < 64; i++) { if ((i%16)==0) { printf("\n"); } //if (global_debug) printf("%2x ", usb_input_buffer[i]); } printf("\n"); } MagX = *(INT16 *)(usb_input_buffer + 56); MagY = *(INT16 *)(usb_input_buffer + 58); MagZ = *(INT16 *)(usb_input_buffer + 60); UnpackSensor(usb_input_buffer+8, &AX,&AY,&AZ); UnpackSensor(usb_input_buffer+16, &GX,&GY,&GZ); ax = (float)0.0001*AX; ay = (float)0.0001*AY; az = (float)0.0001*AZ; gx = (float)0.0001*GX; gy = (float)0.0001*GY; gz = (float)0.0001*GZ; printf("%f %f %f\n", ax, ay, az); float x = 16.4 * 180 / 3.14159265358979323846F; if (global_Show == 1) { printf("gx = %f,gy = %f gz =%f\n", gx*x, gy*x, gz*x); } GyroComputeDrift(&gx, &gy, &gz); if (global_calibration_flag == 1) { if (gyro_cali_count > GYRO_CALIBRATION_SAMPLE_NUMBER) { gyro_cali_count = 0; gxc = gyc = gzc = 0; } gyro_cali_count++; gxc += gx*x; gyc += gy*x; gzc += gz*x; if (gyro_cali_count == GYRO_CALIBRATION_SAMPLE_NUMBER) { gx0 = gxc / gyro_cali_count; gy0 = gyc / gyro_cali_count; gz0 = gzc / gyro_cali_count; gyro_cali_count++; global_calibration_flag = 0; printf("%f %f %f\n", gx0, gy0, gz0); } } global_key[0] = usb_input_buffer[24]; global_key[3] = usb_input_buffer[27]; mx = (float)MagX / 16; my = (float)MagY / 16; mz = (float)MagZ / 16; if (global_calibration_flag==3) { GlobalMag_Updating = true; mx = (float)MagX; my = (float)MagY; mz = (float)MagZ; GlobalMag.x = (float)MagX; GlobalMag.y = (float)MagY; GlobalMag.z = (float)MagZ; GlobalMag_Updating = false; } if(mxlast == 0&&mylast == 0&&mzlast == 0) { mxlast = mx; mylast = my; mzlast = mz; MagFilter(&mx, &my, &mz, 0.95f); for (int i = 0; i < 200; i++) AHRSupdate(0, 0, 0, az, ax, ay, mz, mx, my, 1, Dtime * 400); } else { updatecnt++; if (updatecnt > 10) updatecnt = 0; if (mx == mxlast&&my == my&&mz == mzlast && updatecnt < 10) { AHRSupdate(gz,gx,gy,az,ax,ay,mz,mx,my,1,Dtime); } else { mxlast = mx; mylast = my; mzlast = mz; MagFilter(&mx,&my,&mz,0.95f); AHRSupdate(gz,gx,gy,az,ax,ay,mz,mx,my,1,Dtime); } } } /********************************************************************************************************** * Function Name : AHRSupdate * 输入 : 三轴加速度计、陀螺仪、磁力计的值 * 描述 ：加速度计、陀螺仪、磁力计融合得到的欧拉角 1）这个算法基于标准笛卡尔右手坐标系，z轴朝上，x轴朝人，y轴朝右 2）传感器的方向需要变换到和改算法定义的坐标轴相同。 * 修改 ：2016/11/5 : 根据磁力计采样率比较低的特点，设置了2种模式：1、融合磁力计 2、不融合磁力计 2016/11/6 ：改进了四元数更新算法，原来的四元数更新算法会导致累计误差。 2016/11/9 : 规定了本算法运行的坐标轴，重写了四元数到欧拉角变换的代码。 ***********************************************************************************************************/ float q0 = 1, q1 = 0, q2 = 0, q3 = 0; float exInt = 0, eyInt = 0, ezInt = 0; /*缩放积分误差 */ #define Kp 2.5f /*加速度计和磁力计对陀螺仪的比例修正参数 */ #define Ki 0.005f /*加速度计和磁力计对陀螺仪的积分修正参数 */ #define Kd 0.0f static int AHRS_Init = 0; void ResetAHRS() { AHRS_Init = 0; q0 = 1; q1 = 0; q2 = 0; q3 = 0; } void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, char useMag, int Dtime) { /*原算法的定义*/ float norm, halfT; float hx, hy, hz, bz, by; float vx, vy, vz, wx, wy, wz; float ex, ey, ez; float qw, qx, qy, qz; float AccelAjust, MagAjust; /*方便之后的程序使用，减少计算时间*/ float q0q0 = q0*q0; float q0q1 = q0*q1; float q0q2 = q0*q2; float q0q3 = q0*q3; float q1q1 = q1*q1; float q1q2 = q1*q2; float q1q3 = q1*q3; float q2q2 = q2*q2; float q2q3 = q2*q3; float q3q3 = q3*q3; if (AHRS_Init > 200) { halfT = 0.0005f*Dtime; /*得到每次姿态更新的周期的一半t = (1/SampleFrq) * 0.5 */ } else { if (useMag == 1) { halfT = 0.1; gx = gy = gz = 0; AHRS_Init++; } else { return; } } norm = invSqrt(ax*ax + ay*ay + az*az); /*把加速度计向量转换为单位向量*/ if ((1.0f / norm) > 1.6 || (1.0f / norm) < 0.4){ AccelAjust = 0; MagAjust = 1.2f; } else{ AccelAjust = 1.0f; MagAjust = 1.0f; } ax = ax * norm; ay = ay * norm; az = az * norm; /*把磁力计向量转换为单位向量 */ norm = invSqrt(mx*mx + my*my + mz*mz); mx = mx * norm; my = my * norm; mz = mz * norm; hx = 2.0f*(mx*(0.5f - q2q2 - q3q3) + my*(q1q2 - q0q3) + mz*(q1q3 + q0q2)); /*把(mx,my,mz)转换到地理坐标系的(hx,hy,hz),利用H = Q^-1 * M */ hy = 2.0f*(mx*(q1q2 + q0q3) + my*(0.5f - q1q1 - q3q3) + mz*(q2q3 - q0q1)); hz = 2.0f*(mx*(q1q3 - q0q2) + my*(q2q3 + q0q1) + mz*(0.5f - q1q1 - q2q2)); /*计的数值为(bx,by,0),所以我们不关注hx的数值 */ // bx = sqrtf((hx*hx) + (hy*hy)); /*使磁力计正交化*/ by = sqrtf((hx*hx) + (hy*hy)); bz = hz; /*v代表的是把地理坐标系的加速度(0,0,1g)转换到机体坐标系的加 */ vx = 2.0f*(q1q3 - q0q2); /*速度计(ax,ay,az),其实就是用R*(0,0,1),R为旋转矩阵，此矩阵可 */ vy = 2 * (q0q1 + q2q3); /*由四元数转换得到 */ vz = q0q0 - q1q1 - q2q2 + q3q3; wx = 2.0f*(by*(q1q2 + q0q3) + bz*(q1q3 - q0q2)); /*把正交化的H即B从地理坐标系转换到飞行器坐标系，利用W = Q * B */ wy = 2.0f*(by*(0.5f - q1q1 - q3q3) + bz*(q0q1 + q2q3)); /*这里认为bx = 0 */ wz = 2.0f*(by*(q2q3 - q0q1) + bz*(0.5f - q1q1 - q2q2)); /*如果使用磁力计，则融合磁力计*/ if (useMag != 0) { /*用当前姿态向量和加速度的姿态向量做叉乘，乘积越小说明两个向量方向越相同*/ ex = AccelAjust*(ay*vz - az*vy) + MagAjust*(my*wz - mz*wy); ey = AccelAjust*(az*vx - ax*vz) + MagAjust*(mz*wx - mx*wz); ez = AccelAjust*(ax*vy - ay*vx) + MagAjust*(mx*wy - my*wx); } else { ex = AccelAjust*(ay*vz - az*vy); ey = AccelAjust*(az*vx - ax*vz); /*因为加速度计没有校准，ez不靠谱*/ ez = AccelAjust*(ax*vy - ay*vx); ez = 0; } if (ex != 0.0f && ey != 0.0f && ez != 0.0f) { /*这里使用了PID调节的方式，对角速度修正,Ki 是积分修正，Kp是直接修正 */ exInt = exInt + ex*Ki * halfT; /*对误差进行积分 */ eyInt = eyInt + ey*Ki * halfT; ezInt = ezInt + ez*Ki * halfT; gx = gx + Kp*(1 + Kd*fabsf(ex))*ex + exInt; /*使用比例和积分综合对陀螺仪进行修正，由于存在积分修正，所以才能确保回 */ gy = gy + Kp*(1 + Kd*fabsf(ey))*ey + eyInt; /*到期望回到的位置 */ gz = gz + Kp*(1 + Kd*fabsf(ez))*ez + ezInt; } qw = (-q1*gx - q2*gy - q3*gz)*halfT; /*利用一阶龙格库塔法对四元数进行更新*/ qx = (q0*gx + q2*gz - q3*gy)*halfT; qy = (q0*gy - q1*gz + q3*gx)*halfT; qz = (q0*gz + q1*gy - q2*gx)*halfT; q0 = q0 + qw; q1 = q1 + qx; q2 = q2 + qy; q3 = q3 + qz; norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); /*对四元数进行规范化，即四元数模为1*/ q0 = q0 * norm; //w q1 = q1 * norm; //x q2 = q2 * norm; //y q3 = q3 * norm; //z /*对原变换公式进行重写，避免了以前坐标轴混乱的问题，按照标准y上z右x前*/ Pitch = asin(-2.0f*(q2*q1 - q0*q3))* (180.0f / 3.141592f); /*轴进行四元数到欧拉角的转换 */ Yaw = atan2(q3*q1 + q0*q2, 0.5f - q3*q3 - q2*q2)* (180.0f / 3.141592f); Roll = atan2(q3*q2 + q0*q1, 0.5f - q3*q3 - q1*q1)* (180.0f / 3.141592f); LowPassFilterYaw(&Yaw, 0.9f); if (AHRS_Init > 200) { GlobalQ_Updating = TRUE; GlobalQ.w = q0; GlobalQ.x = q2; GlobalQ.y = q3; GlobalQ.z = q1; GlobalQ_Updating = FALSE; } printf("%f %f %f\n", Yaw, Roll, Pitch); IsHmdIntoStandBy(&Pitch, &Roll, 1.0f); } void MagFilter(float *mx, float *my, float *mz, float sita) { static float xlast = 0, ylast = 0, zlast = 0; if (xlast == 0 && ylast == 0 && zlast == 0) { xlast = *mx; ylast = *my; zlast = *mz; } else { xlast = sita*xlast + (1 - sita)*(*mx); ylast = sita*ylast + (1 - sita)*(*my); zlast = sita*zlast + (1 - sita)*(*mz); } } /******************************************************************************* * Function Name : invSqrt * Description : 快速计算 1/Sqrt(x)，源自雷神3，神奇的0x5f3759df * 输入 : x * 输出 ：1/sqrt(x) *******************************************************************************/ float invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df - (i >> 1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); return y; } void GyroComputeDrift(float *gx, float *gy, float *gz) { static float GxOffect = 0.0f, GyOffect = 0.0f, GzOffect = 0.0f; static float GxOffectTmp = 0.0f, GyOffectTmp = 0.0f, GzOffectTmp = 0.0f; static float GxTmp = 0.0f, GyTmp = 0.0f, GzTmp = 0.0f; static int count = 0; *gx = *gx - GxOffect; *gy = *gy - GyOffect; *gz = *gz - GzOffect; if (HmdStandByState == true){ GxTmp = GxTmp + *gx; GyTmp = GyTmp + *gy; GzTmp = GzTmp + *gz; count++; if (count == 3000){ //printf("陀螺仪校准完成\n"); GxOffectTmp = GxTmp / count; GyOffectTmp = GyTmp / count; GzOffectTmp = GzTmp / count; GxOffect = GxOffect + GxOffectTmp; GyOffect = GyOffect + GyOffectTmp; GzOffect = GzOffect + GzOffectTmp; GxTmp = 0.0f; GyTmp = 0.0f; GzTmp = 0.0f; count = 0; } } else{ count = 0; GxTmp = 0.0f; GyTmp = 0.0f; GzTmp = 0.0f; } } void LowPassFilterYaw(float *z, float alpha) { static float lastz = 0; if (lastz == 0) { lastz = *z; } else { *z = alpha*lastz + (1 - alpha)*(*z); lastz = *z; } } bool IsHmdIntoStandBy(float *Pitch, float *Roll, float dTime) { static float LastPitch = 0.0f, LastRoll = 0.0f; static float LastGyroX = 0.0f, LastGyroY = 0.0f; static int count = 0; float GyroX = 0.0f, GyroY = 0.0f, alpha = 0.98f; /* 初始化静态变量 */ if (LastPitch == 0.0f&&LastRoll == 0.0f){ LastPitch = *Pitch; LastRoll = *Roll; } /*对Pitch和Roll微分，求Gx和Gy */ GyroX = fabsf(*Roll - LastRoll) / dTime; GyroY = fabsf(*Pitch - LastPitch) / dTime; /*初始化静态变量*/ if (LastGyroX == 0.0f&&LastGyroY == 0.0f){ LastGyroX = GyroX; LastGyroY = GyroY; } /*进行一个滑动窗口滤波降低偶然误差*/ GyroX = alpha*LastGyroX + (1 - alpha)*GyroX; GyroY = alpha*LastGyroY + (1 - alpha)*GyroY; printf("%f %f\n", GyroX, GyroY); if (GyroX < 0.0055f&&GyroY < 0.0015f){ count++; if (count == 3000){ count = 0; HmdStandByState = true; printf("头显进入休眠\n"); } } else{ count = 0; HmdStandByState = false; } /* 更新静态变量 */ LastRoll = *Roll; LastPitch = *Pitch; LastGyroX = GyroX; LastGyroY = GyroY; return HmdStandByState; } float DM_GetYaw(void) { return Yaw; } float DM_GetPitch(void) { return Pitch; } float DM_GetRoll(void) { return Roll; } Quaternion CorrectQuaterinonYaw(Quaternion q) { Quaternion qcorr; qcorr.w = q.w; qcorr.x = 0.0f; qcorr.y = q.y; qcorr.z = 0.0f; qcorr = QuaternionInversion(qcorr); float mod = QuaternionMod(qcorr); qcorr.w = qcorr.w / mod; qcorr.x = qcorr.x / mod; qcorr.y = qcorr.y / mod; qcorr.z = qcorr.z / mod; return qcorr; } int ResetHeading(void) { Quaternion q = { GlobalQ.w, 0, GlobalQ.y, 0 }; CQ_DM = CorrectQuaterinonYaw(q); DM_Corrected = true; return DM_Corrected ? 1 : 0; } Quaternion GetPoseQuat(void) { static Quaternion QLast = { 1.0f, 0.0f, 0.0f, 0.0f }; if (!GlobalQ_Updating) { QLast.w = GlobalQ.w; QLast.x = GlobalQ.x; QLast.y = GlobalQ.y; QLast.z = GlobalQ.z; } if (DM_Corrected){ return QuaternionMulti(QLast, CQ_DM); } return QLast; }