四轴--源码分析--001
//传送数据给匿名四轴上位机软件(V2.6版本) //fun:功能字. 0XA0~0XAF //data:数据缓存区,最多28字节!! //len:data区有效数据个数 void usart1_niming_report(u8 fun,u8*data,u8 len) { u8 send_buf[32]; u8 i; if(len>28)return; //最多28字节数据 send_buf[len+3]=0; //校验数置零 send_buf[0]=0X88; //帧头 send_buf[1]=fun; //功能字 send_buf[2]=len; //数据长度 for(i=0;i<len;i++)send_buf[3+i]=data[i]; //复制数据 for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; //计算校验和 for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); //发送数据到串口1 }
为什么数据缓存区最多28字节
答：send_buf[]有32字节，字节0：存放帧头0x88，字节1：存放功能fun，字节2：存放数据长度，最后一个字节（不一定是字节31）存放校验数，也就是前面所有字节数据之和。
32-1-1-1-1 = 28 。
//发送加速度传感器数据和陀螺仪数据 //aacx,aacy,aacz:x,y,z三个方向上面的加速度值 //gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值 void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz) { u8 tbuf[12]; 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; usart1_niming_report(0XA1,tbuf,12);//自定义帧,0XA1 }
将陀螺仪和加速度计的读取数据，不加处理，通过串口1发送到上位机。陀螺仪和加速度计的三个方向都是16位的ADC，需要(3+3)*2=12字节存储。
tbuf[0]存放高八位，tbuf[1]存放低八位，类推。。。
原始数据的来源如下
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
//通过串口1上报结算后的姿态数据给电脑 //aacx,aacy,aacz:x,y,z三个方向上面的加速度值 //gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值 //roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度 //pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度 //yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度 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 }
上报计算好的姿态数据给上位机（同时也把原始数据上传上去了）
姿态数据来自DMP，函数如下
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多) //pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0° //roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0° //yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0° //返回值:0,正常 // 其他,失败 u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw) { float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; unsigned long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1; /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units. * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent. **/ /*if (sensors & INV_XYZ_GYRO ) send_packet(PACKET_TYPE_GYRO, gyro); if (sensors & INV_XYZ_ACCEL) send_packet(PACKET_TYPE_ACCEL, accel); */ /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30. * The orientation is set by the scalar passed to dmp_set_orientation during initialization. **/ if(sensors&INV_WXYZ_QUAT) { q0 = quat[0] / q30; //q30格式转换为浮点数 q1 = quat[1] / q30; q2 = quat[2] / q30; q3 = quat[3] / q30; //计算得到俯仰角/横滚角/航向角 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw }else return 2; return 0; }
具体上报给上位机的方法
usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
分别*100,100,10，这就是为什么
//roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度 //pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度 //yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度
全部源码
#include "stdio.h" #include "sys.h" #include "delay.h" #include "led.h" #include "key.h" #include "ili93xx.h" #include "malloc.h" #include "usart1.h" #include "mpu6050.h" #include "inv_mpu.h" #include "inv_mpu_dmp_motion_driver.h" //串口1发送1个字符 //c:要发送的字符 void usart1_send_char(u8 c) { while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); //循环发送,直到发送完毕 USART_SendData(USART1,c); } //传送数据给匿名四轴上位机软件(V2.6版本) //fun:功能字. 0XA0~0XAF //data:数据缓存区,最多28字节!! //len:data区有效数据个数 void usart1_niming_report(u8 fun,u8*data,u8 len) { u8 send_buf[32]; u8 i; if(len>28)return; //最多28字节数据 send_buf[len+3]=0; //校验数置零 send_buf[0]=0X88; //帧头 send_buf[1]=fun; //功能字 send_buf[2]=len; //数据长度 for(i=0;i<len;i++)send_buf[3+i]=data[i]; //复制数据 for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; //计算校验和 for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); //发送数据到串口1 } //发送加速度传感器数据和陀螺仪数据 //aacx,aacy,aacz:x,y,z三个方向上面的加速度值 //gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值 void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz) { u8 tbuf[12]; 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; usart1_niming_report(0XA1,tbuf,12);//自定义帧,0XA1 } //通过串口1上报结算后的姿态数据给电脑 //aacx,aacy,aacz:x,y,z三个方向上面的加速度值 //gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值 //roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度 //pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度 //yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度 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 } int main(void) { u8 t=0,report=1; //默认开启上报 u8 key; float pitch,roll,yaw; //欧拉角 short aacx,aacy,aacz; //加速度传感器原始数据 short gyrox,gyroy,gyroz; //陀螺仪原始数据 short temp; //温度 NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级 uart1_init(500000); //串口初始化为500000 delay_init(); //延时初始化 LED_Init(); //初始化与LED连接的硬件接口 KEY_Init(); //初始化按键 TFTLCD_Init(); //初始化LCD MPU_Init(); //初始化MPU6050 POINT_COLOR=RED; //设置字体为红色 LCD_ShowString(30,50,200,16,16,"WarShip STM32"); LCD_ShowString(30,70,200,16,16,"MPU6050 TEST"); LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK"); LCD_ShowString(30,110,200,16,16,"2015/1/17"); while(mpu_dmp_init()) { LCD_ShowString(30,130,200,16,16,"MPU6050 Error"); delay_ms(200); LCD_Fill(30,130,239,130+16,WHITE); delay_ms(200); } LCD_ShowString(30,130,200,16,16,"MPU6050 OK"); LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF"); POINT_COLOR=BLUE;//设置字体为蓝色 LCD_ShowString(30,170,200,16,16,"UPLOAD ON "); LCD_ShowString(30,200,200,16,16," Temp: . C"); LCD_ShowString(30,220,200,16,16,"Pitch: . C"); LCD_ShowString(30,240,200,16,16," Roll: . C"); LCD_ShowString(30,260,200,16,16," Yaw : . C"); while(1) { key=KEY_Scan(0); if(key==KEY0_PRES) { report=!report; if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON "); else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF"); } 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(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10)); if((t%10)==0) { if(temp<0) { LCD_ShowChar(30+48,200,'-',16,0); //显示负号 temp=-temp; //转为正数 }else LCD_ShowChar(30+48,200,' ',16,0); //去掉负号 LCD_ShowNum(30+48+8,200,temp/100,3,16); //显示整数部分 LCD_ShowNum(30+48+40,200,temp%10,1,16); //显示小数部分 temp=pitch*10; if(temp<0) { LCD_ShowChar(30+48,220,'-',16,0); //显示负号 temp=-temp; //转为正数 }else LCD_ShowChar(30+48,220,' ',16,0); //去掉负号 LCD_ShowNum(30+48+8,220,temp/10,3,16); //显示整数部分 LCD_ShowNum(30+48+40,220,temp%10,1,16); //显示小数部分 temp=roll*10; if(temp<0) { LCD_ShowChar(30+48,240,'-',16,0); //显示负号 temp=-temp; //转为正数 }else LCD_ShowChar(30+48,240,' ',16,0); //去掉负号 LCD_ShowNum(30+48+8,240,temp/10,3,16); //显示整数部分 LCD_ShowNum(30+48+40,240,temp%10,1,16); //显示小数部分 temp=yaw*10; if(temp<0) { LCD_ShowChar(30+48,260,'-',16,0); //显示负号 temp=-temp; //转为正数 }else LCD_ShowChar(30+48,260,' ',16,0); //去掉负号 LCD_ShowNum(30+48+8,260,temp/10,3,16); //显示整数部分 LCD_ShowNum(30+48+40,260,temp%10,1,16); //显示小数部分 t=0; LED0=!LED0;//LED闪烁 } } t++; } }
