int main(void) {
struct position * pos;
uint16_t i;
for(i = 0; i < 64*30; i ++)
//for(;;)
{
prepare_data(0,0,0,-1,-2,-3);
pos = IMU_update();
if(pos != 0)
printf("yaw:%0.2f,roll:%0.2f,pitch:%0.2f\r\n",pos->yaw,pos->roll,pos->pitch); /* prints !!!Hello World!!! */
//printf("!!!Hello World!!! "); /* prints !!!Hello World!!! */
}
return EXIT_SUCCESS;
}
void AHRS_Attitude()
{
u8 i;
int tempval1 = 0,tempval2 = 0,tempval3 = 0;
// float sin2x,sin2y,abs_Roll_Last,abs_Pitch_Last;
static u8 filpoint = 0; //滤波器指针
//非DMP数据处理
//---陀螺仪----------------------------------------------------------------------------------------------------
GYRO_Raw.X = gyro[0]-GYRO_OFFSET.X; //陀螺仪原始数据
GYRO_Raw.Y = gyro[1]-GYRO_OFFSET.Y;
GYRO_Raw.Z = gyro[2]-GYRO_OFFSET.Z;
if(GYRO_Raw.X<(-5000)||GYRO_Raw.X>(5000)) GYRO_Raw.X = GYRO_Last.X; //如果读数有误，则使用原来值
if(GYRO_Raw.Y<(-5000)||GYRO_Raw.Y>(5000)) GYRO_Raw.Y = GYRO_Last.Y;
if(GYRO_Raw.Z<(-5000)||GYRO_Raw.Z>(5000)) GYRO_Raw.Z = GYRO_Last.Z;
GYRO_Last.X = GYRO_Raw.X;
GYRO_Last.Y = GYRO_Raw.Y;
GYRO_Last.Z = GYRO_Raw.Z;
//---加速度----------------------------------------------------------------------------------------------------
ACC_Raw.X = accel[0]-ACC_OFFSET.X; //陀螺仪原始数据
ACC_Raw.Y = accel[1]-ACC_OFFSET.Y;
ACC_Raw.Z = accel[2];//-ACC_OFFSET.Z; //
FiltArray_AX[filpoint] = (int)ACC_Raw.X; //每采集一个，放入队列中
FiltArray_AY[filpoint] = (int)ACC_Raw.Y;
FiltArray_AZ[filpoint] = (int)ACC_Raw.Z;
for(i=0;i<FILTERZISE_ACC;i++)
{
tempval1 += FiltArray_AX[i];
tempval2 += FiltArray_AY[i];
tempval3 += FiltArray_AZ[i];
}
ACC_Last.X = tempval1/FILTERZISE_ACC;
ACC_Last.Y = tempval2/FILTERZISE_ACC;
ACC_Last.Z = tempval3/FILTERZISE_ACC; //求和求平均
filpoint++;
if(filpoint==FILTERZISE_ACC) filpoint=0;
//---磁阻------------------------------------------------------------------------------------------------------------
magtmp.hx = -hmc5883l.hx; //进行符号和数据类型转换
magtmp.hy = -hmc5883l.hy; //磁阻传感器的的坐标系X,Y刚好与机体坐标系相反
magtmp.hz = hmc5883l.hz;
//---四元数刷新姿态----------------------------------------------------------------------------------------------
IMU_update(GYRO_Last.X*Gyro_Gr,GYRO_Last.Y*Gyro_Gr,GYRO_Last.Z*Gyro_Gr,
ACC_Last.X,ACC_Last.Y,ACC_Last.Z);
// AHRS_update(GYRO_Last.X*Gyro_Gr,GYRO_Last.Y*Gyro_Gr,GYRO_Last.Z*Gyro_Gr,
// ACC_Last.X,ACC_Last.Y,ACC_Last.Z,magtmp.hx,magtmp.hy,magtmp.hz);
//---横滚、俯仰角度----------------------------------------------------------------------------------------------
/*
// roll这里Roll和Pitch有些书上说的正好相反 这里理清：ACC_X对应Roll
*/
Roll_Raw_Rad = asin(-2 * Quat.Q1 * Quat.Q3 + 2 * Quat.Q0* Quat.Q2);
Pitch_Raw_Rad = atan2(2 * Quat.Q2 * Quat.Q3 + 2 * Quat.Q0 * Quat.Q1, -2 * Quat.Q1 * Quat.Q1 - 2 * Quat.Q2* Quat.Q2 + 1); // ptich
// Yaw_Raw_Rad = atan2(2 * Quat.Q1 * Quat.Q2 + 2 * Quat.Q0 * Quat.Q3, -2 * Quat.Q2*Quat.Q2 - 2 * Quat.Q3* Quat.Q3 + 1);
//--更新相关三角函数值-------------------------------------------------------
CosPitch = cos(Pitch_Raw_Rad);
SinPitch = sin(Pitch_Raw_Rad);
CosRoll = cos(Roll_Raw_Rad);
SinRoll = sin(Roll_Raw_Rad);
Yaw_Raw_Rad = Mag_Yaw_Cal(&Magdecp); //单位弧度
Yaw_Raw = Yaw_Raw_Rad*57.3f;
Roll_Raw = Roll_Raw_Rad*57.3f;
Pitch_Raw = Pitch_Raw_Rad*57.3f;
if(Roll_Raw<=(-180)||Roll_Raw>=(180)) Roll_Raw = Roll_Last;
if(Pitch_Raw<=(-180)||Pitch_Raw>=(180)) Pitch_Raw = Pitch_Last;
Roll_Last = Roll_Raw;
Pitch_Last = Pitch_Raw;
Yaw_Last = Yaw_Raw;
Control_proc_att.roll = Roll_Last;
Control_proc_att.pitch= Pitch_Last;
AHRS_SpeedUpdate();
}
