设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › 利用STM8驱动的MPU6050陀螺仪源程序
返回列表 发新帖
查看: 10141|回复: 19
收起左侧

利用STM8驱动的MPU6050陀螺仪源程序

  [复制链接]
ID:335790 发表于 2018-5-22 16:23 | 显示全部楼层 |阅读模式
利用STM8驱动的陀螺仪程序，参考其他人写的32的程序，自己修改接口，完成驱动陀螺仪。
0.png
单片机源程序如下:
  1. #include "MPU6050.h"
  2. #include "IOI2C.h"
  3. #include "usart1.h"
  4. #include <math.h>
  5. #include "inv_mpu_dmp_motion_driver.h"
  6. #include "inv_mpu.h"
  7. #include <stdio.h>
  8. /**************************************************************************/
  9. #define PRINT_ACCEL     (0x01)
  10. #define PRINT_GYRO      (0x02)
  11. #define PRINT_QUAT      (0x04)
  12. #define ACCEL_ON        (0x01)
  13. #define GYRO_ON         (0x02)
  14. #define MOTION          (0)
  15. #define NO_MOTION       (1)
  16. #define DEFAULT_MPU_HZ  (200)
  17. #define FLASH_SIZE      (512)
  18. #define FLASH_MEM_START ((void*)0x1800)
  19. #define q30  1073741824.0f

  21. static signed char gyro_orientation[9] = {-1, 0, 0,
  22.                                            0,-1, 0,
  23.                                            0, 0, 1};

  25. static  unsigned short inv_row_2_scale(const signed char *row)
  26. {
  27.     unsigned short b;

  29.     if (row[0] > 0)
  30.         b = 0;
  31.     else if (row[0] < 0)
  32.         b = 4;
  33.     else if (row[1] > 0)
  34.         b = 1;
  35.     else if (row[1] < 0)
  36.         b = 5;
  37.     else if (row[2] > 0)
  38.         b = 2;
  39.     else if (row[2] < 0)
  40.         b = 6;
  41.     else
  42.         b = 7;      // error
  43.     return b;
  44. }


  47. static  unsigned short inv_orientation_matrix_to_scalar(
  48.     const signed char *mtx)
  49. {
  50.     unsigned short scalar;
  51.     scalar = inv_row_2_scale(mtx);
  52.     scalar |= inv_row_2_scale(mtx + 3) << 3;
  53.     scalar |= inv_row_2_scale(mtx + 6) << 6;


  56.     return scalar;
  57. }

  59. static void run_self_test(void)
  60. {
  61.     int result;
  62.     long gyro[3], accel[3];

  64.     result = mpu_run_self_test(gyro, accel);
  65.     if (result == 0x7) {
  66.         /* Test passed. We can trust the gyro data here, so let's push it down
  67.          * to the DMP.
  68.          */
  69.         float sens;
  70.         unsigned short accel_sens;
  71.         mpu_get_gyro_sens(&sens);
  72.         gyro[0] = (long)(gyro[0] * sens);
  73.         gyro[1] = (long)(gyro[1] * sens);
  74.         gyro[2] = (long)(gyro[2] * sens);
  75.         dmp_set_gyro_bias(gyro);
  76.         mpu_get_accel_sens(&accel_sens);
  77.         accel[0] *= accel_sens;
  78.         accel[1] *= accel_sens;
  79.         accel[2] *= accel_sens;
  80.         dmp_set_accel_bias(accel);
  81.                 printf("setting bias succesfully ......\r\n");
  82.     }
  83. }



  87. uint8_t buffer[14];

  89. int16_t  MPU6050_FIFO[6][11];
  90. int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;


  93. /**************************实现函数********************************************
  94. *函数原型:                void  MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
  95. *功　　能:            将新的ADC数据更新到 FIFO数组，进行滤波处理
  96. *******************************************************************************/

  98. void  MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
  99. {
  100. unsigned char i ;
  101. int32_t sum=0;
  102. for(i=1;i<10;i++){        //FIFO 操作
  103. MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
  104. MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
  105. MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
  106. MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
  107. MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
  108. MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
  109. }
  110. MPU6050_FIFO[0][9]=ax;//将新的数据放置到 数据的最后面
  111. MPU6050_FIFO[1][9]=ay;
  112. MPU6050_FIFO[2][9]=az;
  113. MPU6050_FIFO[3][9]=gx;
  114. MPU6050_FIFO[4][9]=gy;
  115. MPU6050_FIFO[5][9]=gz;

  117. sum=0;
  118. for(i=0;i<10;i++){        //求当前数组的合，再取平均值
  119.    sum+=MPU6050_FIFO[0][i];
  120. }
  121. MPU6050_FIFO[0][10]=sum/10;

  123. sum=0;
  124. for(i=0;i<10;i++){
  125.    sum+=MPU6050_FIFO[1][i];
  126. }
  127. MPU6050_FIFO[1][10]=sum/10;

  129. sum=0;
  130. for(i=0;i<10;i++){
  131.    sum+=MPU6050_FIFO[2][i];
  132. }
  133. MPU6050_FIFO[2][10]=sum/10;

  135. sum=0;
  136. for(i=0;i<10;i++){
  137.    sum+=MPU6050_FIFO[3][i];
  138. }
  139. MPU6050_FIFO[3][10]=sum/10;

  141. sum=0;
  142. for(i=0;i<10;i++){
  143.    sum+=MPU6050_FIFO[4][i];
  144. }
  145. MPU6050_FIFO[4][10]=sum/10;

  147. sum=0;
  148. for(i=0;i<10;i++){
  149.    sum+=MPU6050_FIFO[5][i];
  150. }
  151. MPU6050_FIFO[5][10]=sum/10;
  152. }

  154. /**************************实现函数********************************************
  155. *函数原型:                void MPU6050_setClockSource(uint8_t source)
  156. *功　　能:            设置  MPU6050 的时钟源
  157. * CLK_SEL | Clock Source
  158. * --------+--------------------------------------
  159. * 0       | Internal oscillator
  160. * 1       | PLL with X Gyro reference
  161. * 2       | PLL with Y Gyro reference
  162. * 3       | PLL with Z Gyro reference
  163. * 4       | PLL with external 32.768kHz reference
  164. * 5       | PLL with external 19.2MHz reference
  165. * 6       | Reserved
  166. * 7       | Stops the clock and keeps the timing generator in reset
  167. *******************************************************************************/
  168. void MPU6050_setClockSource(uint8_t source){
  169.     IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);

  171. }

  173. /** Set full-scale gyroscope range.
  174. * @param range New full-scale gyroscope range value
  175. * @see getFullScaleRange()
  176. * @see MPU6050_GYRO_FS_250
  177. * @see MPU6050_RA_GYRO_CONFIG
  178. * @see MPU6050_GCONFIG_FS_SEL_BIT
  179. * @see MPU6050_GCONFIG_FS_SEL_LENGTH
  180. */
  181. void MPU6050_setFullScaleGyroRange(uint8_t range) {
  182.     IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
  183. }

  185. /**************************实现函数********************************************
  186. *函数原型:                void MPU6050_setFullScaleAccelRange(uint8_t range)
  187. *功　　能:            设置  MPU6050 加速度计的最大量程
  188. *******************************************************************************/
  189. void MPU6050_setFullScaleAccelRange(uint8_t range) {
  190.     IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
  191. }

  193. /**************************实现函数********************************************
  194. *函数原型:                void MPU6050_setSleepEnabled(uint8_t enabled)
  195. *功　　能:            设置  MPU6050 是否进入睡眠模式
  196.                                 enabled =1   睡觉
  197.                             enabled =0   工作
  198. *******************************************************************************/
  199. void MPU6050_setSleepEnabled(uint8_t enabled) {
  200.     IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
  201. }

  203. /**************************实现函数********************************************
  204. *函数原型:                uint8_t MPU6050_getDeviceID(void)
  205. *功　　能:            读取  MPU6050 WHO_AM_I 标识         将返回 0x68
  206. *******************************************************************************/
  207. uint8_t MPU6050_getDeviceID(void) {

  209.     IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
  210.     return buffer[0];
  211. }

  213. /**************************实现函数********************************************
  214. *函数原型:                uint8_t MPU6050_testConnection(void)
  215. *功　　能:            检测MPU6050 是否已经连接
  216. *******************************************************************************/
  217. uint8_t MPU6050_testConnection(void) {
  218.    if(MPU6050_getDeviceID() == 0x68)  //0b01101000;
  219.    return 1;
  220.            else return 0;
  221. }

  223. /**************************实现函数********************************************
  224. *函数原型:                void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
  225. *功　　能:            设置 MPU6050 是否为AUX I2C线的主机
  226. *******************************************************************************/
  227. void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
  228.     IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
  229. }

  231. /**************************实现函数********************************************
  232. *函数原型:                void MPU6050_setI2CBypassEnabled(uint8_t enabled)
  233. *功　　能:            设置 MPU6050 是否为AUX I2C线的主机
  234. *******************************************************************************/
  235. void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
  236.     IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
  237. }

  239. /**************************实现函数********************************************
  240. *函数原型:                void MPU6050_initialize(void)
  241. *功　　能:            初始化         MPU6050 以进入可用状态。
  242. *******************************************************************************/
  243. void MPU6050_initialize(void) {
  244.     MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO); //设置时钟
  245.     MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺仪最大量程 +-1000度每秒
  246.     MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);        //加速度度最大量程 +-2G
  247.     MPU6050_setSleepEnabled(0); //进入工作状态
  248.          MPU6050_setI2CMasterModeEnabled(0);         //不让MPU6050 控制AUXI2C
  249.          MPU6050_setI2CBypassEnabled(0);         //主控制器的I2C与        MPU6050的AUXI2C        直通。控制器可以直接访问HMC5883L
  250. }




  255. /**************************************************************************
  256. 函数功能：MPU6050内置DMP的初始化
  257. 入口参数：无
  258. 返回  值：无
  259. 作    者：平衡小车之家
  260. **************************************************************************/
  261. void DMP_Init(void)
  262. {
  263.    u8 temp[1]={0};
  264.    i2cRead(0x68,0x75,1,temp);
  265.          printf("mpu_set_sensor complete ......\r\n");
  266.         if(temp[0]!=0x68)NVIC_SystemReset();
  267.         if(!mpu_init())
  268.   {
  269.           if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
  270.                    printf("mpu_set_sensor complete ......\r\n");
  271.           if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
  272.                    printf("mpu_configure_fifo complete ......\r\n");
  273.           if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
  274.                    printf("mpu_set_sample_rate complete ......\r\n");
  275.           if(!dmp_load_motion_driver_firmware())
  276.                   printf("dmp_load_motion_driver_firmware complete ......\r\n");
  277.           if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
  278.                    printf("dmp_set_orientation complete ......\r\n");
  279.           if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
  280.                 DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
  281.                 DMP_FEATURE_GYRO_CAL))
  282.                    printf("dmp_enable_feature complete ......\r\n");
  283.           if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
  284.                    printf("dmp_set_fifo_rate complete ......\r\n");
  285.           run_self_test();
  286.           if(!mpu_set_dmp_state(1))
  287.                    printf("mpu_set_dmp_state complete ......\r\n");
  288.   }
  289. }
  290. /**************************************************************************
  291. 函数功能：读取MPU6050内置DMP的姿态信息
  292. 入口参数：无
  293. 返回  值：无
  294. 作    者：平衡小车之家
  295. **************************************************************************/
  296. uint8_t Read_DMP(float* Pitch,float* Roll,float* Yaw)
  297. {        
  298. ……………………

  300. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
6050测试3.rar (598.63 KB, 下载次数: 301)


回复

使用道具 举报

ID:158903 发表于 2018-6-21 15:14 | 显示全部楼层

谢谢分享
回复

使用道具 举报

ID:357306 发表于 2018-6-23 16:25 | 显示全部楼层
高手！谢谢，学习了
回复

使用道具 举报

ID:361002 发表于 2018-6-28 14:59 | 显示全部楼层
谢谢分享
回复

使用道具 举报

ID:425665 发表于 2018-11-13 14:28 | 显示全部楼层
正好有个项目可以用到这个程序，感谢楼主。
回复

使用道具 举报

ID:421518 发表于 2019-4-1 09:59 | 显示全部楼层
谢谢分享。。。。。
回复

使用道具 举报

ID:430197 发表于 2019-4-2 12:13 | 显示全部楼层
学习了，谢谢分享
回复

使用道具 举报

ID:578767 发表于 2019-7-6 01:23 | 显示全部楼层
学习 学习 谢谢
回复

使用道具 举报

ID:578767 发表于 2019-7-6 01:24 | 显示全部楼层
学习了，谢谢分享。。
回复

使用道具 举报

ID:585663 发表于 2019-7-22 20:39 | 显示全部楼层
uint8_t buffer[14];

int16_t  MPU6050_FIFO[6][11];
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;

请问这段什么意思？实现什么功能
回复

使用道具 举报

ID:196702 发表于 2019-8-3 00:33 | 显示全部楼层
感谢楼主。
回复

使用道具 举报

ID:645237 发表于 2019-11-19 18:25 | 显示全部楼层
谢谢分享
回复

使用道具 举报

ID:754694 发表于 2020-8-25 12:57 来自手机 | 显示全部楼层
好资料，谢谢分享
回复

使用道具 举报

ID:229821 发表于 2020-9-11 22:47 | 显示全部楼层
好资料。谢谢楼主的共享精神。
回复

使用道具 举报

ID:754694 发表于 2020-10-24 21:24 来自手机 | 显示全部楼层
32到8完美移植，多谢分享
回复

使用道具 举报

ID:81138 发表于 2021-1-27 15:48 | 显示全部楼层
不知道电路是如何设计的
回复

使用道具 举报

ID:928052 发表于 2021-8-8 20:26 | 显示全部楼层
为什么重复下载，需要重复扣分呢？
回复

使用道具 举报

ID:767310 发表于 2021-8-26 16:21 | 显示全部楼层
编译提示NVIC_SystemReset没有定义，并且空间不足 ，请问楼主是用哪个型号的STM8芯片
回复

使用道具 举报

ID:1038866 发表于 2022-7-17 09:05 | 显示全部楼层
高手！谢谢，学习
回复

使用道具 举报

ID:189235 发表于 2023-10-14 00:51 | 显示全部楼层
#在这里快速回复#谢谢分享
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表