void usart1_send_char(u8 c) { while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); USART_SendData(USART1,c); } void usart1_niming_report(u8 fun,u8*data,u8 len) { u8 send_buf[32]; u8 i; if(len>28)return; send_buf[len+3]=0 ; send_buf[0]=0XAA; send_buf[1]=0XAA; send_buf[2]=fun; send_buf[3]=len; for(i=0;i<len;i++)send_buf[4+i]=data; for(i=0;i<len+4;i++)send_buf[len+4]+=send_buf; for(i=0;i<len+5;i++)usart1_send_char(send_buf); } void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz) { u8 tbuf[18]; 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[12]=0; tbuf[13]=0; tbuf[14]=0; tbuf[15]=0; tbuf[16]=0; tbuf[17]=0; usart1_niming_report(0X02,tbuf,18); } void usart1_report_imu(short roll,short pitch,short yaw) { u8 tbuf[18]; tbuf[0]=(roll>>8)&0XFF; tbuf[1]=roll&0XFF; tbuf[2]=(pitch>>8)&0XFF; tbuf[3]=pitch&0XFF; tbuf[4]=(yaw>>8)&0XFF; tbuf[5]=yaw&0XFF; tbuf[6]=0; tbuf[7]=0; tbuf[8]=0; tbuf[9]=0; tbuf[10]=0; tbuf[11]=0; tbuf[12]=0; tbuf[13]=0; tbuf[14]=0; tbuf[15]=0; tbuf[16]=0; tbuf[17]=0; usart1_niming_report(0X01,tbuf,18); } if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0) { temp=MPU_Get_Temperature(); //μÃμ½Î¶èÖμ MPU_Get_Accelerometer(&aacx,&aacy,&aacz); MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);¡ if(report)usart1_report_imu((int)(roll*100),(int)(pitch*100),(int)(yaw*100));
|