热门标签
热门文章
- 1数据工程系列精讲（第四讲）: Data-centric AI 之样本工程
- 2基于Lang-Chain（ChatGLM和ChatChat）知识库大语言模型的部署搭建_langchain 知识库
- 3[论文笔记] Gemini: A Computation-Centric Distributed Graph Processing System_computation centric
- 4漫谈跨境支付架构
- 5flutter：BottomNavigationBar和TabBar_flutter 底部tabbar
- 6springboot-php微信小程序的美食推荐系统ttb19
- 7Linux下解决访问github慢的问题_linux访问github
- 8windows + pytorch + gpu
- 9Springboot - ElasticSearch 查询总结（持续更新）_boolquery.filter
- 10NLP简单项目实战——ChatBOT（一）
当前位置: article > 正文
MPU6050使用心得（简单分享一下）_mpu6050角度值跳跃
作者：小丑西瓜9 | 2024-04-22 06:05:00
赞
踩
mpu6050角度值跳跃
前言
选用MPU6050做 倾斜检测 功能。
前期准备
开发板：正点原子STM32F103 精英版（STM32F103ZET6）
模块：GY-521 MPU6050
其他：杜邦线若干、烧录线、FlyMcu、Keil5、正点原子开发板配套的套件（TFTLCD）
例程、资料下载
源自淘宝卖家：https://pan.baidu.com/share/init?surl=dNDqcp76L9QdM7iSZYfz_A 密码：4eum
GY-521 MPU6050模块 三维角度传感器6DOF三轴加速度计电子陀螺仪\MPU6050六轴角度加速度传感器\11，ATK-MPU6050六轴传感器模块\2，程序源码\（库函数版本，适合精英STM32开发板）实验30 MPU6050六轴传感器实验.rar
我们的例程刚好选择的适配 我们开发版的正点原子例程，虽然这个程序是针对ATK-MPU6050，不过其实2个模块差别不大，可以通用例程，非常方便。
原理图
我们可以看下2个模型的原理图，可以发现ATK-MPU6050只是有几个管脚没有接出来，不使用的话，其实没啥区别
GY-521 MPU6050
ATK-MPU6050
这边的AUX_CL、AUX_DA就没接
引脚描述
SCL、SDA：是连接MCU的IIC接口，MCU通过这个IIC接口来控制MPU6050，此时MPU6050作为一个IIC从机设备，接单片机的I2C_SCL。
XCL、XDA：辅助IIC用来连接其他器件，可用来连接外部从设备，比如磁传感器，这样就可以组成一个九轴传感器，不需要连接单片机。
AD0：地址管脚，可以不接单片机。当MPU6050作为一个IIC从机设备的时候，有8位地址，高7位的地址是固定的，就是WHOAMI寄存器的默认——0x68，最低的一位是由AD0的连线决定的。
AD0接GND时，高8位的最后一位是0，所以iic从机地址是0x68；
AD0接VCC时，高8位的最后一位是1，所以iic从机地址是0x69。
INT：数据输出的中断引脚，可以不接单片机，准备好数据之后,通过中断告诉STM32,从而获取数据。
VCC：接3.3V或5V电源
GND：接地
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
所以ATK-MPU6050这XCL、XDA不接也罢，不影响。
接线
可以参考资料内的ATK-MPU6050六轴传感器模块用户手册_V1.0.pdf，或者直接看例程源码。
VCC -》 3.3V
GND -》 GND
SCL -》 PB10
SDA -》 PB11
AD0 -》 PA15
- 1
- 2
- 3
- 4
- 5
源码关键部分分析
main.c
关注的核心部分就在 while(mpu_dmp_init())，DMP初始化这块，注意会出问题的部分
inv_mpu.c
//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; }
- 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
其中run_self_test()是关键，自检操作会对模块的各角度进行调整到“零度”，所谓的零度，不是真零度，稍后补充。
如果自检这一套不通过，LCD上就会一直跳动MPU6050 Error，当然也许你运气好，直接通过了，也说不准hh
那么按照正常操作，官方demo下，你应该将模块水平地面静止放置，芯片朝上（其实朝下也可以，但是有坑点！！！），然后复位程序（不复位也可以，会死循环跑），顺利的话，你就能看到各个角度值了，当然，细心的你会发现一些端倪，显示的Yaw航向角会跳动，那么这时候，你可以看看这篇文章的讲解：MPU6050常见问题的分析与处理，讲得很好，直击痛点，其实就是硬件问题导致的漂移，需要额外追加磁力计。不过我需要实现的功能只是倾斜检测，所以Yaw航向角可以不做使用，也就不影响了。
那么到了这一步，即使你水平禁止放平了模块，依然无法完成自检，那我们就再继续深入。前情建议：可以换一个模块试试（因为我在深入探究后，最后发现是模块问题）
进入run_self_test(void)，一探究竟
//MPU6050自测试 //返回值:0,正常 // 其他,失败 u8 run_self_test(void) { int result; //char test_packet[4] = {0}; long gyro[3], accel[3]; //float sens; //unsigned short accel_sens; result = mpu_run_self_test(gyro, accel); // 强制自检成功，万不得已的情况下使用 //float sens; //unsigned short accel_sens; /* mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); return 0; */ // 如果加速度和陀螺仪自检通过，就把当前读取到的各个xyz轴数据设置，作为基准值 if (result == 0x3) { /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); return 0; }else return 1; }
- 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
在看到源码后，其实就显而易见了，自检不通过，其实就是mpu_run_self_test函数返回的结果不为0x3，那我们就继续深入mpu_run_self_test函数
/* 这段注释解释了一个触发传感器自检的函数。自检过程会返回一个掩码，用于指示哪些传感器通过了自检，哪些未通过。 其中，第 0 位表示陀螺仪，第 1 位表示加速度计，第 2 位表示磁罗盘。 注释中还提到目前 MPU6500 不支持硬件自检，但仍然可以通过此函数获取加速度计和陀螺仪的偏移。 另外，为了保证自检的准确性，调用此函数时设备应该处于面朝上或面朝下的状态，即 z 轴与重力平行。 函数会返回一个结果掩码，用于表示哪些传感器未通过自检，哪些通过了。 */ /** * @brief Trigger gyro/accel/compass self-test. * On success/error, the self-test returns a mask representing the sensor(s) * that failed. For each bit, a one (1) represents a "pass" case; conversely, * a zero (0) indicates a failure. * * \n The mask is defined as follows: * \n Bit 0: Gyro. * \n Bit 1: Accel. * \n Bit 2: Compass. * * \n Currently, the hardware self-test is unsupported for MPU6500. However, * this function can still be used to obtain the accel and gyro biases. * * \n This function must be called with the device either face-up or face-down * (z-axis is parallel to gravity). 目前，MPU6500 不支持硬件自检。然而，该函数仍可用于获取加速度计和陀螺仪的偏移。 调用此函数时，设备必须处于面朝上或面朝下的状态（z 轴与重力平行）。 * @param[out] gyro Gyro biases in q16 format. * @param[out] accel Accel biases (if applicable) in q16 format. * @return Result mask (see above). */ // 此时的gyro，accel是前一个函数定义的局部变量，没有初始值 int mpu_run_self_test(long *gyro, long *accel) { #ifdef MPU6050 // 原来这个参数为2，堆栈中为0x02，即使改为0x03，在仿真堆栈中任然为0x02 const unsigned char tries = 2; long gyro_st[3], accel_st[3]; unsigned char accel_result, gyro_result; #ifdef AK89xx_SECONDARY unsigned char compass_result; #endif int ii; #endif int result; unsigned char accel_fsr, fifo_sensors, sensors_on; unsigned short gyro_fsr, sample_rate, lpf; unsigned char dmp_was_on; if (st.chip_cfg.dmp_on) { mpu_set_dmp_state(0); dmp_was_on = 1; } else dmp_was_on = 0; /* Get initial settings. */ // 0x07D0 mpu_get_gyro_fsr(&gyro_fsr); // 0x02 mpu_get_accel_fsr(&accel_fsr); // 0x002A mpu_get_lpf(&lpf); // 0x0064 mpu_get_sample_rate(&sample_rate); // 0x78 'x' sensors_on = st.chip_cfg.sensors; // 0x78 'x' mpu_get_fifo_config(&fifo_sensors); /* For older chips, the self-test will be different. */ #if defined MPU6050 for (ii = 0; ii < tries; ii++) if (!get_st_biases(gyro, accel, 0)) break; if (ii == tries) { /* If we reach this point, we most likely encountered an I2C error. * We'll just report an error for all three sensors. */ result = 0; goto restore; } for (ii = 0; ii < tries; ii++) if (!get_st_biases(gyro_st, accel_st, 1)) break; if (ii == tries) { /* Again, probably an I2C error. */ result = 0; goto restore; } printf("accel=%ld, %ld, %ld\r\n", accel[0], accel[1], accel[2]); printf("accel_st=%ld, %ld, %ld\r\n", accel_st[0], accel_st[1], accel_st[2]); printf("gyro=%ld, %ld, %ld\r\n", gyro[0], gyro[1], gyro[2]); printf("gyro_st=%ld, %ld, %ld\r\n", gyro_st[0], gyro_st[1], gyro_st[2]); // 加速度传感器自检 accel_result = accel_self_test(accel, accel_st); // 陀螺仪传感器的自检 gyro_result = gyro_self_test(gyro, gyro_st); printf("accel_result=%d, gyro_result=%d\r\n", accel_result, gyro_result); result = 0; if (!gyro_result) result |= 0x01; if (!accel_result) result |= 0x02; #ifdef AK89xx_SECONDARY compass_result = compass_self_test(); if (!compass_result) result |= 0x04; #endif restore: #elif defined MPU6500 /* For now, this function will return a "pass" result for all three sensors * for compatibility with current test applications. */ get_st_biases(gyro, accel, 0); result = 0x7; #endif /* Set to invalid values to ensure no I2C writes are skipped. */ st.chip_cfg.gyro_fsr = 0xFF; st.chip_cfg.accel_fsr = 0xFF; st.chip_cfg.lpf = 0xFF; st.chip_cfg.sample_rate = 0xFFFF; st.chip_cfg.sensors = 0xFF; st.chip_cfg.fifo_enable = 0xFF; st.chip_cfg.clk_src = INV_CLK_PLL; mpu_set_gyro_fsr(gyro_fsr); mpu_set_accel_fsr(accel_fsr); mpu_set_lpf(lpf); mpu_set_sample_rate(sample_rate); mpu_set_sensors(sensors_on); mpu_configure_fifo(fifo_sensors); if (dmp_was_on) mpu_set_dmp_state(1); return result; }
- 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
进来后其实可以发现，影响result值的其实就是这两个自检函数，两个传感器只要有一个不通过，就GG，同样可以通过返回值判断是哪块的错误。
// 加速度传感器自检
accel_result = accel_self_test(accel, accel_st);
// 陀螺仪传感器的自检
gyro_result = gyro_self_test(gyro, gyro_st);
- 1
- 2
- 3
- 4
当时我的错误是在加速度这块accel_self_test，其实这两块实现也差不多，我们继续深入accel_self_test函数。
/* 用于加速度传感器自检的函数，主要是通过比较两组偏移值来检查传感器是否正常工作。 函数的目标是检测传感器在不同轴向上的偏移值是否在允许的范围内，如果都在范围内，则返回0，表示自检通过。 */ // bias_regular[0] = 0xFFFF14F6, bias_st[0] = 0xFFFFA5DD static int accel_self_test(long *bias_regular, long *bias_st) { int jj, result = 0; float st_shift[3], st_shift_cust, st_shift_var; // 计算加速度传感器的标定偏移 // st_shift[3] = {0.561419249, 0.47499004, 0.60024482} get_accel_prod_shift(st_shift); for(jj = 0; jj < 3; jj++) { // 强制跳过z轴检测 if(2 == jj) break; // 计算当前轴向的自定义偏移，即标定偏移和测试偏移之间的差值除以一个常数 // 第三组数据时，st_shift_cust = 0，0xFFF10000 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; // 如果标定偏移不为零（即不等于零），则进入这个条件判断 if (st_shift[jj]) { // 计算偏移变化，即自定义偏移相对于标定偏移的比例减去1 // test.max_accel_var设置于全局变量，为0.14 // 如果偏移变化的绝对值大于某个阈值 test.max_accel_var，则表示偏移变化超过了允许范围，将当前轴向的位设置到 result 变量中 if (fabs(st_shift_var) > test.max_accel_var) result |= 1 << jj; printf("accel_self_test fabs(st_shift_var)[%d]=%f\r\n", jj, fabs(st_shift_var)); // min_g=0.3， max_g=0.95 } else if ((st_shift_cust < test.min_g) || (st_shift_cust > test.max_g)) { result |= 1 << jj; printf("st_shift_cust[%d]=%f\r\n", jj, st_shift_cust); } } return result; } // 陀螺仪传感器的自检，通过计算偏移值和变化来检测陀螺仪是否在正常工作范围内。如果所有轴向的偏移值都在允许范围内，函数返回0，表示自检通过 // bias_regular[0] = 0xFFFF7447, bias_st[0] = 0x0031289B static int gyro_self_test(long *bias_regular, long *bias_st) { // 定义了整型变量 jj 和 result，用于迭代循环和存储自检结果 int jj, result = 0; // 定义了一个长度为 3 的无符号字符数组 tmp，用于存储临时数据 unsigned char tmp[3]; // 定义了三个浮点数变量 st_shift，st_shift_cust 和 st_shift_var，分别用于存储陀螺仪的标定偏移、自定义偏移和偏移变化 float st_shift, st_shift_cust, st_shift_var; // 从 I2C 设备中读取数据，并将其存储在 tmp 数组中。如果读取失败，返回错误代码 0x07 if (i2c_read(st.hw->addr, 0x0D, 3, tmp)) return 0x07; // 通过按位与操作，将读取的数据中的高三位清零，以获得有效的偏移码 tmp[0] &= 0x1F; tmp[1] &= 0x1F; tmp[2] &= 0x1F; for (jj = 0; jj < 3; jj++) { // 强制跳过z轴检测 if(2 == jj) break; // 计算了自定义偏移和偏移变化，并进行了自检。如果偏移变化超过了阈值，或者自定义偏移值超出了允许的范围，则将当前轴向的位设置到 result 变量中 st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; if (tmp[jj]) { st_shift = 3275.f / test.gyro_sens; // 将 st_shift 初始化为一个计算值。然后根据偏移码逐次进行迭代，每次将 st_shift 乘以 1.046，以计算标定偏移值 while (--tmp[jj]) st_shift *= 1.046f; st_shift_var = st_shift_cust / st_shift - 1.f; if (fabs(st_shift_var) > test.max_gyro_var) { result |= 1 << jj; printf("gyro_self_test fabs(st_shift_var)[%d]=%f\r\n", jj, fabs(st_shift_var)); } } else if ((st_shift_cust < test.min_dps) || (st_shift_cust > test.max_dps)) { result |= 1 << jj; printf("st_shift_cust[%d]=%f\r\n", jj, st_shift_cust); } } return result; }
- 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
可以发现我加了个z轴跳过的部分，大家可以忽略。那么我们来看一看都是什么，其实注释我也加上去了，简单来说就是计算2个偏移值，然后判断是否在限定的正常区间内，不在就会修改result，可以用于判断是那一块出的错，那么一共是3组数据的校验，xyz轴，其实到这里，已经可以定位到具体哪个部分不符合检查标准了，那么你可以通过修改检查区间来降低检查标准，当然可能修改了也仍然不行，那么就只好再进一步了。
我们这个自检的数据源自哪里？往上一级，来自get_st_biases函数。
// 此时传入的gyro accel static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) { unsigned char data[MAX_PACKET_LENGTH]; unsigned char packet_count, ii; unsigned short fifo_count; data[0] = 0x01; data[1] = 0; if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) return -1; delay_ms(200); data[0] = 0; if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) return -1; if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) return -1; if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) return -1; if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) return -1; if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) return -1; // 0x0c data[0] = BIT_FIFO_RST | BIT_DMP_RST; if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) return -1; delay_ms(15); // 0x01 data[0] = st.test->reg_lpf; if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) return -1; // 0x00 data[0] = st.test->reg_rate_div; if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data)) return -1; // 此处在计算2个比较值的地方有所出入 if (hw_test) // 0xE0 data[0] = st.test->reg_gyro_fsr | 0xE0; else data[0] = st.test->reg_gyro_fsr; if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) return -1; // 此处在计算2个比较值的地方有所出入 if (hw_test) // 0XF8 data[0] = st.test->reg_accel_fsr | 0xE0; else data[0] = test.reg_accel_fsr; if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) return -1; // 此处在计算2个比较值的地方有所出入 if (hw_test) delay_ms(200); /* Fill FIFO for test.wait_ms milliseconds. */ // 0x40 data[0] = BIT_FIFO_EN; if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) return -1; // 0x78 data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) return -1; delay_ms(test.wait_ms); data[0] = 0; if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) return -1; // data[0]=0x04 if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) return -1; // 0x0400 fifo_count = (data[0] << 8) | data[1]; // 0x55 packet_count = fifo_count / MAX_PACKET_LENGTH; gyro[0] = gyro[1] = gyro[2] = 0; accel[0] = accel[1] = accel[2] = 0; printf("packet_count=%d\r\n", packet_count); for (ii = 0; ii < packet_count; ii++) { short accel_cur[3], gyro_cur[3]; // 读取寄存器地址st.reg->fifo_r_w数据到data if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data)) return -1; // printf("data[2, 3]=%d, %d\r\n", data[2], data[3]); // printf("data[4, 5]=%d, %d\r\n", data[4], data[5]); accel_cur[0] = ((short)data[0] << 8) | data[1]; accel_cur[1] = ((short)data[2] << 8) | data[3]; accel_cur[2] = ((short)data[4] << 8) | data[5]; accel[0] += (long)accel_cur[0]; accel[1] += (long)accel_cur[1]; accel[2] += (long)accel_cur[2]; gyro_cur[0] = (((short)data[6] << 8) | data[7]); gyro_cur[1] = (((short)data[8] << 8) | data[9]); gyro_cur[2] = (((short)data[10] << 8) | data[11]); gyro[0] += (long)gyro_cur[0]; gyro[1] += (long)gyro_cur[1]; gyro[2] += (long)gyro_cur[2]; } #ifdef EMPL_NO_64BIT gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count); gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count); gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count); if (has_accel) { accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens / packet_count); accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens / packet_count); accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens / packet_count); /* Don't remove gravity! */ accel[2] -= 65536L; } #else /* printf("******* before\r\n"); printf("accel=%ld, %ld, %ld\r\n", accel[0], accel[1], accel[2]); printf("gyro=%ld, %ld, %ld\r\n", gyro[0], gyro[1], gyro[2]); printf("*******\r\n"); */ gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count); gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count); gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count); accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / packet_count); accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / packet_count); accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / packet_count); /* Don't remove gravity! */ printf("******* after\r\n"); printf("accel=%ld, %ld, %ld\r\n", accel[0], accel[1], accel[2]); printf("gyro=%ld, %ld, %ld\r\n", gyro[0], gyro[1], gyro[2]); printf("*******\r\n"); if (accel[2] > 0L) accel[2] -= 65536L; else accel[2] += 65536L; #endif 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
- 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
看到这里，其实数据来源就是通过i2c从模块寄存器中读取数据。那么也将到达我们的尾声了，对应的寄存器是什么内容。参考资料：MPU-6000 & MPU-6050 寄存器表及其描述(中文版).pdf
然后层层上推，找到4.18 S REGISTERS 9 59 TO 4 64 – – ACCELEROMETER MEASUREMENTS
那么此时，通过前面我追加的打印，和我当时模块输出的值，其加速度的第三组数据，也就是Z轴的值不会发现变化，无论我怎么摆放，怎么晃动。其通过计算后，结果为0，然后导致result不为0，最终导致自检永远无法通过，不过强制跳过z轴校验后，其横滚角和俯仰角也勉强看着能用（勉强）。
那么到这，我的判断就是模块损坏导致的这个问题，之后更换模块后，轻松秒杀。。。
既然模块算是测试通过了，我们再深入下，看看 倾斜检测，其实说是模块需要水平静止进行自检，实际上不是水平也可以自检通过，基本上只要静止不动就行，某些角度可能会自检失败，主要问题仍然是加速度z轴这块。不过问题不大，那么这就在最后收一个伏笔，在main中获取到3个角度后，做个简单的区间限定，就能判断是否超出了限定的倾斜阈值了，但是使用中我发现，自检中，Roll横滚角，会有问题！
当你芯片的法线是朝向水平线往上的（朝向天花板），Roll自检后是从0度开始。
当你芯片的法线是朝向水平线往下的（朝向地面），Roll自检后是从-179.9度开始。
坑点出现了，0度左右偏移后是 -1,1。-179.9度左右偏移后是-179,179，另外朝向地面的情况Roll计算会不怎么正常！！！
那么我们最开始限定的区间判断角度在2种自检情况下，就不再适配了。
那么在我们无法限定模块大致朝向的情况下，这个问题影响很大。
那我给出的方案就是，在自检后，通过判断初始的Roll是靠近0 还是 靠近 -180，从而可以得出芯片的朝向，然后适配2种算法做实现，但是仅使用于完全平行地面朝下，斜朝下的话，Roll值读取出来不太正常，整体的角度比例会失调！！！
ps：芯片刚自检完，数据还会波动一会，需要等待一段时间，稳定后，才能正常使用。（尤其是在芯片朝向地面的情况）
需要注意的问题点：
偏航角（yaw）零飘：MPU6050常见问题的分析与处理
万向节锁：欧拉角、万向节死锁理解
其他坑点
拓展
兼容STM32F103C8T6
修改 mpuiic.h
//IO方向设置 //#define MPU_SDA_IN() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;} //#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;} #define MPU_SDA_IN() {GPIOB->CRL &= 0x0FFFFFFF; GPIOB->CRL |= 8<<28;} #define MPU_SDA_OUT() {GPIOB->CRL &= 0x0FFFFFFF; GPIOB->CRL |= 3<<28;} //IO操作函数 /* #define MPU_IIC_SCL PBout(10) //SCL #define MPU_IIC_SDA PBout(11) //SDA #define MPU_READ_SDA PBin(11) //输入SDA */ #define MPU_IIC_SCL PBout(6) //SCL #define MPU_IIC_SDA PBout(7) //SDA #define MPU_READ_SDA PBin(7) //输入SDA
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
修改 mpuiic.c
//初始化IIC void MPU_IIC_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//先使能外设IO PORTB时钟 // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11; // 端口配置 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; // 端口配置 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIO //GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11); //PB10,PB11 输出高 GPIO_SetBits(GPIOB,GPIO_Pin_6|GPIO_Pin_7); //PB10,PB11 输出高 }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小丑西瓜9/article/detail/467161
推荐阅读
相关标签
