21ic电子技术开发论坛 › 单片机与嵌入式系统 › ST MCU › STM32之MPU6050获取欧拉角
1234
返回列表 发新帖我要提问

[应用相关] STM32之MPU6050获取欧拉角

[复制链接]
 楼主| 发顺丰更大nc 发表于 2023-12-28 16:43 | 显示全部楼层
修改几个重要函数
mpu_init() 79303658d351a39e40.png
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:43 | 显示全部楼层
atk_ms6050_dmp_init()uint8_t mpu6050_dmp_init(void)
{
    uint8_t ret;
  
                if(mpu6050_init() == 0){
               
                        ret = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);       /* 开启指定传感器 */
                        if(ret) return 1;
                       
                        ret = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);    /* 设置FIFO */
                        if(ret) return 2;
                       
                        ret = mpu_set_sample_rate(DEFAULT_MPU_HZ);                 /* 设置采样率 */
                        if(ret) return 3;
                       
                        ret = dmp_load_motion_driver_firmware();                   /* 加载DMP固件 */
                        if(ret) return 4;
                       
                        ret = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));  /* 设置陀螺仪方向 */
                        if(ret) return 5;
                       
                        ret = dmp_enable_feature(  DMP_FEATURE_6X_LP_QUAT      |   /* 设置DMP功能 */
                                DMP_FEATURE_TAP             |
                                DMP_FEATURE_ANDROID_ORIENT  |
                                DMP_FEATURE_SEND_RAW_ACCEL  |
                                DMP_FEATURE_SEND_CAL_GYRO   |
                                DMP_FEATURE_GYRO_CAL);
                        if(ret) return 6;
               
                        ret = dmp_set_fifo_rate(DEFAULT_MPU_HZ);                   /* 设置DMP输出速率 */
                        if(ret) return 7;
                       
                        ret = mpu_set_dmp_state(1);                                /* 使能DMP */
                        if(ret) return 8;
                       
                        ret = mpu6050_run_self_test();                          /* 传感器自测试 */
                        if(ret) return 9;
                       
                        return 0;
                }
               
                return 10;
}
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:48 | 显示全部楼层
53903658d36503459b.png
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:48 | 显示全部楼层
atk_ms6050_get_clock_ms（）
这个没什么用，删掉就行
.
.
atk_ms6050_run_self_test()
改名成mpu6050_run_self_test()就行
.
.
atk_ms6050_dmp_get_data()
改名成mpu6050_dmp_get_data()就行
.
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:48 | 显示全部楼层
4. 修改inv_mpu.h文件

uint8_t mpu6050_run_self_test(void);
uint8_t mpu6050_dmp_init(void);
uint8_t mpu6050_dmp_get_data(float *pitch, float *roll, float *yaw);
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:48 | 显示全部楼层
84198658d366ead34d.png
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:49 | 显示全部楼层
. 修改inv_mpu_dmp_motion_driver.c文件

修改头文件
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "dmpKey.h"
#include "dmpmap.h"
#include "delay.h"
#include "usart.h"
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:49 | 显示全部楼层
72053658d36810de68.png
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:49 | 显示全部楼层
修改部分宏定义。 72068658d369008b6a.png 这样我们的DMP库就移植完成了。
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:49 | 显示全部楼层
四元数
mpu6050 的 DMP 输出的四元数是 q30 格式的，也就是将正常浮点格式的四元数放大了 230 倍，因此在换算之前，需要将 DMP 输出的四元数转换为正常的浮点格式，也就是将其除以 230，然后才能将其转换为欧拉角。四元数的介绍可以看这篇文章四元数与欧拉角（Yaw、Pitch、Roll）的转换。
.
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:50 | 显示全部楼层
MPU6050获取欧拉角：俯仰角（Pitch）、横滚角（Roll）、偏航角（Yaw）
前面我们已经使用过mpu6050获取过加速度计和陀螺仪以及温度传感器的原始数据，同时我们也已经成功移植了DMP库，因此只需要修改main.c文件就行。
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:50 | 显示全部楼层
修改main.c文件

