热门标签
热门文章
- 1mysql 创建学生表、课程表、学生选课表_学生表选课表课程表
- 2使用pandas的compare()方法做excel数据对比_pandas两个表格对比
- 3国内外大语言模型调研（更新到2023.09.12）_书生浦语内测
- 4【网安AIGC专题10.11】论文1：生成式模型GPT\CodeX填充式模型CodeT5\INCODER+大模型自动程序修复(生成整个修复函数、修复代码填充、单行代码生产、生成的修复代码排序和过滤)_codex+codet
- 5面试官问你MyBatis SQL是如何执行的？把这篇文章甩给他_mybatis实现了sql语句与代码分离
- 6LINUX【网络编程】UDP程序绑定发送主机IP及端口
- 7libVLC 提取视频帧使用QWidget渲染
- 84条出路，35岁高龄程序员也能实现职业发展与退休规划！_45岁程序员的出路
- 9网络安全应急响应有哪些相关知识？_网络安全灾难恢复是从损失中恢复,这种响应灾难的方式是主动还是被动
- 10文献速递：深度学习肝脏肿瘤诊断---基于深度学习的肝细胞结节性病变在整片组织病理图像上的分类
当前位置: article > 正文
【STM32F4系列】【HAL库】【模块介绍】MPU6050设置与DMP库使用_mpu6050 dmp
作者：知新_RL | 2024-04-11 09:56:05
赞
踩
mpu6050 dmp
概述
MPU6050是一个3轴陀螺仪(测角加速度)和3轴加速度计(测量线加速度)的测量芯片
内部自带运算单元(DMP),可以输出经姿态融合计算后的四元数(一种表示旋转的方法)
而且MPU6050的价格较低(10r以下),常被用于精度不高的场合作为姿态感知的芯片
如经典项目平衡车,某年电赛题目风力摆等
MPU6050可以获取的数据为3轴的角加速度和三轴加速度,为了得到平常使用的欧拉角或者四元数,需要根据这些数据进行姿态解算
可以在单片机内部进行姿态解算,如使用 卡尔曼滤波 但是这样会占用大量单片机资源,因此常用MPU6050自带的运算单元来进行解算
注意,本文的代码借是将 正点原子,大鱼电子,DMP的官方库 进行了整合修改,并非100%原创
注意,本文的代码借是将 正点原子,大鱼电子,DMP的官方库 进行了整合修改,并非100%原创
注意,本文的代码借是将 正点原子,大鱼电子,DMP的官方库 进行了整合修改,并非100%原创
硬件设计
使用官方数据手册给出的电路图进行设计,实测可以使用
要注意CPOUT(20脚)的电容,这是个电荷泵电容,一定要使用2.2nF,不然会出现不稳定
MPU6050使用标准I2C通信,建议在I2C加外部上拉电阻(也可以将单片机内设为开漏上拉输出)
6,7号引脚连接的是地磁计的用于修正偏航角(Yaw),同样是I2C通信
封装是比较难焊的QFN-24,建议使用风枪或者加热台焊接(用烙铁太折磨人了)
这是对应的角加速度和加速度的方向,以及解算后的欧拉角
另外因为硬件原因,偏航角(Yaw)会出现偏移,这是无可避免的,可以通过外接地磁计改善
软件设计
I2C通信
MPU6050的I2C从器件地址是依据AD0(9号引脚)的电平而变化的
AD0引脚为低电平(通过10K电阻接地)则地址是 0x68 << 1 = 0xD0 (也就是常说的除最低位外的地址)
AD0引脚为高电平则地址是 0x69 << 1 = 0xD2 (也就是常说的除最低位外的地址)
最后一位和标准I2C一样是根据读写来确定的
地址要特别注意,容易出错
I2C协议没什么好说的,请看这篇博客
通过宏定义来选择硬件和软件I2C
/* 硬件I2C模式 需要： 1.I2C I2C (默认设置) 标准模式 时钟频率100kHz 地址长度7bit 不用填写设备地址 取消下方注释 */ // extern I2C_HandleTypeDef hi2c2; // #define MPU6050_I2C_Handle hi2c2 // #define MPU6050_Hardware_I2C /* 软件I2C模式 需要： 1.GPIO 2个 均为开漏输出（上不上拉取决于外部电路） 最高等级 取消下方注释,按照自己的管脚更改即可 */ #define MPU6050_Software_I2C #ifdef MPU6050_Software_I2C #define I2C_Group_SCL GPIOA // I2C的时钟GPIO组号 #define I2C_SCL GPIO_PIN_0 // I2C时钟的GPIO端口号 #define I2C_Group_SDA GPIOA // I2C的数据GPIO组号 #define I2C_SDA GPIO_PIN_1 // I2C数据的GPIO端口号 #define I2C_Write_SCL(x) HAL_GPIO_WritePin(I2C_Group_SCL, I2C_SCL, x) #define I2C_Write_SDA(x) HAL_GPIO_WritePin(I2C_Group_SDA, I2C_SDA, x) #define I2C_Read_SCL() HAL_GPIO_ReadPin(I2C_Group_SCL, I2C_SCL) #define I2C_Read_SDA() HAL_GPIO_ReadPin(I2C_Group_SDA, I2C_SDA) #endif
- 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
#include "mpuiic.h" #if defined(MPU6050_Software_I2C) /** * @brief 一段延迟 * @param 无 * @return 无 * @author HZ12138 * @date 2022-07-27 08:53:30 */ void I2C_Delay() { int z = 0xff; while (z--) ; } /** * @brief 产生I2C起始信号 * @param 无 * @return 无 * @author HZ12138 * @date 2022-07-27 08:54:48 */ void I2C_Start(void) { I2C_Write_SDA(GPIO_PIN_SET); //需在SCL之前设定 I2C_Write_SCL(GPIO_PIN_SET); // SCL->高 I2C_Delay(); //延时 I2C_Write_SDA(GPIO_PIN_RESET); // SDA由1->0,产生开始信号 I2C_Delay(); //延时 I2C_Write_SCL(GPIO_PIN_RESET); // SCL->低 } /** * @brief 产生I2C结束信号 * @param 无 * @return 无 * @author HZ12138 * @date 2022-07-27 08:57:03 */ void I2C_End(void) { I2C_Write_SDA(GPIO_PIN_RESET); //在SCL之前拉低 I2C_Write_SCL(GPIO_PIN_SET); // SCL->高 I2C_Delay(); //延时 I2C_Write_SDA(GPIO_PIN_SET); // SDA由0->1,产生结束信号 I2C_Delay(); //延时 } /** * @brief 发送应答码 * @param ack:0 应答 1 不应达 * @return 无 * @author HZ12138 * @date 2022-07-27 09:03:38 */ void IIC_Send_ACK(uint8_t ack) { if (ack == 1) I2C_Write_SDA(GPIO_PIN_SET); //产生应答电平 else I2C_Write_SDA(GPIO_PIN_RESET); I2C_Delay(); I2C_Write_SCL(GPIO_PIN_SET); //发送应答信号 I2C_Delay(); //延时至少4us I2C_Write_SCL(GPIO_PIN_RESET); //整个期间保持应答信号 } /** * @brief 接受应答码 * @param 无 * @return 应答码 0 应答 1 不应达 * @author HZ12138 * @date 2022-07-27 09:04:28 */ uint8_t IIC_Get_ACK(void) { uint8_t ret; //用来接收返回值 I2C_Write_SDA(GPIO_PIN_SET); //电阻上拉,进入读 I2C_Delay(); I2C_Write_SCL(GPIO_PIN_SET); //进入应答检测 I2C_Delay(); //至少延时4us ret = I2C_Read_SDA(); //保存应答信号 I2C_Write_SCL(GPIO_PIN_RESET); return ret; } /** * @brief I2C写1Byte * @param dat:1Byte数据 * @return 应答结果 0 应答 1 不应达 * @author HZ12138 * @date 2022-07-27 09:05:14 */ uint8_t I2C_SendByte(uint8_t dat) { uint8_t ack; for (int i = 0; i < 8; i++) { // 高在前低在后 if (dat & 0x80) I2C_Write_SDA(GPIO_PIN_SET); else I2C_Write_SDA(GPIO_PIN_RESET); I2C_Delay(); I2C_Write_SCL(GPIO_PIN_SET); I2C_Delay(); //延时至少4us I2C_Write_SCL(GPIO_PIN_RESET); dat <<= 1; //低位向高位移动 } ack = IIC_Get_ACK(); return ack; } /** * @brief I2C读取1Byte数据 * @param ack:应答 0 应答 1 不应达 * @return 接受到的数据 * @author HZ12138 * @date 2022-07-27 09:06:13 */ uint8_t I2C_ReadByte(uint8_t ack) { uint8_t ret = 0; // OLED_Read_SDA() 设置输入方向 I2C_Write_SDA(GPIO_PIN_SET); for (int i = 0; i < 8; i++) { ret <<= 1; I2C_Write_SCL(GPIO_PIN_SET); I2C_Delay(); // 高在前低在后 if (I2C_Read_SDA()) { ret++; } I2C_Write_SCL(GPIO_PIN_RESET); I2C_Delay(); } IIC_Send_ACK(ack); return ret; } #endif /** * @brief MUP6050 I2C连续写 * @param addr:器件地址 * @param reg:寄存器地址 * @param len:长度 * @param buf:缓冲区地址 * @return 状态 0成功 其他失败 * @author HZ12138 * @date 2022-08-08 15:47:11 */ uint8_t MPU_Write_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { #if defined(MPU6050_Software_I2C) uint8_t i; I2C_Start(); I2C_SendByte((addr << 1) | 0); //发送器件地址+写命令 I2C_SendByte(reg); //写寄存器地址 for (i = 0; i < len; i++) { I2C_SendByte(buf[i]); //发送数据 } I2C_End(); return 0; #elif defined(MPU6050_Hardware_I2C) HAL_I2C_Mem_Write(&MPU6050_I2C_Handle, MPU_WRITE, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff); return 0; #endif } /** * @brief MUP6050 I2C连续读 * @param addr:器件地址 * @param reg:寄存器地址 * @param len:长度 * @param buf:缓冲区地址 * @return 状态 0成功 其他失败 * @author HZ12138 * @date 2022-08-08 15:47:11 */ uint8_t MPU_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { #if defined(MPU6050_Software_I2C) I2C_Start(); I2C_SendByte((addr << 1) | 0); //发送器件地址+写命令 I2C_SendByte(reg); //写寄存器地址 I2C_Start(); I2C_SendByte((addr << 1) | 1); //发送器件地址+读命令 while (len) { if (len == 1) *buf = I2C_ReadByte(1); //读数据,发送nACK else *buf = I2C_ReadByte(0); //读数据,发送ACK len--; buf++; } I2C_End(); //产生一个停止条件 return 0; #elif defined(MPU6050_Hardware_I2C) HAL_I2C_Mem_Read(&MPU6050_I2C_Handle, MPU_READ, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff); HAL_Delay(1); return 0; #endif } /** * @brief MUP6050 I2C写一个字节 * @param reg:寄存器地址 * @param data:数据 * @return 状态 0成功 其他失败 * @author HZ12138 * @date 2022-08-08 15:47:11 */ uint8_t MPU_Write_Byte(uint8_t reg, uint8_t data) { #if defined(MPU6050_Software_I2C) I2C_Start(); I2C_SendByte((MPU_ADDR << 1) | 0); //发送器件地址+写命令 I2C_SendByte(reg); //写寄存器地址 I2C_SendByte(data); //发送数据 I2C_End(); return 0; #elif defined(MPU6050_Hardware_I2C) return HAL_I2C_Mem_Write(&MPU6050_I2C_Handle, (MPU_ADDR << 1), reg, 1, &data, 1, 0xfff); #endif } /** * @brief MUP6050 I2C写一个字节 * @param reg:寄存器地址 * @return 读取到的数据 * @author HZ12138 * @date 2022-08-08 15:47:11 */ uint8_t MPU_Read_Byte(uint8_t reg) { #if defined(MPU6050_Software_I2C) uint8_t res; I2C_Start(); I2C_SendByte((MPU_ADDR << 1) | 0); //发送器件地址+写命令 I2C_SendByte(reg); //写寄存器地址 I2C_Start(); I2C_SendByte((MPU_ADDR << 1) | 1); //发送器件地址+读命令 res = I2C_ReadByte(1); //读取数据,发送nACK I2C_End(); //产生一个停止条件 return res; #elif defined(MPU6050_Hardware_I2C) uint8_t zj; HAL_I2C_Mem_Read(&MPU6050_I2C_Handle, (MPU_ADDR << 1), reg, 1, &zj, 1, 0xfff); return zj; #endif }
- 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
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
- 210
- 211
- 212
- 213
- 214
- 215
- 216
- 217
- 218
- 219
- 220
- 221
- 222
- 223
- 224
- 225
- 226
- 227
- 228
- 229
- 230
- 231
- 232
- 233
- 234
- 235
- 236
- 237
- 238
- 239
- 240
- 241
- 242
- 243
- 244
- 245
- 246
- 247
- 248
- 249
- 250
- 251
特别注意:使用软件I2C时请注意延迟函数,不要过快(通信失败),不要过慢(通信时间长)
void I2C_Delay()
MPU6050设置
这部分是借鉴自正点原子的内容
#include "mpu6050.h" #include "delay.h" #include "usart.h" #include "i2c.h" #include "Print.h" /** * @brief 初始化MPU6050 * @param 无 * @return 状态 0成功 其他失败 * @author HZ12138 * @date 2022-08-08 14:51:59 */ uint8_t MPU_Init(void) { MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X80); //复位MPU6050 HAL_Delay(100); MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X00); //唤醒MPU6050 MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps MPU_Set_Accel_Fsr(0); //加速度传感器,±2g MPU_Set_Rate(50); //设置采样率50Hz MPU_Write_Byte(MPU_INT_EN_REG, 0X00); //关闭所有中断 MPU_Write_Byte(MPU_USER_CTRL_REG, 0X00); // I2C主模式关闭 MPU_Write_Byte(MPU_FIFO_EN_REG, 0X00); //关闭FIFO MPU_Write_Byte(MPU_INTBP_CFG_REG, 0X80); // INT引脚低电平有效 MPU_Read_Byte(MPU_DEVICE_ID_REG); MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X01); //设置CLKSEL,PLL X轴为参考 MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0X00); //加速度与陀螺仪都工作 MPU_Set_Rate(200); //设置采样率为50Hz return 0; } //设置MPU6050陀螺仪传感器满量程范围 // fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps //返回值:0,设置成功 // 其他,设置失败 uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr) { return MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3); //设置陀螺仪满量程范围 } //设置MPU6050加速度传感器满量程范围 // fsr:0,±2g;1,±4g;2,±8g;3,±16g //返回值:0,设置成功 // 其他,设置失败 uint8_t MPU_Set_Accel_Fsr(uint8_t fsr) { return MPU_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3); //设置加速度传感器满量程范围 } //设置MPU6050的数字低通滤波器 // lpf:数字低通滤波频率(Hz) //返回值:0,设置成功 // 其他,设置失败 uint8_t MPU_Set_LPF(uint16_t lpf) { uint8_t 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); //设置数字低通滤波器 } //设置MPU6050的采样率(假定Fs=1KHz) // rate:4~1000(Hz) //返回值:0,设置成功 // 其他,设置失败 uint8_t MPU_Set_Rate(uint16_t rate) { uint8_t 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); //自动设置LPF为采样率的一半 } //得到温度值 //返回值:温度值(扩大了100倍) short MPU_Get_Temperature(void) { uint8_t buf[2]; short raw; float temp; MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf); raw = ((uint16_t)buf[0] << 8) | buf[1]; temp = 36.53 + ((double)raw) / 340; return temp * 100; ; } //得到陀螺仪值(原始值) // gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号) //返回值:0,成功 // 其他,错误代码 uint8_t MPU_Get_Gyroscope(short *gx, short *gy, short *gz) { uint8_t buf[6], res; res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf); if (res == 0) { *gx = ((uint16_t)buf[0] << 8) | buf[1]; *gy = ((uint16_t)buf[2] << 8) | buf[3]; *gz = ((uint16_t)buf[4] << 8) | buf[5]; } return res; } //得到加速度值(原始值) // gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号) //返回值:0,成功 // 其他,错误代码 uint8_t MPU_Get_Accelerometer(short *ax, short *ay, short *az) { uint8_t buf[6], res; res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf); if (res == 0) { *ax = ((uint16_t)buf[0] << 8) | buf[1]; *ay = ((uint16_t)buf[2] << 8) | buf[3]; *az = ((uint16_t)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
- 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
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
比较常用的是
short MPU_Get_Temperature(void)获取温度
uint8_t MPU_Get_Gyroscope(short *gx, short *gy, short *gz)获取角加速度
uint8_t MPU_Get_Accelerometer(short *ax, short *ay, short *az)获取线加速度
这是MPU6050的初始化函数,将器件ID的校验去掉了(因为我这边有个品牌的芯片读取ID不正确但可以正常使用)
在程序开始时调用即可
uint8_t MPU_Init(void)
DMP设置
这部分是来自官方库
DMP部分是不开源的,不然也不需要使用官方库了
需要更改
inv_mpu.c里的几个API,关于时间,I2C和打印信息的部分
需要将
int mpu_init(void)函数(在inv_mpu.c)中的这段注释并改成这个(光标选中的)
重写读取欧拉角和初始化函数,主要是去掉了几个验证
/** * @brief 初始化DMP * @param 无 * @return 状态 0成功 其他失败 * @author HZ12138 * @date 2022-08-08 14:50:05 */ uint8_t mpu_dmp_init(void) { uint8_t res = 0; 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; } /** * @brief 得到DMP处理后的欧拉角 * @param pitch:俯仰角( -90° - 90° ) * @param roll:横滚角( -180° - 180° ) * @param yaw:航向角( -180° - 180° ) * @return 状态 0成功 其他失败 * @author HZ12138 * @date 2022-08-08 14:46:44 */ uint8_t 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
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
其他部分来自大鱼电子的移植库,无其他修改了
常用的函数就2个
DMP初始化函数
uint8_t mpu_dmp_init(void)
获取欧拉角函数
uint8_t mpu_dmp_get_data(float *pitch, float *roll, float *yaw)
注意
1 通过DMP解算出的欧拉角顺序是 Z-Y-X (这个很重要)
2.因为没有地磁计,所以z(yow)的零点漂移会很严重,基本上没法看
3.mpu6050是相对位置,是基于上电时的位置计算的(但是好像DMP有自己的修正算法,可以让数据几乎保持绝对)
4.尽量在水平位置上电,即使dmp有修正也会出现10°以上的误差
成品
声明：本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：【wpsshop博客】
推荐阅读
相关标签
