内容介绍
内容介绍
一、项目信息
该项目为一个IMU的设计，主要功能是输出IMU产品基本的六轴数据以及四元数、欧拉角。
该项目通过核心板和底板叠加的结构，实现不同种通信方式的数据输出，目前包括串口输出以及CAN输出。
二、硬件介绍
1.核心板包括单片机、电源、传感器三部分。
单片机选用的是STM32F103T8U6，具体资源详见下图。
电源选择的是MIC5219，5V输入3.3V输出，供给单片机和传感器。
传感器选用的是MPU6050，该MPU的DMP引擎十分方便。具体的原理图详见附件。
2.底板目前有两块，一块是USB转串口板，另一块是CAN输出板。
其中USB转串口板，是选用的CH340E芯片。
CAN输出板，是选用的美信的MAX3051EKA和ADI的ADP1710JZ-3.3V芯片，该两芯片原理图如下 。
三、软件介绍
1.该IMU是使用MPU6050自身的DMP引擎，通过导入MPU的三轴加速度和三轴角速度数据，实现四元数的处理输出，最终计算输出欧拉角，基本程序如下所示。
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //ÉèÖÃNVICÖжϷÖ×é2:2λÇÀÕ¼ÓÅÏȼ¶£¬2λÏìÓ¦ÓÅÏȼ¶
uart_init(921600); //´®¿Ú³õʼ»¯Îª500000
delay_init(); //ÑÓʱ³õʼ»¯
delay_ms(2000);
MPU_Init(); //³õʼ»¯MPU6050
delay_ms(1000);
GPIO_SetBits(GPIOA,GPIO_Pin_6);//ָʾµÆÃð
while(mpu_dmp_init())
{
delay_ms(200);
GPIO_ResetBits(GPIOA,GPIO_Pin_6);
delay_ms(200);
GPIO_SetBits(GPIOA,GPIO_Pin_6);
}
printf("DMP Initialized Successfully! \n");
printf("All Devices Initialized Successfully!\n");
while(1)
{
uart_read();
// ii++;
// MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //µÃµ½¼ÓËٶȴ«¸ÐÆ÷Êý¾Ý
// MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //µÃµ½ÍÓÂÝÒÇÊý¾Ý
// printf("aacx= %d \n",aacx);
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)//DMPÊý¾Ý»ñÈ¡
{
// temp=MPU_Get_Temperature(); //µÃµ½Î¶ÈÖµ
// MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //µÃµ½¼ÓËٶȴ«¸ÐÆ÷Êý¾Ý
// MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //µÃµ½ÍÓÂÝÒÇÊý¾Ý
if(filter == 1)
{
//**ÒÔÏÂΪÊý¾ÝÂ˲¨³ÌÐò**//
for(u8 ii=5;ii>0;ii--)//Êý¾ÝÁ÷¶¯
{
AX_New[ii] = AX_New[ii-1];
AY_New[ii] = AY_New[ii-1];
AZ_New[ii] = AZ_New[ii-1];
}
//**×îÐÂÊý¾Ý´æÈëÊý×飬Êý¾ÝºóÈë**//
AX_New[0] = accel_x;
AY_New[0] = accel_y;
AZ_New[0] = accel_z;
//**µ±Ç°Êµ¼Ê²ÎÊýÁбíÊýÖµ**//
for(u8 ii=0 ; ii<6 ; ii++)
{
AX_Old[ii] = AX_New[ii];
AY_Old[ii] = AY_New[ii];
AZ_Old[ii] = AZ_New[ii];
}
//**ðÅÝÅÅÐò**//
bubbleSort(AX_Old,6);
bubbleSort(AY_Old,6);
bubbleSort(AZ_Old,6);
accel_x = (AX_Old[2] + AX_Old[3])/2;
accel_y = (AY_Old[2] + AY_Old[3])/2;
accel_z = (AZ_Old[2] + AZ_Old[3])/2;
if(accel_z>0)accel_z = accel_z*(0.965);
accel_x = (accel_x - 400)/16384*9.8;
accel_y = (accel_y + 200)/16384*9.8;
accel_z = (accel_z )/16384*9.8;
gyro_x = gyro_x/16.4;
gyro_y = gyro_y/16.4;
gyro_z = gyro_z/16.4;
// printf("AZ[0]= %d \n",AY_Old[0]);
// printf("AZ[1]= %d \n",AY_Old[1]);
// printf("AZ[2]= %d \n",AY_Old[2]);
// printf("AZ[3]= %d \n",AY_Old[3]);
// printf("AZ[4]= %d \n",AY_Old[4]);
// printf("AZ[5]= %d \n",AY_Old[5]);
// printf("NOW= %d \n",aacy);
}
if(report==1)Data_Send();
else if(report==2)//Êý¾ÝͨÐÅ
{
// RS232_Q(Quat1,Quat2,Quat3,Quat4); //ËÄÔªÊýÖúÊÖÏÔʾ
// RS232_Angle(pitch,roll,yaw); //Å·À½ÇÖúÊÖÏÔʾ
RS232_Accel(accel_x,accel_y,accel_z);
// RS232_Accel(aacx-751,aacy+500,(aacz-1000)/16384*9.8);
// RS232_Gyro(gyro_x,gyro_y,gyro_z);
}
else if(report==3)//ÄäÃûÉÏλ»ú
{
//if(1)mpu6050_send_data(accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z);//ÓÃ×Ô¶¨ÒåÖ¡·¢ËͼÓËٶȺÍÍÓÂÝÒÇÔʼÊý¾Ý
if(1)usart1_report_imu(accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
Test_Send(accel_x*1000,accel_y*1000,accel_z*1000,gyro_x*10,gyro_y*10,gyro_z*10,roll*100,pitch*100,yaw*100,Quat1*10000,Quat2*10000,Quat3*10000,Quat4*10000);
}
}
}
}2.除此之外，该IMU还选用了匿名上位机作为测试工具，因此在软件中增加匿名上位机的通信程序，程序内容如下：
void usart1_report_imu(short aacx, short aacy, short aacz, short gyrox, short gyroy, short gyroz, short roll, short pitch, short yaw)
{
u8 tbuf[28];
u8 i;
for (i = 0;i<28;i++)tbuf[i] = 0;//Çå0
tbuf[0] = (aacx >> 8) & 0XFF;
tbuf[1] = aacx & 0XFF;
tbuf[2] = (aacy >> 8) & 0XFF;
tbuf[3] = aacy & 0XFF;
tbuf[4] = (aacz >> 8) & 0XFF;
tbuf[5] = aacz & 0XFF;
tbuf[6] = (gyrox >> 8) & 0XFF;
tbuf[7] = gyrox & 0XFF;
tbuf[8] = (gyroy >> 8) & 0XFF;
tbuf[9] = gyroy & 0XFF;
tbuf[10] = (gyroz >> 8) & 0XFF;
tbuf[11] = gyroz & 0XFF;
tbuf[18] = (roll >> 8) & 0XFF;
tbuf[19] = roll & 0XFF;
tbuf[20] = (pitch >> 8) & 0XFF;
tbuf[21] = pitch & 0XFF;
tbuf[22] = (yaw >> 8) & 0XFF;
tbuf[23] = yaw & 0XFF;
usart1_niming_report(0XAF, tbuf, 28);//·É¿ØÏÔʾ֡,0XAF
}
/**
**********************************
* @brief ´«ËÍÊý¾Ý¸øÄäÃûµØÃæÕ¾Èí¼þ(V4.0°æ±¾)
* @param data:Êý¾Ý»º´æÇø,×î¶à28×Ö½Ú!!
len:dataÇøÓÐЧÊý¾Ý¸öÊý
* @retval None
**********************************
*/
void ANO_DT_Send_Data(u8 *data, u8 len)
{
u8 i;
if (len>28)return; //×î¶à28×Ö½ÚÊý¾Ý
for (i = 0;i<len;i++)usart1_send_char(data[i]); //·¢ËÍÊý¾Ýµ½´®¿Ú1
}
/**
**********************************
* @brief ·¢ËͼÓËٶȴ«¸ÐÆ÷Êý¾ÝºÍÍÓÂÝÒÇÊý¾Ý
* @param aacx,aacy,aacz:x,y,zÈý¸ö·½ÏòÉÏÃæµÄ¼ÓËÙ¶ÈÖµ
gyrox,gyroy,gyroz:x,y,zÈý¸ö·½ÏòÉÏÃæµÄÍÓÂÝÒÇÖµ
* @retval None
**********************************
*/
void ANO_DT_Test_Send(s16 a_x,s16 a_y,s16 a_z,s16 g_x,s16 g_y,s16 g_z)
{
u8 _cnt=0;
vs16 _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
data_to_send[_cnt++]=0;
_temp = a_x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = a_y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = a_z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = g_x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = g_y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = g_z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
void Fifo_Buffer_Send()
{
u8 tbuf[31];
u8 i,t;
for (i = 0;i<31;i++)tbuf[i] = 0;//Çå0
tbuf[0] = 0xa5;
tbuf[1] = 0x0a;
tbuf[2] = fifo_buffer[0];
tbuf[3] = fifo_buffer[1];
tbuf[4] = fifo_buffer[4];
tbuf[5] = fifo_buffer[5];
tbuf[6] = fifo_buffer[8];
tbuf[7] = fifo_buffer[9];
tbuf[8] = fifo_buffer[12];
tbuf[9] = fifo_buffer[13];
tbuf[10] = fifo_buffer[16];
tbuf[11] = fifo_buffer[17];
tbuf[12] = fifo_buffer[20];
tbuf[13] = fifo_buffer[21];
tbuf[14] = fifo_buffer[24];
tbuf[15] = fifo_buffer[25];
tbuf[16] = fifo_buffer[28];
tbuf[17] = fifo_buffer[29];
tbuf[18] = fifo_buffer[32];
tbuf[19] = fifo_buffer[33];
tbuf[20] = fifo_buffer[36];
tbuf[21] = fifo_buffer[37];
tbuf[22] = 0x0d;
tbuf[23] = 0x0a;
for(t=0;t<31;t++)
{
USART_ClearFlag(USART1, USART_FLAG_TC);
USART_SendData(USART1,tbuf[t]);
while((USART_GetFlagStatus(USART1, USART_FLAG_TC))== RESET);//µÈ´ý·¢ËͽáÊø
}
}
void Test_Send(short aacx, short aacy, short aacz, short gyrox, short gyroy, short gyroz, short roll, short pitch, short yaw, short Quat1, short Quat2, short Quat3, short Quat4)
{
u8 tbuf[31];
u8 i,t;
for (i = 0;i<31;i++)tbuf[i] = 0;//Çå0
tbuf[0]=0xa5;
tbuf[1]=0x0a;
tbuf[2]=0x01;
tbuf[3] = (aacx >> 8) & 0XFF;
tbuf[4] = aacx & 0XFF;
tbuf[5] = (aacy >> 8) & 0XFF;
tbuf[6] = aacy & 0XFF;
tbuf[7] = (aacz >> 8) & 0XFF;
tbuf[8] = aacz & 0XFF;
tbuf[9] = (gyrox >> 8) & 0XFF;
tbuf[10] = gyrox & 0XFF;
tbuf[11] = (gyroy >> 8) & 0XFF;
tbuf[12] = gyroy & 0XFF;
tbuf[13] = (gyroz >> 8) & 0XFF;
tbuf[14] = gyroz & 0XFF;
tbuf[15] = (roll >> 8) & 0XFF;
tbuf[16] = roll & 0XFF;
tbuf[17] = (pitch >> 8) & 0XFF;
tbuf[18] = pitch & 0XFF;
tbuf[19] = (yaw >> 8) & 0XFF;
tbuf[20] = yaw & 0XFF;
tbuf[21] = (Quat1 >> 8) & 0XFF;
tbuf[22] = Quat1 & 0XFF;
tbuf[23] = (Quat2 >> 8) & 0XFF;
tbuf[24] = Quat2 & 0XFF;
tbuf[25] = (Quat3 >> 8) & 0XFF;
tbuf[26] = Quat3 & 0XFF;
tbuf[27] = (Quat4 >> 8) & 0XFF;
tbuf[28] = Quat4 & 0XFF;
tbuf[29]=0x0d;
tbuf[30]=0x0a;
for (i = 0;i<31;i++)usart1_send_char(tbuf[i]); //·¢ËÍÊý¾Ýµ½´®¿Ú1
// for(t=0;t<31;t++)
// {
// USART_ClearFlag(USART1, USART_FLAG_TC);
// USART_SendData(USART1,tbuf[t]);
// while((USART_GetFlagStatus(USART1, USART_FLAG_TC))== RESET);//µÈ´ý·¢ËͽáÊø
// }
}
四、功能测试
该IMU的功能测试包括两部分：
1.测试欧拉角的稳定性，保证达到0.1度的稳定。
2.测试偏航角的零偏稳定性，测试其在一小时内的零漂角度。
功能测试利用的是匿名上位机，通过该上位机，可以直观地观测出上述稳定性情况。
五、后续
在视频里有说，MPU6050的DMP解算出的RPY数据，R和P是非常稳定的，但是Y的零漂是非常严重的，这里可以引入地磁计的修正，后续又设计了MPU6050+HMC5883L版本和MPU9250版本的核心板，程序也都调试完毕，引入地磁后，ROLL和PITCH的角度数据不如无地磁时候，但是YAW的零漂情况得到了有效遏制。
MPU6050+HMC5883L采用AHRS算法，地磁标定完成后，可以基本做到12小时零漂2度左右；MPU9250采用的MPL库处理，调试后yaw可以做到12小时漂移0.7度。除此之外，为了对照这几款IMU的性能，我还购买了市面上百元以内的几款IMU，性能与AHRS算法相近，不如MPU9250的MPL。
当然也是因为我对AHRS的调试不够好，如果有大佬对这方面有更多经验，欢迎联系我（请一定要联系我。。。）。
哦对了，由于MPU已经停产，我有选择了TDK的新款芯片ICM-20948和ICM-40605，如果有使用过这两款芯片的大佬也请联系我（救救我吧今天直接没读出数来，可能又买到残次品了，说到残次品淘宝上惯性模块的次品率太高了，最近买了十几款芯片，有1/3都读不出数据）
软硬件
电路图
附件下载
IMU_6050_Core.SchDoc
iMU_Carrier_Plate_CAN.SchDoc
iMU_Carrier_Plate.SchDoc
团队介绍
个人团队，自己做着玩
评论
0 / 100
查看更多
