logzhan
/
GeekIMU
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
GeekIMU/4.Software/GeekIMU Manager GUI 1.2/GeekIMUDriver 1.2/Src/SensorImpl.cpp

589 lines
15 KiB
C++
Raw Normal View History

修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
/******************** (C) COPYRIGHT 2023 Geek************************************
* File Name : SensorImpl.cpp
* Current Version : V1.0
* Author : zhanli
* Date of Issued : 2023.09.10
* Comments : GeekIMU <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD>
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
#include "windows.h"
#include "SensorImpl.h"
#include "stdio.h"
#include "quaternion.h"
#include "math.h"
#include "UsbHid_CAPI.h"
#include "MyCls.h"
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
#include "log.h"
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
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);
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
/**----------------------------------------------------------------------
* Function : UnpackSensor
* Description : Oculus <EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><EFBFBD><EFBFBD>
* Date : 2023/09/10 logzhan : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*---------------------------------------------------------------------**/
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
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);
}
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;
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
float ax, ay, az, gx, gy, gz, mx, my, mz;
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
static float mxlast=0,mylast=0,mzlast=0;
static int Dtime = 0;
static UINT LastTime = 0;
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<EFBFBD><EFBFBD><EFBFBD><EFBFBD>\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;
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
float x = 16.4f * 180.f / 3.1415926535f;
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
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;
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
//WriteLog(Info, "Get Magnet Data: %.4f %.4f %.4f\n", GlobalMag.x, GlobalMag.y, GlobalMag.z);
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
}
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
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD> : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼơ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD>ֵ
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼơ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ںϵõ<EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD>ڱ<EFBFBD>׼<EFBFBD>ѿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>z<EFBFBD>ᳯ<EFBFBD>ϣ<EFBFBD>x<EFBFBD>ᳯ<EFBFBD>ˣ<EFBFBD>y<EFBFBD>ᳯ<EFBFBD><EFBFBD>
2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD>任<EFBFBD><EFBFBD><EFBFBD>͸<EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><EFBFBD>
* <EFBFBD>޸<EFBFBD> <EFBFBD><EFBFBD>2016/11/5 : <EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱȽϵ͵<EFBFBD><EFBFBD>ص㣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD>ںϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ںϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2016/11/6 <EFBFBD><EFBFBD><EFBFBD>Ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD>ᵼ<EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2016/11/9 : <EFBFBD>涨<EFBFBD>˱<EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᣬ<EFBFBD><EFBFBD>д<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><EFBFBD><EFBFBD>DZ任<EFBFBD>Ĵ<EFBFBD><EFBFBD>롣
***********************************************************************************************************/
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0; /*<2A><><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
#define Kp 2.5f /*<2A><><EFBFBD>ٶȼƺʹ<C6BA><CDB4><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵı<C7B5><C4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
#define Ki 0.005f /*<2A><><EFBFBD>ٶȼƺʹ<C6BA><CDB4><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵĻ<C7B5><C4BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
#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)
{
/*ԭ<>㷨<EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD>*/
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;
/*<2A><><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ij<EFBFBD><C4B3><EFBFBD>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD>ʱ<EFBFBD><CAB1>*/
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; /*<2A>õ<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ڵ<EFBFBD>һ<EFBFBD><D2BB>t = (1/SampleFrq) * 0.5 */
}
else
{
if (useMag == 1)
{
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
halfT = 0.1f;
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
gx = gy = gz = 0;
AHRS_Init++;
}
else
{
return;
}
}
norm = invSqrt(ax*ax + ay*ay + az*az); /*<2A>Ѽ<EFBFBD><D1BC>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>*/
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; /*<2A>Ѵ<EFBFBD><D1B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD> */
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)); /*<2A><>(mx,my,mz)ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>(hx,hy,hz),<2C><><EFBFBD><EFBFBD>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));
/*<2A>Ƶ<EFBFBD><C6B5><EFBFBD>ֵΪ(bx,by,0),<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Dz<EFBFBD><C7B2><EFBFBD>עhx<68><78><EFBFBD><EFBFBD>ֵ */
// bx = sqrtf((hx*hx) + (hy*hy)); /*ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
by = sqrtf((hx*hx) + (hy*hy));
bz = hz;
/*v<><76><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰѵ<C7B0><D1B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ļ<EFBFBD><C4BC>ٶ<EFBFBD>(0,0,1g)ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ļ<EFBFBD> */
vx = 2.0f*(q1q3 - q0q2); /*<2A>ٶȼ<D9B6>(ax,ay,az),<2C><>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R*(0,0,1),RΪ<52><CEAA>ת<EFBFBD><D7AA><EFBFBD>󣬴˾<F3A3ACB4><CBBE><EFBFBD><EFBFBD><EFBFBD> */
vy = 2 * (q0q1 + q2q3); /*<2A><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>ת<EFBFBD><D7AA><EFBFBD>õ<EFBFBD> */
vz = q0q0 - q1q1 - q2q2 + q3q3;
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
wx = 2.0f*(by*(q1q2 + q0q3) + bz*(q1q3 - q0q2)); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>H<EFBFBD><48>B<EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>W = Q * B */
wy = 2.0f*(by*(0.5f - q1q1 - q3q3) + bz*(q0q1 + q2q3)); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊbx = 0 */
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
wz = 2.0f*(by*(q2q3 - q0q1) + bz*(0.5f - q1q1 - q2q2));
/*<2A><><EFBFBD><EFBFBD>ʹ<EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>ںϴ<DABA><CFB4><EFBFBD><EFBFBD><EFBFBD>*/
if (useMag != 0)
{ /*<2A>õ<EFBFBD>ǰ<EFBFBD><C7B0>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC>ٶȵ<D9B6><C8B5><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD><CBA3>˻<EFBFBD>ԽС˵<D0A1><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD>ͬ*/
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); /*<2A><>Ϊ<EFBFBD><CEAA><EFBFBD>ٶȼ<D9B6>û<EFBFBD><C3BB>У׼<D0A3><D7BC>ez<65><7A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ez = AccelAjust*(ax*vy - ay*vx);
ez = 0;
}
if (ex != 0.0f && ey != 0.0f && ez != 0.0f)
{
/*<2A><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>PID<49><44><EFBFBD>ڵķ<DAB5>ʽ<EFBFBD><CABD><EFBFBD>Խ<EFBFBD><D4BD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>,Ki <20>ǻ<EFBFBD><C7BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Kp<4B><70>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
exInt = exInt + ex*Ki * halfT; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD> */
eyInt = eyInt + ey*Ki * halfT;
ezInt = ezInt + ez*Ki * halfT;
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
gx = gx + Kp*(1 + Kd*fabsf(ex))*ex + exInt; /*ʹ<>ñ<EFBFBD><C3B1><EFBFBD><EFBFBD>ͻ<EFBFBD><CDBB><EFBFBD><EFBFBD>ۺ϶<DBBA><CFB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD> */
gy = gy + Kp*(1 + Kd*fabsf(ey))*ey + eyInt; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>λ<EFBFBD><CEBB> */
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
gz = gz + Kp*(1 + Kd*fabsf(ez))*ez + ezInt;
}
qw = (-q1*gx - q2*gy - q3*gz)*halfT; /*<2A><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD>*/
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;
//LowPassFilterYaw(&q3,0.98);
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); /*<2A><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD>й淶<D0B9><E6B7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>ģΪ1*/
q0 = q0 * norm; //w
q1 = q1 * norm; //x
q2 = q2 * norm; //y
q3 = q3 * norm; //z
/*<2A><>ԭ<EFBFBD>任<EFBFBD><E4BBBB>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>⣬<EFBFBD><E2A3AC><EFBFBD>ձ<EFBFBD>׼y<D7BC><79>z<EFBFBD><7A>xǰ*/
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
Pitch = asinf(-2.0f*(q2*q1 - q0*q3))* (180.0f / 3.141592f); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǵ<EFBFBD>ת<EFBFBD><D7AA> */
Yaw = atan2f(q3*q1 + q0*q2, 0.5f - q3*q3 - q2*q2)* (180.0f / 3.141592f);
Roll = atan2f(q3*q2 + q0*q1, 0.5f - q3*q3 - q1*q1)* (180.0f / 3.141592f);
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
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);
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
//WriteLog(Info, "Get Magnet Data: %.4f %.4f %.4f\n", v.x, v.y, v.z);
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
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 : <EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><EFBFBD><EFBFBD> 1/Sqrt(x)<EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0x5f3759df
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD> : x
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD>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("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>\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;
/* <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD> */
if (LastPitch == 0.0f&&LastRoll == 0.0f){
LastPitch = *Pitch;
LastRoll = *Roll;
}
/*<2A><>Pitch<63><68>Roll΢<6C>֣<EFBFBD><D6A3><EFBFBD>Gx<47><78>Gy */
GyroX = fabsf(*Roll - LastRoll) / dTime;
GyroY = fabsf(*Pitch - LastPitch) / dTime;
/*<2A><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>*/
if (LastGyroX == 0.0f&&LastGyroY == 0.0f){
LastGyroX = GyroX;
LastGyroY = GyroY;
}
/*<2A><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>żȻ<C5BC><C8BB><EFBFBD><EFBFBD>*/
GyroX = alpha*LastGyroX + (1 - alpha)*GyroX;
GyroY = alpha*LastGyroY + (1 - alpha)*GyroY;
if (GyroX < 0.0055f&&GyroY < 0.0015f){
count++;
if (count == 3000){
count = 0;
HmdStandByState = true;
printf("ͷ<EFBFBD>Խ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n");
}
}
else{
count = 0;
HmdStandByState = false;
}
/* <20><><EFBFBD>¾<EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD> */
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;
修复Release模式下不能识别IMU的bug、修复磁力计校准接口异常，优化磁力计校准数据加载时的磁力计异常数据校验
2025-01-27 22:28:36 +08:00
}
Vector3 GetMagData()
{
Vector3 v = { 0 };
static Vector3 VLast = { 0 };
if (!GlobalMag_Updating) {
VLast = GlobalMag;
}
v = VLast;
WriteLog(Info, "Get Magnet Data: %.4f %.4f %.4f\n", v.x, v.y, v.z);
return v;
}