- 1设计模式-组合模式_组合设计模式菜鸟
- 2程序员的八个级别_程序员最高职位
- 3程序员的十个层次 你属于哪一层？
- 4kafka可视化管理工具环境搭建windows+linux【EFAK v3.0.1】_kafka-eagle 3.0.1端口
- 5优选算法——双指针
- 6基于python的旅游数据分析可视化系统
- 7ERROR: Error installing json:The 'json' native gem requires installed build tools.
- 8【command not found:nvm】Mac M1解决安装完nvm后，依旧报错：zsh: command not found:nvm。亲测有效_m1 zsh: command not found
- 9Android 系统界面增强自动亮度调节功能_安卓亮度调节及界面分析
- 10电脑能ping通却打不开网页_ping通但是不能访问
STM32 使用DMP库处理MPU6050数据_mpu6050库函数
赞
踩
0、系列目录
1、实验分析
MPU6050包含一个三轴陀螺仪，三轴加速度计，并且可以通过AUX_CL和AUX_DA再扩展一个磁力计，内部设有一个可扩展的数字运动处理器DMP，可以将欧拉角以四元数的形式输出。本次实验我们使用DMP库对MPU6050初始化并且通过DMP中的库函数读取加速度值和陀螺仪值显示在LCD上
2、实验前准备
使用DMP库需要在MPU官方下载固件并将下面六个导入工程
并且在固件库中需要进行一定的设定。
在inv_mpu.c文件中预留有调用文件的接口，需要我们提前设定一下
首先
#define MPU6050 //定义我们使用的传感器为MPU6050
#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F1)
其次
要对接接口函数
/* The following functions must be defined for this platform: 1. i2c_write(unsigned char slave_addr, unsigned char reg_addr, 2. unsigned char length, unsigned char const *data) 3. i2c_read(unsigned char slave_addr, unsigned char reg_addr, 4. unsigned char length, unsigned char *data) 5. delay_ms(unsigned long num_ms) 6. get_ms(unsigned long *count) 7. reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin) 8. labs(long x) 9. fabsf(float x) 10. min(int a, int b) */ #define i2c_write MPU_Write_Len #define i2c_read MPU_Read_Len #define delay_ms delay_ms #define get_ms mget_ms #define log_i printf //打印信息 #define log_e printf //打印信息
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
按照官方注释的要求，接口函数的入口参数必须按照要求对齐，否则将无法使用DMP库函数。
3、实验分析
上一次我们编写了软模拟IIC的程序，本次需要增加IIC连续写和连续读，这样我们才可以直接操作MPU中的寄存器。
操作步骤：
IIC初始化-》MPU的初始化-》DMP库初始化-》使用库函数读取数据
（1）MPU初试步骤
- 设置AD0引脚电平（低电平，从机地址为0x68；高电平，从机地址为0x69）
- IIC初始化
- 复位从机，唤醒从机
- 设置陀螺仪、加速度计、设置采样速率
- 关闭所有中断、IIC主模式关闭、关闭FIFO
- 设置INT引脚低电平有效
- 判断从机地址是否正确
- 设置正方向
- 使能陀螺仪和加速度计
4、MPU6050寄存器分析
1、自检寄存器
MPU自检寄存器，会产生自检反馈：STR=Gyroscope Output with Self Test Enabled — Gyroscope Output with Self Test Disabled。可以判断自检成功or失败
2、采样频率分频器
传感器寄存器输出， FITO 输出， DMP 采样， Motion 检测， Zero Motion 检测和 Free Fall检测都基于这个采样频率。
采样频率=陀螺仪输出频率/（ 1+SMPLRT_DIV）
陀螺仪的输出频率是1KHz或者8KHz，与数字低通滤波器（DLPF）有关，当DLPF_CFG=0/7时，频率为8K，其他为1K，而且 DLPF 滤波频率一般设置为采样率的一半。采样率，我们假定设置为 50Hz，那么SMPLRT_DIV=1000/50-1=19。
3、陀螺仪配置寄存器
这个寄存器我们只需要设置FS_SEL位，用于设置陀螺仪满量程范围：0，±250°/s；1，±500°/s；2，±1000°/s；3，±2000°/s
陀螺仪为16位的ADC，所以灵敏度可以达到65536/4000=16.4LSB/(° /S)。
4、加速度计配置寄存器
和陀螺仪一样，只需要设置AFS_SEL位，0，±2g； 1，±4g； 2，±8g； 3，±16g；我们一般设置为 0，即±2g，因为加速度传感器的 ADC,也是 16 位，所以得到灵敏度为： 65536/4=16384LSB/g。
5、FIFO使能寄存器
该寄存器用于控制 FIFO 使能，在简单读取传感器数据的时候，可以不用 FIFO，设置对应位为 0 即可禁止 FIFO，设置为 1，则使能 FIFO。注意加速度传感器的 3 个轴，全由 1 个位（ACCEL_FIFO_EN）控制，只要该位置 1，则加速度传感器的三个通道都开启 FIFO。
6、配置寄存器
这里的DLPF_CFG位就是数字低通滤波器的设置位
一般我们设置角速度传感器的带宽为其采样率的一半，比如前面设置的采样率为100Hz，那么带宽就应该设置50Hz，取表中角速度近似值42Hz，就应该设置DLPF_CFG=011;
7、电源管理寄存器1
DEVICE_RESET 位用来控制复位，设置为 1，复位 MPU6050，复位结束后， MPU硬件自动清零该位。
SLEEEP 位用于控制 MPU6050 的工作模式，复位后，该位为 1，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式。
TEMP_DIS 用于设置是否使能温度传感器，设置为 0，则使能。
CLKSEL[2:0]用于选择系统时钟源
内部8M晶振，不太准确，一般采用X轴的PLL作为时钟源
8、电源管理寄存器2
LP_WAKE_CTRL 用于控制低功耗时的唤醒频率，用不到。
剩下的 6 位，分别控制加速度和陀螺仪的 x/y/z 轴是否进入待机模式，这里我们全部都不进入待机模式，所以全部设置为 0 即可。
9、数据输出寄存器
陀螺仪数据输出寄存器，总共有 6 个寄存器组成，
地址为： 0X43~0X48，
通过读取这 6 个寄存器，就可以读到陀螺仪 x/y/z 轴的值，
比如 x 轴的数据，可以通过读取 0X43（高 8 位）和 0X44（低 8 位）寄存器得到，其他轴以此类推。
同样，加速度传感器数据输出寄存器，也有 6 个，地址为： 0X3B~0X40，
通过读取这 6 个寄存器，就可以读到加速度传感器 x/y/z 轴的值，
比如读 x 轴的数据，可以通过读取 0X3B（高8 位）和 0X3C（低 8 位）寄存器得到，其他轴以此类推。
10、温度传感器
温度传感器的值，可以通过读取 0X41（高 8 位）和 0X42（低 8 位）寄存器得到，温度换算公式为：
Temperature = 36.53 + regval/340
Temperature 为计算得到的温度值，单位为℃， regval 为从 0X41 和 0X42 读到的温度传感器值。
5、代码示例
1、读写数据
/** * 功能：IIC写一个字节 * 入口参数：reg，寄存器地址；data，数据 * 返回值：0，正常；1，错误 */ u8 MPU_Write_Byte(u8 reg,u8 data) { MPU_IIC_Start(); MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//ADDR左移一位+写操作 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; } /** * 功能：IIC读一个字节 * 入口参数：reg，寄存器地址 * 返回值：数据 */ 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; } /** * 功能：IIC连续写 * 入口参数：addr，外设地址；reg，寄存器地址；len，写入长度；buf，数据 * 返回值：0，正常；1，错误 */ 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; } /** * 功能：IIC连续读 * 入口参数：addr，外设地址；reg，寄存器地址；len，读取长度；buf，储存数据 * 返回值：0,正常；1，错误 */ 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; }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
2、陀螺仪初始化代码
/** * 功能：初始化MPU6050 * 入口参数：无 * 返回值：0，成功；其他，错误 */ u8 MPU_Init(void) { u8 res; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA,ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA,&GPIO_InitStructure); GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁止JTAG，使PA15变为普通IO MPU_AD0_CTRL = 0;//控制MPU6050的AD0为低电平，从机地址=0x68； // 高电平， 0x69； 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);//陀螺仪传感器 +-2000dps MPU_Set_Accel_Fsr(0);//加速度传感器 +-2g MPU_Set_Rate(50); MPU_Write_Byte(MPU_INT_EN_REG,0x00);//关闭所有中断 MPU_Write_Byte(MPU_USER_CTRL_REG,0x00);//IIC主模式关闭 MPU_Write_Byte(MPU_FIFO_EN_REG,0x00);//关闭FIFO MPU_Write_Byte(MPU_INTBP_CFG_REG,0x80);//INT低电平有效 res = MPU_Read_Byte(MPU_DEVICE_ID_REG); if(res == MPU_ADDR) { MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x01);//设置CLKSEL PLL X轴为参考 MPU_Write_Byte(MPU_PWR_MGMT2_REG,0x00);//加速度计和陀螺仪使能 MPU_Set_Rate(50); } else return 1; return 0; } /** * 功能：设置陀螺仪传感器满量程范围 * 入口参数：fsr,0,+-250dps * 1,+-500dps * 2,+-1000dps * 3,+-2000dps * 返回值：0,成功；1，失败 */ u8 MPU_Set_Gyro_Fsr(u8 fsr) { return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3); } /** * 功能：设置加速度传感器满量程范围 * 入口参数：fsr，0,+-2g * 1,+-4g * 2,+-8g * 3,+-16g * 返回值：0,成功；1，失败 */ u8 MPU_Set_Accel_Fsr(u8 fsr) { return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3); } /** * 功能：设置数字低通滤波器 * 入口参数：lpf，低通滤波器频率（Hz） * 返回值：0,成功；1，失败 */ 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); } /** * 功能：设置采样率（Fs=1KHz） * 入口参数：rate=[4,1000]Hz * 返回值：0,设置成功；1，设置失败 */ 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); }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
3、获取原始数据代码
/** * 功能：获取MPU内部温度 * 入口参数：无 * 返回值：温度*100 */ 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; } /** * 功能：获得陀螺仪原始值 * 入口参数：gx,gy,gz,陀螺仪XYZ轴原始数据（带符号） * 返回值：0，成功；1，失败 */ 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; } /** * 功能：获得加速度原始值 * 入口参数：ax,ay,az,加速度计XYZ的原始数据（） * 返回值：0，正常；1，错误 */ 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; }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
4、DMP库函数调用
//mpu6050,dmp初始化 //返回值:0,正常 // 其他,失败 u8 mpu_dmp_init(void) { u8 res=0; MPU_IIC_Init(); //初始化IIC总线 if(mpu_init()==0) //初始化MPU6050 { res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器 if(res)return 1; res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO if(res)return 2; res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率 if(res)return 3; res=dmp_load_motion_driver_firmware(); //加载dmp固件 if(res)return 4; res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向 if(res)return 5; res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能 DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO| DMP_FEATURE_GYRO_CAL); if(res)return 6; res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz) if(res)return 7; res=run_self_test(); //自检 if(res)return 8; res=mpu_set_dmp_state(1); //使能DMP if(res)return 9; }else return 10; return 0; } //得到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; }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
6、实验现象
7、总结
对于MPU6050数据的处理，使用DMP库是一种比较简单的办法，如果理论扎实，可以自行读取原始数据，通过四元数解算获取欧拉角。下一次我们将会把陀螺仪得到的数据发送到上位机，下次主要分析上位机的通讯协议。
