// 编译环境 Keil uVision2
//****************************************
#include "include.h"
#include "math.h"
//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
//卡尔曼滤波参数与函数 Rol_Kalman_Filter
float rol_angle_kl=0,rol_angle_dot=0;//角度和角速度
float rol_P[2][2] = {{ 1, 0 },
{ 0, 1 }};
float rol_Pdot[4] ={ 0,0,0,0};
float rol_q_bias=0, rol_angle_err=0, rol_PCt_0=0, rol_PCt_1=0, rol_E=0, rol_K_0=0, rol_K_1=0, rol_t_0=0, rol_t_1=0;
//卡尔曼滤波参数与函数
float angle_kl=0, yijiPitAngle=0,yijiRolAngle=0, angle_dot=0;//角度和角速度
float P[2][2] = {{ 1, 0 },
{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.003; //角度数据置信度,角速度数据置信度
//float Q_angle=0.9, Q_gyro=0.01; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
//float R_angle=0.05 ,C_0 = 1;
float q_bias = 0, angle_err=0, PCt_0=0, PCt_1=0, E=0, K_0=0, K_1=0, t_0=0, t_1=0;
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
uint32_t TimeCount100ms = 0,TimeCount100msfTimer=0,TimeCount1minute=0,TimeCount1hour=0,TimeCount10ms = 0,TimeCount10s = 0;
uint8_t lineGXMaxCnt = 0,lineGYMaxCnt = 0,lineGZMaxCnt = 0,lineGXMinCnt = 0,lineGYMinCnt = 0,lineGZMinCnt = 0;
u8 AccRawBufCount =0;
u8 StableCount = 0;
u8 MoveCount = 0;
u8 StopCount = 0;
u8 changeFrqWalk10sCnt = 0,changeFrqCar10sCnt = 0,changeFrqTrain10sCnt = 0;
s16 ChangePer10Sec = 0;
Stable_StatusTypeDef Status;
T_float_xyz line_g_max_tmp,line_g_max_tmp_pre,line_g_min_tmp,line_g_min_tmp_pre;
T_int16_xyz acc_max_tmp,acc_min_tmp,acc_max_tmp_pre,acc_min_tmp_pre;
float tempX=0,tempY=0,tempZ=0;
u8 line_g_status = 0;
uint32_t line_g_peak_count = 0;
u8 line_g_rise_cnt =0,line_g_drop_cnt=0,line_g_stable_cnt=0,line_g_cons_cnt=0;
u8 pattern_rise_cnt =0,pattern_drop_cnt=0,pattern_stable_cnt=0,pattern_cons_cnt=0;
u16 cmdRecogState = 0;
float AccX,AccY,AccZ;
T_int16_xyz Acc_pre;
float AngleOffsetRol=0,AngleOffsetPit=0,AngleOffsetYaw=0,AngleRolPre=0,AnglePitPre=0,AngleYawPre=0;
int YawBuf[filterWidth]={0},YawBufSum = 0,YawBufSumPre=0;
short AccXRawBuf[filterWidth]={0};
short AccYRawBuf[filterWidth]={0};
short AccZRawBuf[filterWidth]={0};
short AccXFilBuf[filterWidth]={0};
short AccYFilBuf[filterWidth]={0};
short AccZFilBuf[filterWidth]={0};
short AccXFilFinishBuf[filterWidth]={0};
short AccYFilFinishBuf[filterWidth]={0};
short AccZFilFinishBuf[filterWidth]={0};
float line_g_XBuf[filterWidth]={0},line_g_YBuf[filterWidth]={0},line_g_ZBuf[filterWidth]={0};
short GyrXRawBuf[filterWidth]={0};
short GyrYRawBuf[filterWidth]={0};
short GyrZRawBuf[filterWidth]={0};
short MagXRawBuf[filterWidth]={0};
short MagYRawBuf[filterWidth]={0};
short MagZRawBuf[filterWidth]={0};
/************
*******************************************************************
* Function Name : TIM2_Int_Init
* Description : TIM2 initialization function
* Input : arr 重装初值
psc 预分频
* Output : None
* Return : None
* Attention : None
*******************************************************************************/
void TIM2_Int_Init(u16 arr,u16 psc)//7199 9 100ms = 7200,1000
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //时钟使能
//定时器TIM3初始化
TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根据指定的参数初始化TIMx的时间基数单位
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE ); //使能指定的TIM3中断,允许更新中断
//中断优先级NVIC设置
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; //TIM3中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; //先占优先级0级
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; //从优先级3级
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
NVIC_Init(&NVIC_InitStructure); //初始化NVIC寄存器
TIM_Cmd(TIM2, ENABLE); //使能TIMx
}
/******************************************************************************/
/* */
/* 定时器2中断服务程序 */
/******************************************************************************/
extern int CntAccdMax;
void TIM2_IRQHandler(void) // 10ms counter
{
int temp = 0,i=0;
if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) //检查TIM3更新中断发生与否
{
TIM_ClearITPendingBit(TIM2, TIM_IT_Update ); //清除TIMx更新中断标志
TimeCount10ms++;
if(TimeCount10ms % 1000 ==0)// 10s counted, 10ms counter
{
TimeCount10s++;
changeFrqWalk10sCnt++;
changeFrqTrain10sCnt++;
changeFrqCar10sCnt++;
Status.acc_g_chanfrq = line_g_peak_count;
//printf("acc_g_chanfrq=%d\r\n",Status.acc_g_chanfrq);
line_g_peak_count = 0;
}
if(TimeCount10ms % 100 ==0)//1s counter
{
}
if(TimeCount10ms % 10 ==0)//100ms counter
{
//printf("TIM2_IRQHandler\r\n");
TimeCount100msfTimer ++;
//bmi160_read_step_count(&(Status.StepCount));
ReportDevStatusHandle();
}
if(TimeCount100msfTimer == 600)// 1 minute
{
TimeCount1minute ++;
TimeCount100msfTimer = 0;
}
if(TimeCount1minute == 60)// 1 hour
{
TimeCount1hour ++;
TimeCount1minute =0;
}
}
}
/*******************************************************************************
快速计算 1/Sqrt(x)，源自雷神3的一段代码，神奇的0x5f3759df！比正常的代码快4倍
*******************************************************************************/
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;
}
float AccSAxis_dV_dS(short *AccBuf,u8 count,float *dV,float *dS,float dt)
{
float temp;
int i;
for (i=1; i
temp=((AccBuf[i-1] + AccBuf[i])/2);// change to g
(*dV)=(*dV)+(temp/16384)*9.8*dt;
(*dS)=(*dS)+(*dV)*dt;
//printf("dV_dS *dV=%f\r\n",*dV);
//printf("dV_dS *dS=%f\r\n",*dS);
}
}
#define CalibrationCnt 200
void Calibration()
{
int32_t tempgx=0,tempgy=0,tempgz=0,tempax=0,tempay=0,tempaz=0,tempmx=0,tempmy=0,tempmz=0;
uint8_t cnt_g=0,j;
s8 func_return = 0;
for(cnt_g=0;cnt_g
AccRawBufCount = cnt_g%filterWidth;
ReadData();
#ifdef BMI160_MAG
tempmx += magxyz.x;
tempmy += magxyz.y;
tempmz += magxyz.z;
#endif
tempax += AccXRawBuf[AccRawBufCount];
tempay += AccYRawBuf[AccRawBufCount];
tempaz += AccZRawBuf[AccRawBufCount];
tempgx += GyrXRawBuf[AccRawBufCount];
tempgy += GyrYRawBuf[AccRawBufCount];
tempgz += GyrZRawBuf[AccRawBufCount];
AccXFilFinishBuf[AccRawBufCount] =AccXFilBuf[AccRawBufCount] = AccXRawBuf[AccRawBufCount];
AccYFilFinishBuf[AccRawBufCount] =AccYFilBuf[AccRawBufCount] = AccYRawBuf[AccRawBufCount];
AccZFilFinishBuf[AccRawBufCount] =AccZFilBuf[AccRawBufCount] = AccZRawBuf[AccRawBufCount];
Delay_ms(1);
//printf("BufCount=%d,tempax=%d,tempay=%d,tempaz=%d\r\n",AccRawBufCount,tempax,tempay,tempaz);
}
GYRO_OFFSET.X=tempgx/CalibrationCnt;
GYRO_OFFSET.Y=tempgy/CalibrationCnt;
GYRO_OFFSET.Z=tempgz/CalibrationCnt;
ACC_OFFSET.X=tempax/CalibrationCnt;
ACC_OFFSET.Y=tempay/CalibrationCnt;
//ACC_OFFSET.Z=0;
ACC_OFFSET.Z=(tempaz/CalibrationCnt)-gravity_g;
#ifdef BMI160_MAG
MAG_OFFSET.X=tempmx/CalibrationCnt;
MAG_OFFSET.Y=tempmy/CalibrationCnt;
MAG_OFFSET.Z=tempmz/CalibrationCnt;
#else
#endif
AccRawBufCount = 0;
printf("aOffX=%d,aOffY=%d,aOffZ=%d,magXoff=%d,magYoff=%d,magZoff=%d\r\n",ACC_OFFSET.X,ACC_OFFSET.Y,ACC_OFFSET.Z,MAG_OFFSET.X,MAG_OFFSET.Y,MAG_OFFSET.Z);
}
void ReadData(void)
{
#ifdef BMI160
{
bmi160_read_gyro_xyz(&gyroxyz);
/* read accel data */
bmi160_read_accel_xyz(&accelxyz);
#ifdef BMI160_MAG
bmi160_bmm150_mag_compensate_xyz(&magxyz);
#endif
Gyr_pre.X = Status.gyrxyz.X;
Gyr_pre.Y = Status.gyrxyz.Y;
Gyr_pre.Z = Status.gyrxyz.Z;
Status.gyrxyz.X = gyroxyz.x;
Status.gyrxyz.Y = gyroxyz.y;
Status.gyrxyz.Z = gyroxyz.z;
#ifdef BMI160_MAG
Status.magxyz.X = (short)magxyz.x;
Status.magxyz.Y = (short)magxyz.y;
Status.magxyz.Z = (short)magxyz.z;
MagXRawBuf[AccRawBufCount] = Status.magxyz.X;
MagYRawBuf[AccRawBufCount] = Status.magxyz.Y;
MagZRawBuf[AccRawBufCount] = Status.magxyz.Z;
#endif
/* read mag data*/
//func_return += bmi160_bmm150_mag_compensate_xyz(&magxyz);
//printf("---x=%d,y=%d,z=%d--ax=%d,ay=%d,az=%d---\r\n",magxyz.x,magxyz.y,magxyz.z,accelxyz.x,accelxyz.y,accelxyz.z);
}
#else
{
accelxyz.X = GetAccXData();
accelxyz.Y = GetAccYData();
accelxyz.Z = GetAccZData();
Gyr_pre.X = Status.gyrxyz.X;
Gyr_pre.Y = Status.gyrxyz.Y;
Gyr_pre.Z = Status.gyrxyz.Z;
Status.gyrxyz.X = GetGyroXData();
Status.gyrxyz.Y = GetGyroYData();
Status.gyrxyz.Z = GetGyroZData();
//printf("AccX=%d,AccY=%d,AccZ=%d,GyrX=%d,GyrY=%d,GyrZ=%d\r\n",Acc.X,Acc.Y,Acc.Z,Status.gyrxyz.X,Status.gyrxyz.Y,Status.gyrxyz.Z);
}
#endif
AccXRawBuf[AccRawBufCount] = accelxyz.x;
AccYRawBuf[AccRawBufCount] = accelxyz.y;
AccZRawBuf[AccRawBufCount] = accelxyz.z;
GyrXRawBuf[AccRawBufCount] = Status.gyrxyz.X;
GyrYRawBuf[AccRawBufCount] = Status.gyrxyz.Y;
GyrZRawBuf[AccRawBufCount] = Status.gyrxyz.Z;
}
void PrepareData(u8 status)
{
AccXFilBuf[AccRawBufCount] = filter_midle(AccXRawBuf,filterWidth);
AccYFilBuf[AccRawBufCount] = filter_midle(AccYRawBuf,filterWidth);
AccZFilBuf[AccRawBufCount] = filter_midle(AccZRawBuf,filterWidth);
//printf("AccX_3=%d,AccRawBufCount=%d,Status.accxyz.X=%d\r\n",AccXFilBuf[AccRawBufCount],AccRawBufCount,Status.accxyz.X);
Acc_pre.X = Status.accxyz.X;
Acc_pre.Y = Status.accxyz.Y;
Acc_pre.Z = Status.accxyz.Z;
Status.accxyz.X = filter_slide(AccXFilBuf,filterWidth);//-ACC_OFFSET.X;
Status.accxyz.Y = filter_slide(AccYFilBuf,filterWidth);//-ACC_OFFSET.Y;
Status.accxyz.Z = filter_slide(AccZFilBuf,filterWidth);//-ACC_OFFSET.Z;
AccXFilFinishBuf[AccRawBufCount] = Status.accxyz.X;
AccYFilFinishBuf[AccRawBufCount] = Status.accxyz.Y;
AccZFilFinishBuf[AccRawBufCount] = Status.accxyz.Z;
acc_max_tmp_pre.X = acc_max_tmp.X;
acc_max_tmp_pre.Y = acc_max_tmp.Y;
acc_max_tmp_pre.Z = acc_max_tmp.Z;
acc_min_tmp_pre.X = acc_min_tmp.X;
acc_min_tmp_pre.Y = acc_min_tmp.Y;
acc_min_tmp_pre.Z = acc_min_tmp.Z;
CalAccMaxMin();
CalCountPeak();// now count peak and calculate the rise state
PatternRecognition();
//printf("Acc.X=%d,Status.accxyz.X=%d\r\n",Acc.X,Status.accxyz.X);
//printf("AccX_4=%d\r\n",Status.accxyz.X);
Status.gyrxyz.X = filter_slide(GyrXRawBuf,filterWidth);//-GYRO_OFFSET.X;
Status.gyrxyz.Y = filter_slide(GyrYRawBuf,filterWidth);//-GYRO_OFFSET.Y;
Status.gyrxyz.Z = filter_slide(GyrZRawBuf,filterWidth);//-GYRO_OFFSET.Z;
#ifd
ef BMI160_MAG
Status.magxyz.X = filter_slide(MagXRawBuf,filterWidth);//-MAG_OFFSET.X;
Status.magxyz.Y = filter_slide(MagYRawBuf,filterWidth);//-MAG_OFFSET.Y;
Status.magxyz.Z = filter_slide(MagZRawBuf,filterWidth);//-MAG_OFFSET.Z;
#else
#endif
AngleYawPre = Status.angle.yaw;
#ifdef KALMAN_FILTER
KalmanFilterupdate(&Status.gyrxyz,&(Status.accxyz),&Status.angle);
//yiOrderFilterupdate(&Status.gyrxyz,&(Status.accxyz),&Status.angle);
//MadgwickAHRSupdate(&Status.gyrxyz,&(Status.accxyz),&(Status.magxyz),&Status.angle);
//IMU_AHRSupdate(&Status.gyrxyz,&(Status.accxyz),&(Status.magxyz),&Status.angle);//float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
#else
IMUupdate(&Status.gyrxyz,&(Status.accxyz),&Status.angle);
#endif
ComplimentaryFilter(&Status.gyrxyz,&(Status.accxyz),&Status.angle);
YawBuf[AccRawBufCount] = (abs)((int)(AngleYawPre*RtA) - (int)(Status.angle.yaw*RtA));
//printf("YawBuf=%d,yaw=%d,yawpre=%d\r\n",YawBuf[AccRawBufCount],(int)(Status.angle.yaw*RtA),(int)(AngleYawPre*RtA));
//printf("fx=%d,gx=%d,R=%.3f,cR=%.3f,P=%.3f,cP=%.3f,Y=%.3f,cY=%.3f\r\n",Status.accxyz.X,Status.gyrxyz.X,Status.angle.rol*RtA,https://www.doczj.com/doc/524476137.html,p_angle.rol,Status.angle.pit*RtA,https://www.doczj.com/doc/524476137.html,p_angle.pit,Status.angle.yaw*RtA,https://www.doczj.com/doc/524476137.html,p_angle.yaw);
// begin lowpass filter
linear_acceleration_pre[0] = linear_acceleration[0];
linear_acceleration_pre[1] = linear_acceleration[1];
linear_acceleration_pre[2] = linear_acceleration[2];
line_g_XBuf[AccRawBufCount] = linear_acceleration[0];
line_g_YBuf[AccRawBufCount] = linear_acceleration[1];
line_g_ZBuf[AccRawBufCount] = linear_acceleration[2];
AccX = ((float)(Status.accxyz.X))/gravity_g*9.8;
AccY = ((float)(Status.accxyz.Y))/gravity_g*9.8;
AccZ = ((float)(Status.accxyz.Z))/gravity_g*9.8;
#if 0
gravity[0] = alpha * gravity[0] + (1 - alpha) * AccX;
gravity[1] = alpha * gravity[1] + (1 - alpha) * AccY;
gravity[2] = alpha * gravity[2] + (1 - alpha) * AccZ;
linear_acceleration[0] = AccX - gravity[0];
linear_acceleration[1] = AccY - gravity[1];
linear_acceleration[2] = AccZ - gravity[2];
// end lowpass filter
#endif
if(fabs(Status.angle.pit)>0.26)// > 15 degree
{
#ifdef KALMAN_FILTER
Status.g_inxyz.X = 9.8*sin(Status.angle.pit)*cos(Status.angle.rol);
#else
Status.g_inxyz.X = -9.8*sin(Status.angle.pit)*cos(Status.angle.rol);
#endif
}
else
{
#ifdef KALMAN_FILTER
Status.g_inxyz.X = 9.8*sin(https://www.doczj.com/doc/524476137.html,p_angle.pit)*cos(https://www.doczj.com/doc/524476137.html,p_angle.rol);
#else
Status.g_inxyz.X = -9.8*sin(https://www.doczj.com/doc/524476137.html,p_angle.pit)*cos(https://www.doczj.com/doc/524476137.html,p_angle.rol);
#endif
}
if(fabs(Status.angle.rol)>0.26)// >15 degree
{
Status.g_inxyz.Y = 9.8*sin(Status.angle.rol);
}
else
{
Status.g_inxyz.Y = 9.8*sin(https://www.doczj.com/doc/524476137.html,p_angle.rol);
}
if(fabs(Status.angle.yaw)>0.26)// >15 degree
{
Status.g_inxyz.Z
= 9.8*cos(Status.angle.pit)*cos(Status.angle.rol);
}
{
Status.g_inxyz.Z = 9.8*cos(https://www.doczj.com/doc/524476137.html,p_angle.pit)*cos(https://www.doczj.com/doc/524476137.html,p_angle.rol);
}
#if 1
if(fabs(Status.angle.pit)>0.04)//Status.angle.pit
linear_acceleration[0] = AccX -Status.g_inxyz.X ;
else
linear_acceleration[0] = AccX;
if(fabs(Status.angle.rol)>0.04)
linear_acceleration[1] = AccY -Status.g_inxyz.Y ;
else
linear_acceleration[1] = AccY;
if((fabs(Status.angle.pit)>0.04)&&(fabs(Status.angle.rol>0.04)))
linear_acceleration[2] = AccZ -Status.g_inxyz.Z ;
else
linear_acceleration[2] = AccZ;
#else
linear_acceleration[0] = AccX -Status.g_inxyz.X ;
linear_acceleration[1] = AccY -Status.g_inxyz.Y ;
linear_acceleration[2] = AccZ -Status.g_inxyz.Z ;
#endif
//printf("fx=%d,R=%.3f,cR=%.3f,P=%.3f,cP=%.3f,AX=%.4f,giX=%.4f,l=%.4f,giX_b=%f\r\n",Status.accxyz.X,Status.angle.rol*RtA,https://www.doczj.com/doc/524476137.html,p_angle.rol*RtA,Status.angle.pit*RtA,https://www.doczj.com/doc/524476137.html,p_angle.pit*RtA,AccY,Status.g_inxyz.X,linear_acceleration[0], -9.8*sin(Status.angle.pit)*cos(Status.angle.rol));
tempX=(linear_acceleration_pre[0] +linear_acceleration[0])/2;
tempY=(linear_acceleration_pre[1] +linear_acceleration[1])/2;
tempZ=(linear_acceleration_pre[2] +linear_acceleration[2])/2;
//printf("liX=%f,gravity[0]=%f,tempX=%f,Acc_pre.X=%d,liY=%f,AccX=%f,AccY=%f\r\n",linear_acceleration[0],gravity[0],tempX,Acc_pre.X,linear_acceleration[1],AccX,AccY);
}
////////////////////////////////////////////////////////////////////////////////
#define betaDef 0.1f // 2 * proportional gain
#define sampleFreq 10.0f // sample frequency in Hz
//---------------------------------------------------------------------------------------------------
// Variable definitions
volatile float beta = betaDef;
void MadgwickAHRSupdate(T_int16_xyz *gyr, T_int16_xyz *acc, T_int16_xyz *mag, T_float_angle *angle)
void IMU_AHRSupdate(T_int16_xyz *gyr, T_int16_xyz *acc, T_int16_xyz *mag, T_float_angle *angle)//float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
/*
void Erjielvbo(float angle_m,float gyro_m)
{
x1=(angle_m-angle2)*(1-K2)*(1-K2);
y1=y1+x1*Acc_dt;
x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
angle2=angle2+ x2*Acc_dt;
}
*/
//卡尔曼滤波
float Rol_Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
//卡尔曼滤波
float Pit_Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
void KalmanFilterupdate(T_int16_xyz *gyr, T_int16_xyz *acc, T_float_angle *angle)
void ComplimentaryFilter(T_int16_xyz *gyr, T_int16_xyz *acc, T_float_angle *angle)
void IMUupdate(T_int16_xyz *gyr, T_int16_xyz *acc, T_float_angle *angle)
{
//所以在软件解算中，我们要首先把加速度计采集到的值(三维向量)转化为单位向量,即向量除以模，
//传入参数是陀螺仪x,y,z值和
加速度计x,y,z值：
float ax = acc->X,ay = acc->Y,az = acc->Z;
float gx = gyr->X,gy = gyr->Y,gz = gyr->Z;
float norm;
float vx, vy, vz;
float ex, ey, ez;
// ???????????
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(ax*ay*az==0)
// return;
gx *= Gyro_Gr;
gy *= Gyro_Gr;
gz *= Gyro_Gr;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
//下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx,vy,vz 其实就是上一次的欧拉角
//（四元数）的机体坐标参考系换算出来的重力的单位向量。
// estimated direction of gravity
vx = 2*(q1q3 - q0q2); //????xyz???
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
//axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
//axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机
//体坐标参照系上的重力向量。 那它们之间的误差向量，就是陀螺积分后的姿态和加计测出
//来的姿态之间的误差。 向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，
//exyz就是两个重力向量的叉积。 这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差
//也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。
//（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现
//在对机体坐标系的纠正。
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
//用叉积误差来做PI修正陀螺零偏
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
//四元数微分方程，其中T为测量周期，为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库
//塔求解四元数微分方程：
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角
：
//printf("pit asin = %f, atan1=%f,atan2=%f\r\n",(-2 * q1 * q3 + 2 * q0* q2),(2 * q2 * q3 + 2 * q0 * q1), (-2 * q1 * q1 - 2 * q2* q2 + 1));
//angle->yaw += gyr->Z*Gyro_G*0.002f;
//angle->pit = (float)(asin(-2 * q1 * q3 + 2 * q0* q2))* 57.3 ;//- AngleOffset_Pit; // pitch
//angle->rol = (float)(atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1))* 57.3;// - AngleOffset_Rol; // roll
angle->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1); // yaw
angle->pit = asin(-2 * q1 * q3 + 2 * q0* q2); // pitch
angle->rol = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); // roll
}
void InverseMatrix(void)
{
float IAI = 0;
Status.IMtri[0][0] = cos(Status.angle.pit);
Status.IMtri[0][1] = sin(Status.angle.pit)*sin(Status.angle.rol);
Status.IMtri[0][2] = -sin(Status.angle.pit)*cos(Status.angle.rol);
Status.IMtri[1][0] = 0;
Status.IMtri[1][1] = cos(Status.angle.rol);
Status.IMtri[1][2] = sin(Status.angle.rol);
Status.IMtri[2][0] = sin(Status.angle.pit);
Status.IMtri[2][1] = -cos(Status.angle.pit)*sin(Status.angle.rol);
Status.IMtri[2][2] = cos(Status.angle.pit)*cos(Status.angle.rol);
//|A|=a11a22a33+a12a23a31+a13a21a32-a11a23a32-a12a21a33-a13a22a31；
IAI = Status.IMtri[0][0]*Status.IMtri[1][1]*Status.IMtri[2][2] + Status.IMtri[0][1]*Status.IMtri[1][2]*Status.IMtri[2][0] + Status.IMtri[0][2]*Status.IMtri[1][0]*Status.IMtri[2][1]
- Status.IMtri[0][0]*Status.IMtri[1][2]*Status.IMtri[2][1] - Status.IMtri[0][1]*Status.IMtri[1][2]*Status.IMtri[2][2] - Status.IMtri[0][2]*Status.IMtri[1][1]*Status.IMtri[2][0];
Status.IMtro[0][0] = ( Status.IMtri[1][1]*Status.IMtri[2][2]-Status.IMtri[2][1]*Status.IMtri[1][2])/IAI;
Status.IMtro[0][1] = (-Status.IMtri[0][1]*Status.IMtri[2][2]+Status.IMtri[2][1]*Status.IMtri[0][2])/IAI;
Status.IMtro[0][2] = ( Status.IMtri[0][1]*Status.IMtri[1][2]-Status.IMtri[0][2]*Status.IMtri[1][1])/IAI;
Status.IMtro[1][0] = ( Status.IMtri[1][2]*Status.IMtri[2][0]-Status.IMtri[2][2]*Status.IMtri[1][0])/IAI;
Status.IMtro[1][1] = (-Status.IMtri[0][2]*Status.IMtri[2][0]+Status.IMtri[2][2]*Status.IMtri[0][0])/IAI;
Status.IMtro[1][2] = ( Status.IMtri[0][2]*Status.IMtri[1][0]-Status.IMtri[0][0]*Status.IMtri[1][2])/IAI;
Status.IMtro[2][0] = ( Status.IMtri[1][0]*Status.IMtri[2][1]-Status.IMtri[2][0]*Status.IMtri[1][1])/IAI;
Status.IMtro[2][1] = (-Status.IMtri[0][0]*Status.IMtri[2][1]+Status.IMtri[2][0]*Status.IMtri[0][1])/IAI;
Status.IMtro[2][2] = ( Status.IMtri[0][0]*Status.IMtri[1][1]-Status.IMtri[0][1]*Status.IMtri[1][0])/IAI;
}
/*
void IMUupdate(T_int16_xyz *gyr, T_int16_xyz *acc, T_float_angle *angle)
*/