复制
  1. /* USER CODE BEGIN Header */
    

  2. /**
    

  3.   ******************************************************************************
    

  4.   * @file           : main.c
    

  5.   * @brief          : Main program body
    

  6.   ******************************************************************************
    

  7.   * @attention
    

  8.   *
    

  9.   * Copyright (c) 2023 STMicroelectronics.
    

  10.   * All rights reserved.
    

  11.   *
    

  12.   * This software is licensed under terms that can be found in the LICENSE file
    

  13.   * in the root directory of this software component.
    

  14.   * If no LICENSE file comes with this software, it is provided AS-IS.
    

  15.   *
    

  16.   ******************************************************************************
    

  17.   */
    

  18. /* USER CODE END Header */
    

  19. /* Includes ------------------------------------------------------------------*/
    

  20. #include "main.h"
    

  21. #include "usart.h"
    

  22. #include "gpio.h"
    


  23. 

  24. /* Private includes ----------------------------------------------------------*/
    

  25. /* USER CODE BEGIN Includes */
    


  26. 

  27. #include <stdio.h>
    

  28. #include "mpu6050.h"
    

  29. #include "delay.h"
    

  30. #include "inv_mpu.h"
    

  31. #include "inv_mpu_dmp_motion_driver.h"
    


  32. 

  33. /* USER CODE END Includes */
    


  34. 

  35. /* Private typedef -----------------------------------------------------------*/
    

  36. /* USER CODE BEGIN PTD */
    


  37. 

  38. /* USER CODE END PTD */
    


  39. 

  40. /* Private define ------------------------------------------------------------*/
    

  41. /* USER CODE BEGIN PD */
    

  42. /* USER CODE END PD */
    


  43. 

  44. /* Private macro -------------------------------------------------------------*/
    

  45. /* USER CODE BEGIN PM */
    


  46. 

  47. /* USER CODE END PM */
    


  48. 

  49. /* Private variables ---------------------------------------------------------*/
    


  50. 

  51. /* USER CODE BEGIN PV */
    


  52. 

  53. /* USER CODE END PV */
    


  54. 

  55. /* Private function prototypes -----------------------------------------------*/
    

  56. void SystemClock_Config(void);
    

  57. /* USER CODE BEGIN PFP */
    


  58. 

  59. /* USER CODE END PFP */
    


  60. 

  61. /* Private user code ---------------------------------------------------------*/
    

  62. /* USER CODE BEGIN 0 */
    


  63. 

  64. float temperature = 0;
    


  65. 

  66. float pitch,roll,yaw;                 //欧拉角
    


  67. 


  68. 


  69. 


  70. 

  71. /* USER CODE END 0 */
    


  72. 

  73. /**
    

  74.   * @brief  The application entry point.
    

  75.   * @retval int
    

  76.   */
    

  77. int main(void)
    

  78. {
    

  79.   /* USER CODE BEGIN 1 */
    


  80. 

  81.   /* USER CODE END 1 */
    


  82. 

  83.   /* MCU Configuration--------------------------------------------------------*/
    


  84. 

  85.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
    

  86.   HAL_Init();
    


  87. 

  88.   /* USER CODE BEGIN Init */
    


  89. 

  90.   /* USER CODE END Init */
    


  91. 

  92.   /* Configure the system clock */
    

  93.   SystemClock_Config();
    


  94. 

  95.   /* USER CODE BEGIN SysInit */
    


  96. 

  97.   /* USER CODE END SysInit */
    


  98. 

  99.   /* Initialize all configured peripherals */
    

  100.   MX_GPIO_Init();
    

  101.   MX_USART3_UART_Init();
    

  102.   /* USER CODE BEGIN 2 */
    


  103. 

  104.         while(mpu6050_dmp_init())
    

  105.         {
    

  106.                 HAL_Delay(200);
    

  107.                 printf("%s\r\n","Mpu6050_DMP Init Wrong!");
    

  108.                 printf("   %d",mpu6050_dmp_init());
    

  109.         }
    

  110.         
    

  111.         printf("%s\r\n","DMP_Mpu6050 Init OK!");
    

  112.         
    


  113. 

  114.   /* USER CODE END 2 */
    


  115. 

  116.   /* Infinite loop */
    

  117.   /* USER CODE BEGIN WHILE */
    

  118.   while (1)
    

  119.   {
    

  120.     /* USER CODE END WHILE */
    


  121. 

  122.     /* USER CODE BEGIN 3 */
    

  123.                 
    

  124.                 temperature= mpu6050_read_temperature();
    

  125.                 
    

  126.                 if(mpu6050_dmp_get_data(&pitch,&roll,&yaw)==0){
    

  127.                         
    

  128.                         printf("三轴角度：%f-%f-%f\r\n",pitch,roll,yaw);
    

  129.                         printf("temp:%f\r\n",temperature);
    

  130.                         printf("pitch:%f\r\n",pitch);
    

  131.                         printf("roll: %f\r\n",roll);
    

  132.                         printf("yaw:  %f\r\n",yaw);
    

  133.                         
    

  134.                 }
    

  135.                 
    

  136.                 delay_ms(100);
    

  137.                 
    

  138.                 usart1_receive_data_handle();
    

  139.                 
    

  140.   }
    

  141.   /* USER CODE END 3 */
    

  142. }
    


  143. 

  144. /**
    

  145.   * @brief System Clock Configuration
    

  146.   * @retval None
    

  147.   */
    

  148. void SystemClock_Config(void)
    

  149. {
    

  150.   RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    

  151.   RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
    


  152. 

  153.   /** Initializes the RCC Oscillators according to the specified parameters
    

  154.   * in the RCC_OscInitTypeDef structure.
    

  155.   */
    

  156.   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    

  157.   RCC_OscInitStruct.HSEState = RCC_HSE_ON;
    

  158.   RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
    

  159.   RCC_OscInitStruct.HSIState = RCC_HSI_ON;
    

  160.   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    

  161.   RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    

  162.   RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
    

  163.   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    

  164.   {
    

  165.     Error_Handler();
    

  166.   }
    


  167. 

  168.   /** Initializes the CPU, AHB and APB buses clocks
    

  169.   */
    

  170.   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
    

  171.                               |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
    

  172.   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    

  173.   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    

  174.   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
    

  175.   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
    


  176. 

  177.   if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
    

  178.   {
    

  179.     Error_Handler();
    

  180.   }
    

  181. }
    


  182. 

  183. /* USER CODE BEGIN 4 */
    


  184. 

  185. /* USER CODE END 4 */
    


  186. 

  187. /**
    

  188.   * @brief  This function is executed in case of error occurrence.
    

  189.   * @retval None
    

  190.   */
    

  191. void Error_Handler(void)
    

  192. {
    

  193.   /* USER CODE BEGIN Error_Handler_Debug */
    

  194.   /* User can add his own implementation to report the HAL error return state */
    

  195.   __disable_irq();
    

  196.   while (1)
    

  197.   {
    

  198.   }
    

  199.   /* USER CODE END Error_Handler_Debug */
    

  200. }
    


  201. 

  202. #ifdef  USE_FULL_ASSERT
    

  203. /**
    

  204.   * @brief  Reports the name of the source file and the source line number
    

  205.   *         where the assert_param error has occurred.
    

  206.   * @param  file: pointer to the source file name
    

  207.   * @param  line: assert_param error line source number
    

  208.   * @retval None
    

  209.   */
    

  210. void assert_failed(uint8_t *file, uint32_t line)
    

  211. {
    

  212.   /* USER CODE BEGIN 6 */
    

  213.   /* User can add his own implementation to report the file name and line number,
    

  214.      ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    

  215.   /* USER CODE END 6 */
    

  216. }
    

  217. #endif /* USE_FULL_ASSERT */
    


  218. 


  219.
回复 赏

举报

 楼主| 发顺丰更大nc 发表于 2023-12-28 16:51 | 显示全部楼层
实验效果
15766658d36f95f259.png
回复 赏

举报

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

本版积分规则

快速回复 在线客服 返回列表 返回顶部
快速回复 在线客服 返回列表 返回顶部