大二时设计过一款角速度测量装置 ,用于测量特定物品的角速度,现整理下。实现效果是 旋转物体开始旋转到停止时,收到上位机信号,将测量数据传给上位机,上位机分析后得出一条角速度曲线。
引言
本项目是基于stm32f103c8t6单片机的角速度测量装置及上位机的开发。目前市场上测量小型旋转轴的角速度的传感器量程相对较小,普遍在300°/sec上下,远远不够满足当前的需求。因此开发了这种小型装置用于测量相对快速的旋转轴的角速度。
下位机设计 :主控stm32,数据通过mpu6050采集数存到Sram23LC1024,上位机需要数据时,发个命令数据从Sram中读出通过蓝牙传到上位机。
上位机设计:接收下位机的数据并进行数据分析得出角速度,汇出相关波形图像。
方案选择
方案一:
Mpu6050 原始数据读取,比较简单精准。传感器上传的原始数据就为三轴角速度和三轴加速度 直接获取Z轴角速度即可
但其最大量程为正负2000度每秒,转速最大约5.56圈每秒,实验几次速度一加大就会超量程。
优点:速度快,测速精准,且运算时间短
缺点:量程不够大
方案二:
通过姿态融合解算出当前板子yaw轴角度,每隔一段时间测量一次,获取角度差,除上对应时间差得出角速度,当时间差足够小时(最小20ms)可近似看成瞬时角速度。
优点:量程允许 理论最大可测角速度为(360/0.02=18000度每秒)
缺点:姿态融合算法复杂 ,较一方案误差稍大
一、基本电路原理
1、stm32最小系统电路
stm32最小系统电路 包括晶振电路,复位电路,下载电路。我还在其中加了电源电压检测电路,用于实时检测电源电压
- 电源电路
因为该装置需要体积小巧,所以我采用了纽扣电池2032供电。一节纽扣电池为3v,整个系统需要3.3v供电,所以我采用两节2032电池串联6v 用LMS1117-3.3稳压至3.3v供电。
- MPU6050外围电路
用于测量角速度
4、蓝牙外围电路
蓝牙外围电路由于蓝牙要配对,加之其电流需求较大。为了配对和调试方便,我将蓝牙电源和其他电源分离。
5、SRAM外围电路
用于拓展内存,快速存取数据加快响应时间,该模块为SPI接口,SRAM_CSN为片选端
6、LED指示灯模块
用于提示指示作用
- PCB布线布局
确保传输不受影响,蓝牙天线下必须不走线 不铺地甚至镂空。
高频走线旁边可放置接地过孔
正面布线
反面布线
正面全局
背面全局
- 下位机程序编写及分析
- 程序流程图
- SRAM驱动编程
本项目为了加快传输速度和扩大内存,选用了外扩SRAM,具体型号为23LC1024。是一款支持SPI(SDI,SQI)串行口的储存芯片。该芯片有三种读写模式 分别为连续读写(默认),字节读写,和页读写。本项目为了增加速度,选用SPI连续读写模式。以下为其时序图:
SPI采用stm32硬件spi接口 速度18M 以下为spi配置:
#include "spi.h"
SPI_InitTypeDef SPI_InitStructure;
//初始化SPI
void SPI1_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_SPI1,ENABLE);
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStructure);
GPIO_SetBits(GPIOA,GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7);
SPI_InitStructure.SPI_Direction=SPI_Direction_2Lines_FullDuplex;
SPI_InitStructure.SPI_Mode=SPI_Mode_Master;
SPI_InitStructure.SPI_DataSize=SPI_DataSize_8b;
SPI_InitStructure.SPI_CPOL=SPI_CPOL_High;
SPI_InitStructure.SPI_CPHA=SPI_CPHA_2Edge;
SPI_InitStructure.SPI_NSS=SPI_NSS_Soft;
SPI_InitStructure.SPI_BaudRatePrescaler=SPI_BaudRatePrescaler_256;
SPI_InitStructure.SPI_FirstBit=SPI_FirstBit_MSB;
SPI_InitStructure.SPI_CRCPolynomial=7;
SPI_Init(SPI1,&SPI_InitStructure);
SPI_Cmd(SPI1,ENABLE);
SPI1_ReadWriteByte(0xff);
}
//指定速度18M
void SPI1_SetSpeed(u8 SpeedSet)
{
SPI1->CR1&=0XFFC7;
SPI1->CR1|=SpeedSet;
SPI1->CR1|=1<<6;
}
//读写字节
u8 SPI1_ReadWriteByte(u8 TxData)
{
u8 retry=0;
while(SPI_I2S_GetFlagStatus(SPI1,SPI_I2S_FLAG_TXE)==RESET)
{
retry++;
if(retry>200)return 0;
}
SPI_I2S_SendData(SPI1,TxData);
retry=0;
while(SPI_I2S_GetFlagStatus(SPI1,SPI_I2S_FLAG_RXNE)==RESET)
{
retry++;
if(retry>200)return 0;
}
return SPI_I2S_ReceiveData(SPI1);
}
以下为SRAM读写时序配置
:#include "sram.h"
#include "bluetooth.h"
#include "spi.h"
void SRAM_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOA, ENABLE );
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; //SPI CS
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //¸´ÓÃÍÆÍìÊä³ö
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_SetBits(GPIOA,GPIO_Pin_2);
SPI1_Init(); //³õʼ»¯SPI
SPI1_SetSpeed(SPI_BaudRatePrescaler_4); //ÉèÖÃΪ18MʱÖÓ,¸ßËÙģʽ
}
void SRAM_WRByte_Mode()
{
SPI_SRAM_CS=0;
SPI1_ReadWriteByte(WR_MR);
SPI1_ReadWriteByte(BYTE_RW);
SPI_SRAM_CS=1;
}
void SRAM_RWSequ_Mode()
{
SPI_SRAM_CS=0;
SPI1_ReadWriteByte(WR_MR);
SPI1_ReadWriteByte(SEQU_RW);
SPI_SRAM_CS=1;
}
void SRAM_WriteOneByte(u8 WriteDataBuffer,u32 Adr)
{
SRAM_WRByte_Mode();
__NOP();
SPI_SRAM_CS=0;
SPI1_ReadWriteByte(WRITE);
SRAM_Write_Adr(Adr);
SPI1_ReadWriteByte(WriteDataBuffer);
SPI_SRAM_CS=1;
}
u8 SRAM_ReadOneByte(u32 Adr)
{
u8 ReadDataBuffer;
SRAM_WRByte_Mode();
__NOP();
SPI_SRAM_CS=0;
SPI1_ReadWriteByte(READ);
SRAM_Write_Adr(Adr);
ReadDataBuffer=SPI1_ReadWriteByte(0xff);
SPI_SRAM_CS=1;
return ReadDataBuffer;
}
void SRAM_Write_Adr(u32 Addr)
{
SPI1_ReadWriteByte((u8)((Addr)>>16)&0xff);
SPI1_ReadWriteByte((u8)((Addr)>>8)&0xff);
SPI1_ReadWriteByte((u8)(Addr)&0xff);
}
void SRAM_Write_Data(u32 Addr,u8 *pBuffer,u32 WriteBytesNum)
{
u8 i;
SRAM_RWSequ_Mode();
__NOP();
SPI_SRAM_CS=0;
SPI1_ReadWriteByte(WRITE);
SRAM_Write_Adr(Addr);
for(i = 0;i < WriteBytesNum; i++)
{
SPI1_ReadWriteByte(pBuffer[i]);
}
SPI_SRAM_CS=1;
}
void SRAM_Read_Data(u32 Addr,u8 *pBuffer,u32 ReadBytesNum)
{
u8 i;
SRAM_RWSequ_Mode();
__NOP();
SPI_SRAM_CS=0;
SPI1_ReadWriteByte(READ);
SRAM_Write_Adr(Addr);
for(i = 0;i < ReadBytesNum; i++)
{
pBuffer[i] = SPI1_ReadWriteByte(0xff);
}
SPI_SRAM_CS=1;
}
u8 sram_test()
{
SRAM_WriteOneByte(TEST_byte,0);
if(SRAM_ReadOneByte(0)==TEST_byte)
return 0;
else
return 1;
}
- MPU6050编程
本项目传感器,iic接口。原始数据为三轴加速度和三轴角速度。本可以直接三轴角速度采样,但由于量程不够大,所以只能姿态解算出每个时刻的角度测角速度。姿态解算采用官方DMP移植,关于欧拉角和四元数这里就不细细讨论了。以下贴出部分代码
初始化
u8 MPU_Init(void)
{
u8 res;
MPU_IIC_Init();
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);
MPU_Set_Gyro_Fsr(3);
MPU_Set_Accel_Fsr(0);
MPU_Set_Rate(50);
MPU_Write_Byte(MPU_INT_EN_REG,0X00);
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//Æ÷¼þIDÕýÈ·
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);
MPU_Set_Rate(50);
}else return 1;
return 0;
}
量程设置
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);
}
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);
}
低通滤波器设置
u8 MPU_Set_LPF(u16 lpf)
{
u8 data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);
}
采样频率
u8 MPU_Set_Rate(u16 rate)
{
u8 data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);
return MPU_Set_LPF(rate/2);
}
温度值
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((u16)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;;
}
得到原始数据
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((u16)buf[0]<<8)|buf[1];
*ay=((u16)buf[2]<<8)|buf[3];
*az=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
写
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);
if(MPU_IIC_Wait_Ack())
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg);
MPU_IIC_Wait_Ack();
for(i=0;i<len;i++)
{
MPU_IIC_Send_Byte(buf[i]);
if(MPU_IIC_Wait_Ack())
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_Stop();
return 0;
}
读
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);
if(MPU_IIC_Wait_Ack())
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg);
MPU_IIC_Wait_Ack();
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|1);
MPU_IIC_Wait_Ack();
while(len)
{
if(len==1)*buf=MPU_IIC_Read_Byte(0);
else *buf=MPU_IIC_Read_Byte(1);
len--;
buf++;
}
MPU_IIC_Stop(); //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
return 0;
}
u8 MPU_Write_Byte(u8 reg,u8 data)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);
if(MPU_IIC_Wait_Ack())
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg);
MPU_IIC_Wait_Ack();
MPU_IIC_Send_Byte(data);
if(MPU_IIC_Wait_Ack())
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
}
u8 MPU_Read_Byte(u8 reg)
{
u8 res;
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);
MPU_IIC_Wait_Ack();
MPU_IIC_Send_Byte(reg);
MPU_IIC_Wait_Ack();
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);
MPU_IIC_Wait_Ack();
res=MPU_IIC_Read_Byte(0);
MPU_IIC_Stop();
return res;
}
姿态解算
int dmp_read_fifo(short *gyro, short *accel, long *quat,
unsigned long *timestamp, short *sensors, unsigned char *more)
{
unsigned char fifo_data[MAX_PACKET_LENGTH];
unsigned char ii = 0;
/* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
* cache this value and save some cycles.
*/
sensors[0] = 0;
/* Get a packet. */
if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
return -1;
/* Parse DMP packet. */
if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
#ifdef FIFO_CORRUPTION_CHECK
long quat_q14[4], quat_mag_sq;
#endif
quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
((long)fifo_data[2] << 8) | fifo_data[3];
quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
((long)fifo_data[6] << 8) | fifo_data[7];
quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
((long)fifo_data[10] << 8) | fifo_data[11];
quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
((long)fifo_data[14] << 8) | fifo_data[15];
ii += 16;
#ifdef FIFO_CORRUPTION_CHECK
/* We can detect a corrupted FIFO by monitoring the quaternion data and
* ensuring that the magnitude is always normalized to one. This
* shouldn't happen in normal operation, but if an I2C error occurs,
* the FIFO reads might become misaligned.
*
* Let's start by scaling down the quaternion data to avoid long long
* math.
*/
quat_q14[0] = quat[0] >> 16;
quat_q14[1] = quat[1] >> 16;
quat_q14[2] = quat[2] >> 16;
quat_q14[3] = quat[3] >> 16;
quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
(quat_mag_sq > QUAT_MAG_SQ_MAX)) {
/* Quaternion is outside of the acceptable threshold. */
mpu_reset_fifo();
sensors[0] = 0;
return -1;
}
sensors[0] |= INV_WXYZ_QUAT;
#endif
}
if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
ii += 6;
sensors[0] |= INV_XYZ_ACCEL;
}
if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
ii += 6;
sensors[0] |= INV_XYZ_GYRO;
}
/* Gesture data is at the end of the DMP packet. Parse it and call
* the gesture callbacks (if registered).
*/
if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
decode_gesture(fifo_data + ii);
get_ms(timestamp);
return 0;
}
得到姿态角 pitch roll yaw
我们需要的是yaw轴数据
- 串口响应程序
收到 0x0d 0x0a结尾的数据 立即送Sram发送数据。
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
{
Res =USART_ReceiveData(USART1);
if((USART_RX_STA&0x8000)==0)
{
if(USART_RX_STA&0x4000)
if(Res!=0x0a)USART_RX_STA=0;
else
{
USART_RX_STA|=0x8000;
LED5=~LED5;
TIM_Cmd(TIM3,DISABLE);
Send_Data_From_Sram(Data_buffer,i_address);
USART_RX_STA=0;
}
}
else
{
if(Res==0x0d)USART_RX_STA|=0x4000;
else
{
USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
USART_RX_STA++;
if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;
}
}
}
}
- 整体框架及算法
初始化及主函数
先模块初始化 然后在中断中写入数据 直到串口 获得信号 输出数据
u8 Init()
{
u8 Flag_MPU;
TIM3_Int_Init(500,71);
uart_init(115200);
delay_init();
LED_Init();
SRAM_Init();
MPU_Init();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
Flag_MPU=mpu_dmp_init()||sram_test();
return Flag_MPU;
}
int main(void)
{
u8 Flag;
Flag=Init();
while(Flag)
{
LED1=0;
Flag=mpu_dmp_init()||sram_test();
}
while(1)
{
}
}
中断函数
十毫秒采一次样 写入SRAM 数据帧格式为 0xaa 0x 0x 0xbb
short gyrox,gyroy,gyroz;
//u8 tbuf[2];u8 tbuf[5];
u32 i_address;
float pitch,roll,yaw;
int last_yaw,now_yaw,temp_yaw,error_yaw;
void TIM3_IRQHandler(void)
{
static u8 ms10 = 0;
static u16 ms1000=0;
if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) {
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
ms10++;
ms1000++;
if(ms10==20)
{
ms10=0;
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0);
temp_yaw=now_yaw;
now_yaw=(int)(yaw*10000);
last_yaw=temp_yaw;
error_yaw=now_yaw-last_yaw;
if(error_yaw!=0)
{
tbuf[0]=0xaa;
tbuf[1]=(u8)((error_yaw>>16)&0XFF);
tbuf[2]=(u8)((error_yaw>>8)&0XFF);
tbuf[3]=(u8)(error_yaw&0XFF);
tbuf[4]=0xbb;
SRAM_Write_Data(i_address,tbuf,5);
i_address+=5;
if(i_address>=131070)
{
LED3=0; i_address=131070;
}
}
}
if(ms1000==2000)
{
ms1000=0;
LED1=~LED1;
}
}
}
- 上位机程序编写及分析
- 上位机简介
此项目用的上位机是基于c#编写而成,分为串口调试助手和波形显示两部分。波形绘制基于GDI库
串口助手部分
波形显示部分
- 数据处理
下位机上传数据分析
这里的图被word吃掉了 。。。。。。。
0处为初始化位置。
顺时针转0~-180 对应yaw*10000数据为FFFFFF~E488BF 此方向标记为负方向
逆时针转0~180 对应yaw*10000数据为000000~1B7740 次方向标记为正方向
上传数据为 十毫秒间隔的相邻位置差。
负方向数据处理 -((0-差值)&0x00ffffff/10000*100)
正方向数据处理 差值/10000*100
上传数据>7fffff 为负方向数据
<7fffff 为正方向数据
越过1的数据点 自动忽略不计入数据
具体实现代码
if (Data[i + 1]>=0X7f)
buffer = (double)(-((0-(int)((Data[i + 1] << 16) | (Data[i + 2] <<8) | (Data[i + 3])))&0X00FFFFFF)/100.00);
else
buffer = (double)((int)((Data[i + 1] << 16) | (Data[i + 2] << 8) | (Data[i + 3])) / 100.00);
换算出来的数据单位为度每秒
Data_Save 将换算后的数据保存为excel表格导入matlab 出来结果单位为R\min
部分数据
不带纺轮 带纺轮
Matlab代码
A = xlsread('C:\Users\Administrator\Desktop\二类四次.xlsx');
t=A(:,1);
y=A(:,3);
plot(t,y);
hold on;
B = xlsread('C:\Users\Administrator\Desktop\一类三次.xlsx');
t=B(:,1);
y=B(:,3);
plot(t,y);
MATLAB图像
图像解释:
不带纺轮加速度比较大,速度回到0的时间较短而且会正转
带纺轮角速度比较小,可以转的时间比较长而且回到零速度时不太会正转。
最下面的平线 我觉得还是有点问题,可能是超过量程,精度。