578 lines
14 KiB
C++
578 lines
14 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;
|
||
|
||
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);
|
||
|
||
|
||
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<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;
|
||
|
||
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
|
||
* <20><><EFBFBD><EFBFBD> : <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼơ<C8BC><C6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD>ֵ
|
||
* <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>ٶȼơ<C8BC><C6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ںϵõ<CFB5><C3B5><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD>
|
||
|
||
1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD>ڱ<EFBFBD><EFBFBD>ѿ<EFBFBD><D1BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>z<EFBFBD>ᳯ<EFBFBD>ϣ<EFBFBD>x<EFBFBD>ᳯ<EFBFBD>ˣ<EFBFBD>y<EFBFBD>ᳯ<EFBFBD><E1B3AF>
|
||
2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><CDB8>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>
|
||
|
||
* <20><EFBFBD> <20><>2016/11/5 : <20><><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD><EFBFBD>ʱȽϵ͵<CFB5><CDB5>ص㣬<D8B5><E3A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32>ģʽ<C4A3><CABD>1<EFBFBD><31><EFBFBD>ںϴ<DABA><CFB4><EFBFBD><EFBFBD><EFBFBD> 2<><32><EFBFBD><EFBFBD><EFBFBD>ںϴ<DABA><CFB4><EFBFBD><EFBFBD><EFBFBD>
|
||
2016/11/6 <20><><EFBFBD>Ľ<EFBFBD><C4BD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD>ᵼ<EFBFBD><E1B5BC><EFBFBD>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD>
|
||
2016/11/9 : <20>涨<EFBFBD>˱<EFBFBD><CBB1>㷨<EFBFBD><E3B7A8><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᣬ<EFBFBD><E1A3AC>д<EFBFBD><D0B4><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>DZ任<C7B1>Ĵ<EFBFBD><C4B4>롣
|
||
***********************************************************************************************************/
|
||
|
||
|
||
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;
|
||
}
|
||
|
||
|
||
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;
|
||
|
||
//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ǰ*/
|
||
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 (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 : <20><><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD> 1/Sqrt(x)<29><>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0x5f3759df
|
||
* <20><><EFBFBD><EFBFBD> : x
|
||
* <20><><EFBFBD><EFBFBD> <20><>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;
|
||
|
||
//printf("%f %f\n", GyroX, 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;
|
||
} |