638 lines
17 KiB
C++
638 lines
17 KiB
C++
|
#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;
|
|||
|
|
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
|
|||
|
|
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 DM_GetMag()
|
|||
|
|
{
|
|||
|
|
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;
|
|||
|
|
INT16 MagX1, MagY1, MagZ1;
|
|||
|
|
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;
|
|||
|
|
//printf("NowTime = %d LastTime = %d Dtime = %d\n",NowTime,LastTime,Dtime);
|
|||
|
|
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");
|
|||
|
|
}
|
|||
|
|
//if(Dtime>3)printf("NowTime = %d LastTime = %d Dtime = %d\n",NowTime,LastTime,Dtime);
|
|||
|
|
// MagX1 = *(INT16 *)(usb_input_buffer + 46);
|
|||
|
|
// MagY1 = *(INT16 *)(usb_input_buffer + 48);
|
|||
|
|
// MagZ1 = *(INT16 *)(usb_input_buffer + 50);
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// q0 = *(float *)(usb_input_buffer+24);
|
|||
|
|
// q1 = *(float *)(usb_input_buffer+28);
|
|||
|
|
// q2 = *(float *)(usb_input_buffer+32);
|
|||
|
|
// q3 = *(float *)(usb_input_buffer+36);
|
|||
|
|
// q0 = *(float *)(usb_input_buffer + 32);
|
|||
|
|
// q1 = *(float *)(usb_input_buffer + 36);
|
|||
|
|
// q2 = *(float *)(usb_input_buffer + 40);
|
|||
|
|
// q3 = *(float *)(usb_input_buffer + 44);
|
|||
|
|
global_key[0] = usb_input_buffer[24];
|
|||
|
|
//global_key[0] = usb_input_buffer[25];
|
|||
|
|
//global_key[0] = usb_input_buffer[26];
|
|||
|
|
global_key[3] = usb_input_buffer[27];
|
|||
|
|
// GlobalQ_Updating = TRUE;
|
|||
|
|
// GlobalQ.w = q0;
|
|||
|
|
// GlobalQ.x = q2;
|
|||
|
|
// GlobalQ.y = q3;
|
|||
|
|
// GlobalQ.z = q1;
|
|||
|
|
// GlobalQ_Updating = FALSE;
|
|||
|
|
// Pitch = asin(-2.0f*(q3*q1 - q0*q2))* (180.0f / 3.141592f); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǵ<EFBFBD>ת<EFBFBD><D7AA> */
|
|||
|
|
// Yaw = atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3)* (180.0f / 3.141592f);
|
|||
|
|
// Roll = atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1)* (180.0f / 3.141592f);
|
|||
|
|
//
|
|||
|
|
// if (global_Show == 4)
|
|||
|
|
// printf("Yaw = %f,Pitch = %f Roll =%f\n", Yaw, Pitch, Roll);
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
// mx1 = (float)MagX1;
|
|||
|
|
// my1 = (float)MagY1;
|
|||
|
|
// mz1 = (float)MagZ1;
|
|||
|
|
GlobalMag.x = (float)MagX;
|
|||
|
|
GlobalMag.y = (float)MagY;
|
|||
|
|
GlobalMag.z = (float)MagZ;
|
|||
|
|
GlobalMag_Updating = false;
|
|||
|
|
}
|
|||
|
|
// mx = (float)MagX;
|
|||
|
|
// my = (float)MagY;
|
|||
|
|
// mz = (float)MagZ;
|
|||
|
|
|
|||
|
|
|
|||
|
|
//Pitch = asin(-2.0f*(q3*q1-q0*q2))* (180.0f /3.141592f); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǵ<EFBFBD>ת<EFBFBD><D7AA> */
|
|||
|
|
//Yaw = atan2(q2*q1 + q0*q3,0.5f - q2*q2 - q3*q3)* (180.0f /3.141592f);
|
|||
|
|
//Roll = atan2(q2*q3 + q0*q1,0.5f - q2*q2 - q1*q1)* (180.0f /3.141592f);
|
|||
|
|
//
|
|||
|
|
|
|||
|
|
//printf("Yaw = %f,Pitch = %f Roll =%f\n",Yaw,Pitch,Roll);
|
|||
|
|
|
|||
|
|
if(mxlast == 0&&mylast == 0&&mylast == 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);
|
|||
|
|
//printf("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB>ͬ...\n");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
mxlast = mx;
|
|||
|
|
mylast = my;
|
|||
|
|
mzlast = mz;
|
|||
|
|
// if (global_Show == 3)
|
|||
|
|
// printf("mx = %f my= %f mz= %f\n", mx, my, mz);
|
|||
|
|
// if (global_calibration_flag == 3)
|
|||
|
|
// {
|
|||
|
|
//
|
|||
|
|
// if (Getfpdata() != NULL)
|
|||
|
|
// {
|
|||
|
|
// fprintf(Getfpdata(), "%f %f %f\n", mx, my, mz);
|
|||
|
|
// Sleep(5);
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
MagFilter(&mx,&my,&mz,0.95f);
|
|||
|
|
|
|||
|
|
// printf("mx = %f my= %f mz= %f\n", mx, my, mz);
|
|||
|
|
// if (Getfpdata()!=NULL)
|
|||
|
|
// {
|
|||
|
|
// fprintf(Getfpdata(), "%f %f %f\n", mx, my, mz);
|
|||
|
|
// }
|
|||
|
|
AHRSupdate(gz,gx,gy,az,ax,ay,mz,mx,my,1,Dtime);
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//printf("mx = %f my= %f mz= %f\n",mx,my,mz);
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
/**********************************************************************************************************
|
|||
|
|
* 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> */
|
|||
|
|
#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> */
|
|||
|
|
#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)
|
|||
|
|
{
|
|||
|
|
halfT = 0.1;
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
// printf("<22><><EFBFBD>ٶȹ<D9B6><C8B9><EFBFBD>...\n");
|
|||
|
|
// getchar();
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
|
|||
|
|
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 */
|
|||
|
|
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;
|
|||
|
|
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> */
|
|||
|
|
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;
|
|||
|
|
|
|||
|
|
//QuaternionFilter(&q1,&q2,&q3,0.98f);
|
|||
|
|
|
|||
|
|
//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><EFBFBD><D7BC>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
|||
|
|
// Pitch = asin(-2.0f*(q3*q1 - q0*q2))* (180.0f / 3.141592f); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǵ<EFBFBD>ת<EFBFBD><D7AA> */
|
|||
|
|
// Yaw = atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3)* (180.0f / 3.141592f);
|
|||
|
|
// Roll = atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1)* (180.0f / 3.141592f);
|
|||
|
|
|
|||
|
|
// /*<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>x<EFBFBD><78>z<EFBFBD><7A>*/
|
|||
|
|
// Pitch = asin(-2.0f*(q2*q3 - q0*q1))* (180.0f / 3.141592f); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǵ<EFBFBD>ת<EFBFBD><D7AA> */
|
|||
|
|
// Yaw = atan2(q1*q3 + q0*q2, 0.5f - q1*q1 - q2*q2)* (180.0f / 3.141592f);
|
|||
|
|
// Roll = atan2(q1*q2 + q0*q3, 0.5f - q1*q1 - q3*q3)* (180.0f / 3.141592f);
|
|||
|
|
|
|||
|
|
/*<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ǰ*/
|
|||
|
|
Pitch = asin(-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 = 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 (global_Show == 4)
|
|||
|
|
// printf("Yaw = %f,Pitch = %f Roll =%f\n", Yaw, Pitch, Roll);
|
|||
|
|
if (AHRS_Init > 200)
|
|||
|
|
{
|
|||
|
|
GlobalQ_Updating = TRUE;
|
|||
|
|
GlobalQ.w = q0;
|
|||
|
|
GlobalQ.x = q2;
|
|||
|
|
GlobalQ.y = q3;
|
|||
|
|
GlobalQ.z = q1;
|
|||
|
|
GlobalQ_Updating = FALSE;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
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 && ylast == 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.0015f&&GyroY < 0.0015f){
|
|||
|
|
count++;
|
|||
|
|
if (count == 3000){
|
|||
|
|
count = 0;
|
|||
|
|
HmdStandByState = true;
|
|||
|
|
//printf("ͷ<>Խ<EFBFBD><D4BD><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 CQ_DM = { 1, 0, 0, 0 };
|
|||
|
|
bool DM_Corrected = false;
|
|||
|
|
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 DM_Correct(void)
|
|||
|
|
{
|
|||
|
|
Quaternion q = { GlobalQ.w, 0, GlobalQ.y, 0 };
|
|||
|
|
CQ_DM = CorrectQuaterinonYaw(q);
|
|||
|
|
DM_Corrected = true;
|
|||
|
|
return DM_Corrected ? 1 : 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Quaternion DM_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);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
return QLast;
|
|||
|
|
}
|