void Get_Attitude(void)
{
IMUupdate(MPU6050_GYRO_LAST.X*Gyro_Gr, //四元数算法！！！
MPU6050_GYRO_LAST.Y*Gyro_Gr,
MPU6050_GYRO_LAST.Z*Gyro_Gr,
ACC_AVG.X,ACC_AVG.Y,ACC_AVG.Z); //*0.0174转成弧度
}
void main()
{
int i2c_file;
int i;
unsigned char val;
short accel[3]={0};
short gyro[3]={0};
if ((i2c_file = open(I2C_FILE_NAME, O_RDWR)) < 0) {
perror("Unable to open i2c control file");
exit(1);
}
mpu6050_init(i2c_file);
initkalman();
while(1)
{
getmotion6(i2c_file,&accel[0],&accel[1],&accel[2],&gyro[0],&gyro[1],&gyro[2]);
kalman((float)gyro[0],(float)gyro[1],(float)gyro[2],(float)accel[0],(float)accel[1],(float)accel[2]);
IMUupdate((float)predictdata[3],(float)predictdata[4],(float)predictdata[5],(float)predictdata[0],(float)predictdata[1],(float)predictdata[2]);
//printf("accel = %d ,%d ,%d ,gyro = %d ,%d ,%d\n",accel[0],accel[1],accel[2],gyro[0],gyro[1],gyro[2]);
//printf("q0 = %f,q1 = %f,q2 = %f,q3 = %f\n",q0,q1,q2,q3);
printf("Pitch = %f,Roll = %f,Yaw = %f\n",Pitch,Roll,Yaw);
}
for(i=0;i<=0x75;i++)
{
get_i2c_register(i2c_file,0x68,i,&val);
printf("addr %x= %x\n",i,val);
}
}
void Duty_5ms()
{
float outer_loop_time;
outer_loop_time = Get_Cycle_T(2); //获取外环准确的执行周期
test[2] = GetSysTime_us()/1000000.0f;
/*IMU更新姿态。输入：半个执行周期，三轴陀螺仪数据（转换到度每秒），三轴加速度计数据（4096--1G）；输出：ROLPITYAW姿态角*/
IMUupdate(0.5f *outer_loop_time,mpu6050.Gyro_deg.x, mpu6050.Gyro_deg.y, mpu6050.Gyro_deg.z, mpu6050.Acc.x, mpu6050.Acc.y, mpu6050.Acc.z,&Roll,&Pitch,&Yaw);
CTRL_2( outer_loop_time ); // 外环角度控制。输入：执行周期，期望角度（摇杆量），姿态角度；输出：期望角速度。<函数未封装>
test[3] = GetSysTime_us()/1000000.0f;
}
void ANGLE_Update(void) // 360бу = 2ж╨ 2000бу = 11.111...ж╨ = 34.906585 32768
{
static float t_roll = .0, t_pitch = .0, t_yaw = .0, temp;
GYRO.x = GYRO.x / 32768.0 * 34.906585;
GYRO.y = GYRO.y / 32768.0 * 34.906585;
GYRO.z = GYRO.z / 32768.0 * 34.906585;
IMUupdate(GYRO.x, GYRO.y, GYRO.z, ACC.x, ACC.y, ACC.z);
t_roll = atan2f(2.0f*(q0*q1+q2*q3), 1-2.0f*(q1*q1+q2*q2));
temp = 2.0f*(q0*q2-q1*q3);
temp = (temp > 1.0)? 1.0: temp;
temp = (temp < -1.0)? -1.0: temp;
t_pitch = asinf(temp);
t_yaw = atan2f(2.0f*(q1*q2-q0*q3), 2.0f*(q0*q0+q1*q1)-1);
ANGLE.roll = t_roll * 57.295780;
ANGLE.pitch = t_pitch * 57.295780;
ANGLE.yaw = t_yaw * 57.295780;
}
//*****TIM2中断函数2ms*****//
void TIM2_IRQHandler(void)
{
if(TIM_GetITStatus(TIM2,TIM_IT_Update) == SET)//检测TIM2溢出中断是否发生
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
time2_tick++;
time2_tick = time2_tick%1000;//2HZ
//LED0(ON);
//START////////////////////数据读取+姿态解算+PID控制 1.4ms////////////////////////////
MPU6050_READ();//读取
acc[0] = MPU6050_ACC_LAST.X;
acc[1] = MPU6050_ACC_LAST.Y;
acc[2] = MPU6050_ACC_LAST.Z;
gyro[0] = MPU6050_GYRO_LAST.X - GYRO_OFFSET.X;
gyro[1] = -(MPU6050_GYRO_LAST.Y - GYRO_OFFSET.Y);
gyro[2] = MPU6050_GYRO_LAST.Z - GYRO_OFFSET.Z;
if(gyro[2]>=-5 &&gyro[2]<=5) gyro[2] = 0;//Z轴陀螺仪,漂移处理
//printf("%d,%d,%d",acc[0],acc[1],acc[2]);
IMUupdate(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2],Vaule_2_Gyro());//非全姿态角解算
//IMUupdate1(number_to_dps1(gyro[0]),number_to_dps1(gyro[1]),number_to_dps1(gyro[2]),acc[0],acc[1],acc[2]);
//IMUupdate2(number_to_dps1(gyro[0]),number_to_dps1(gyro[1]),number_to_dps1(gyro[2]),acc[0],acc[1],acc[2]);
CONTROL(Q_ANGLE.ROLL,Q_ANGLE.PITCH,Q_ANGLE.YAW,Value_2_Thr(),Value_2_Roll(),Value_2_Pitch(),Vaule_2_Gyro());//对电机进行PID控制
//CONTROL1(Q_ANGLE.ROLL,Q_ANGLE.PITCH,Q_ANGLE.YAW,Value_2_Thr(),Value_2_Roll(),Value_2_Pitch(),Vaule_2_Gyro(), acc, gyro);
//END///////////////////////////////////////////////////////////////////////
//LED0(OFF);
if(time2_tick%5 == 0)//10ms -- 100HZ进行一次
{
}
if(time2_tick%25 == 0)//50ms -- 20HZ进行一次
{
DMA_DATA_SEND_FLAG = 1;//上传数据
}
if(time2_tick%50 == 0)//100ms -- 10HZ进行一次
{
}
if(time2_tick%500 == 0)//1000ms -- 1HZ进行一次
{
if(ARMED == 1)
{
Is_DisArmed(RC_CH[3-1],RC_CH[4-1]);//是否加锁
}
else
{
Is_Armed(RC_CH[3-1],RC_CH[4-1]);//是否解锁
}
}
time2_led++;
if(time2_led>=1000) time2_led=time2_led-1000;
// time2_led = time2_led%1000;//2ms一个周期
switch(led_state)
{
case 0:Led_Flash0();break;
case 1:Led_Flash1();break;
case 2:Led_Flash2();break;
case 3:Led_Flash3();break;
}
}
}
int main(void)
{
static u8 att_cnt=0;
static u8 rc_cnt=0;
static T_int16_xyz mpu6050_dataacc1,mpu6050_dataacc2,mpu6050_datagyr1,mpu6050_datagyr2;
static u8 senser_cnt=0,status_cnt=0,dt_rc_cnt=0,dt_moto_cnt=0;
SYS_INIT();
while (1)
{
if(FLAG_ATT) //1ms
{
FLAG_ATT = 0;
att_cnt++;
rc_cnt++;
if(att_cnt==1)
MPU6050_Dataanl(&mpu6050_dataacc1,&mpu6050_datagyr1); //2ms
else
{
att_cnt = 0;
MPU6050_Dataanl(&mpu6050_dataacc2,&mpu6050_datagyr2);
Acc.X = (mpu6050_dataacc1.X+mpu6050_dataacc2.X)/2;
Acc.Y = (mpu6050_dataacc1.Y+mpu6050_dataacc2.Y)/2;
Acc.Z = (mpu6050_dataacc1.Z+mpu6050_dataacc2.Z)/2;
Gyr.X = (mpu6050_datagyr1.X+mpu6050_datagyr2.X)/2;
Gyr.Y = (mpu6050_datagyr1.Y+mpu6050_datagyr2.Y)/2;
Gyr.Z = (mpu6050_datagyr1.Z+mpu6050_datagyr2.Z)/2;
Prepare_Data(&Acc,&Acc_AVG); //加速度滤波
IMUupdate(&Gyr,&Acc_AVG,&Att_Angle); //IMU姿态解算
Control(&Att_Angle,&Gyr,&Rc_D,Rc_C.ARMED); //2ms
if(Rc_C.ARMED)
LED3_ON
else
LED3_OFF
senser_cnt++;
status_cnt++;
dt_rc_cnt++;
dt_moto_cnt++;
if(senser_cnt==5) //10ms
{
senser_cnt = 0;
Send_Senser = 1;
}
if(status_cnt==5) //10ms
{
status_cnt = 0;
Send_Status = 1;
}
if(dt_rc_cnt==10) //20ms
{
dt_rc_cnt=0;
Send_RCData = 1;
}
if(dt_moto_cnt==10) //20ms
{
dt_moto_cnt=0;
Send_MotoPwm = 1;
LED2_OFF
}
}
}
}
