[STM32L4+] 【STEVAL-STWINKT1B测评】4.驱动六轴陀螺仪(Ism330)

[复制链接]
 楼主| 电子烂人 发表于 2024-8-29 11:07 | 显示全部楼层 |阅读模式
<
win, ev, TE, ISM, ST
本帖最后由 电子烂人 于 2024-8-29 11:07 编辑

#申请开发板# #申请原创# 之前的IS3DWB遇到了一些bug,在联系FAE修改。目前用polling的代码修改,识别到whoami错误。屏蔽掉这部分后只能读取到一次数据,之后会卡在循环中。
看了下nanoedge AI貌似对这个传感器不太好用,所以换ISM330传感器来采集数据1.传感器简介
ISM330DHCX是一种系统级封装器件,它具有专为工业4.0应用而量身定制的高性能3D数字加速度计和3D数字陀螺仪。加速计可选量程为±2/±4/±8/±16 g,陀螺仪可选量程为±125/±250/±500/±1000/±2000/±4000 dps,能够用在各种应用中。并且在ISM330DHCX的所有设计环节和校准方面进行了优化,实现了极高的精度、稳定性、极低噪声和完全的数据同步。
另外,ISM330DHCX还有:
  • 可编程有限状态机,处理来自加速度计、陀螺仪和一个外部传感器的数据
  • 机器学习内核
  • 智能嵌入式功能和中断:倾斜检测、自由落体、唤醒、6D/4D方向检测、单击和双击


该传感器完全可以满足工业自动化中各类电机的监测功能。
2.初始化配置:
6087266ce9252d7fca.png
基础配置和IS3DWB别无二致,只需要改一下对应的引脚即可。
8077666ce94ed2f11c.png
7831966ce9483b773c.png
3975566ce94cf8351a.png

另外INT1/INT2这两个引脚是配置传感器内部寄存器的
3024266ce94a5caa5b.png
5913866ce94aca1013.png

经询问ST的FAE,这两个引脚不配置也可以正常工作,只是工作模式固定
597366ce95b83670c.png
具体的IO配置如图:
6300466ce9bb8ce206.png
3995766ce9c1aeb422.png
3202466ce9c29abbf1.png
cube-mems包的配置同前一篇帖子,不再展示
4.代码分析:
定义加速度、陀螺仪的参数:
  1. #ifndef AXIS
  2.   #define AXIS                          3                         /* Axis should be defined between 1 and 3 */
  3. #endif
  4. #ifndef SAMPLES
  5.   #define SAMPLES                       256                       /* Should be between 16 & 4096 */
  6. #endif
  7. #define MAX_FIFO_SIZE                   256                       /* The maximum number of data we can get in the FIFO is 512 but here we define max to 256 for our need */
  8. #define FIFO_FULL                       512                       /* FIFO full size */
  9. /************************************************************ Sensor type part ************************************************************/
  10. #define ACCELEROMETER                                             /* Could be either ACCELEROMETER or GYROSCOPE */
  11. /************************************************************ Sensors configuration part ************************************************************/
  12. #ifdef ACCELEROMETER
  13.   #ifndef ACCELEROMETER_ODR
  14.     #define ACCELEROMETER_ODR           ISM330DHCX_XL_ODR_1666Hz  /* Shoud be between ISM330DHCX_XL_ODR_12Hz5 and ISM330DHCX_XL_ODR_6667Hz */
  15.   #endif
  16.   #ifndef ACCELEROMETER_FS
  17.     #define ACCELEROMETER_FS            ISM330DHCX_2g             /* Should be between ISM330DHCX_2g and ISM330DHCX_8g */
  18.   #endif
  19. #else
  20.   #ifndef GYROSCOPE_ODR
  21.     #define GYROSCOPE_ODR               ISM330DHCX_GY_ODR_1666Hz  /* Shoud be between ISM330DHCX_GY_ODR_12Hz5 and ISM330DHCX_GY_ODR_6667Hz */
  22.   #endif
  23.   #ifndef GYROSCOPE_FS
  24.     #define GYROSCOPE_FS                ISM330DHCX_2000dps        /* Should be between ISM330DHCX_125dps and ISM330DHCX_4000dps */
  25.   #endif
  26. #endif
  27. /************************************************************ Datalogger / NEAI mode part ************************************************************/
  28. #ifndef NEAI_MODE
  29.   #define NEAI_MODE                     0                         /* 0: Datalogger mode, 1: NEAI functions mode */
  30. #endif
  31. #if (NEAI_MODE == 1)
  32.   #ifndef NEAI_LEARN_NB
  33.     #define NEAI_LEARN_NB               20                        /* Number of buffers to be learn by the NEAI library */
  34.   #endif
  35. #endif
定义部分

  1. /* USER CODE BEGIN PV */
  2. static int16_t data_raw_acceleration[3];
  3. static int16_t data_raw_angular_rate[3];
  4. static uint8_t whoamI, rst;
  5. uint8_t neai_similarity = 0, neai_state = 0;
  6. volatile uint8_t drdy = 0;
  7. uint16_t data_left = (uint16_t) SAMPLES, number_read = 0, neai_buffer_ptr = 0, neai_cnt = 0;
  8. float neai_time = 0.0;
  9. static float neai_buffer[AXIS * SAMPLES] = {0.0};
  10. stmdev_ctx_t dev_ctx;


  11. /* USER CODE END PV */
初始化要用到的函数:
  1. /* USER CODE BEGIN PV */
  2. static int16_t data_raw_acceleration[3];
  3. static int16_t data_raw_angular_rate[3];
  4. static uint8_t whoamI, rst;
  5. uint8_t neai_similarity = 0, neai_state = 0;
  6. volatile uint8_t drdy = 0;
  7. uint16_t data_left = (uint16_t) SAMPLES, number_read = 0, neai_buffer_ptr = 0, neai_cnt = 0;
  8. float neai_time = 0.0;
  9. static float neai_buffer[AXIS * SAMPLES] = {0.0};
  10. stmdev_ctx_t dev_ctx;


  11. /* USER CODE END PV */
  1. /* USER CODE BEGIN PFP */
  2. static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len);
  3. static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len);
  4. static void ism330dhcx_initialize(void);
  5. static void ism330dhcx_initialize_basics(void);
  6. static void ism330dhcx_initialize_fifo(void);
  7. static void ism330dhcx_get_buffer_from_fifo(uint16_t nb);
  8. static float ism330dhcx_convert_gyro_data_to_mdps(int16_t gyro_raw_data);
  9. static float ism330dhcx_convert_accel_data_to_mg(int16_t accel_raw_data);

  10. /* USER CODE END PFP */
同前一篇帖子一样,定义platform
  1. /* USER CODE BEGIN 1 */
  2.   /* Initialize mems driver interface */
  3.   dev_ctx.write_reg = platform_write;
  4.   dev_ctx.read_reg = platform_read;
  5.   dev_ctx.handle = &hspi3;
  6.   /* USER CODE END 1 */
在主循环中用FIFO采集加速度数据,并用串口打印出来。
  1. while (1)
  2.   {
  3.     uint8_t wtm_flag = 0, status2 = 0;
  4.     uint16_t num = 0;
  5.     if (drdy) {
  6.       /* Reset data ready condition */
  7.       drdy = 0;
  8.       ism330dhcx_read_reg(&dev_ctx, ISM330DHCX_FIFO_STATUS2, &status2, 1);
  9.       wtm_flag = status2 >> 7;
  10.       if (wtm_flag) {
  11.         ism330dhcx_fifo_data_level_get(&dev_ctx, &num);
  12.         if (data_left < num) {
  13.           num = data_left;
  14.         }
  15.         ism330dhcx_get_buffer_from_fifo(num);
  16.         data_left -= num;
  17.         number_read += num;
  18.         if (data_left == 0) {
  19.           ism330dhcx_fifo_mode_set(&dev_ctx, ISM330DHCX_BYPASS_MODE);
  20. #if NEAI_MODE
  21.           uint32_t cycles_cnt = 0;
  22.           if (neai_cnt < NEAI_LEARN_NB) {
  23.             neai_cnt++;
  24.             KIN1_ResetCycleCounter();
  25.             neai_state = neai_anomalydetection_learn(neai_buffer);
  26.             cycles_cnt = KIN1_GetCycleCounter();
  27.             neai_time = (cycles_cnt * 1000000.0) / HAL_RCC_GetSysClockFreq();
  28.             printf("Learn: %d / %d. NEAI learn return: %d. Cycles counter: %ld = %.1f µs at %ld Hz.\n",
  29.                   neai_cnt, NEAI_LEARN_NB, neai_state, cycles_cnt, neai_time, HAL_RCC_GetSysClockFreq());
  30.           }
  31.           else {
  32.             KIN1_ResetCycleCounter();
  33.             neai_state = neai_anomalydetection_detect(neai_buffer, &neai_similarity);
  34.             cycles_cnt = KIN1_GetCycleCounter();
  35.             neai_time = (cycles_cnt * 1000000.0) / HAL_RCC_GetSysClockFreq();
  36.             printf("Similarity: %d / 100. NEAI detect return: %d. Cycles counter: %ld = %.1f µs at %ld Hz.\n",
  37.                   neai_similarity, neai_state, cycles_cnt, neai_time, HAL_RCC_GetSysClockFreq());
  38.           }
  39. #else
  40.           for (uint16_t i = 0; i < AXIS * SAMPLES; i++) {
  41.             printf("%.3f ", neai_buffer[i]);
  42.           }
  43.           printf("\n");
  44. #endif
  45.           data_left = (uint16_t) SAMPLES;
  46.           number_read = 0;
  47.           memset(neai_buffer, 0x00, AXIS * SAMPLES * sizeof(float));
  48.           if (SAMPLES <= MAX_FIFO_SIZE) {
  49.             ism330dhcx_fifo_watermark_set(&dev_ctx, (uint16_t) SAMPLES);
  50.           }
  51.           else {
  52.             ism330dhcx_fifo_watermark_set(&dev_ctx, (uint16_t) MAX_FIFO_SIZE);
  53.           }
  54.           ism330dhcx_fifo_mode_set(&dev_ctx, ISM330DHCX_STREAM_MODE);
  55.         }
  56.         else if (data_left < MAX_FIFO_SIZE) {
  57.           ism330dhcx_fifo_watermark_set(&dev_ctx, data_left);
  58.         }
  59.       }
  60.     }
  61.     /* USER CODE END WHILE */

  62.     /* USER CODE BEGIN 3 */
  63.   }
  64.   /* USER CODE END 3 */
另外把串口数据打印部分重定义一下:
  1. int __io_putchar(int ch)
  2. {
  3. uint8_t c[1];
  4. c[0] = ch & 0x00FF;
  5. HAL_UART_Transmit(&huart2, &*c, 1, 10);
  6. return ch;
  7. }
其他几个函数如写入,读取等直接使用例程:
  1. static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
  2. {
  3.   HAL_GPIO_WritePin(CS_DHC_GPIO_Port, CS_DHC_Pin, GPIO_PIN_RESET);
  4.   HAL_SPI_Transmit(handle, ®, 1, 1000);
  5.   HAL_SPI_Transmit(handle, (uint8_t*) bufp, len, 1000);
  6.   HAL_GPIO_WritePin(CS_DHC_GPIO_Port, CS_DHC_Pin, GPIO_PIN_SET);
  7.   return 0;
  8. }

  9. /*
  10. * [url=home.php?mod=space&uid=247401]@brief[/url]  Read generic device register (platform dependent)
  11. *
  12. * @param  handle    customizable argument. In this examples is used in
  13. *                   order to select the correct sensor bus handler.
  14. * @param  reg       register to read
  15. * @param  bufp      pointer to buffer that store the data read
  16. * @param  len       number of consecutive register to read
  17. *
  18. */
  19. static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
  20. {
  21.   reg |= 0x80;
  22.   HAL_GPIO_WritePin(CS_DHC_GPIO_Port, CS_DHC_Pin, GPIO_PIN_RESET);
  23.   HAL_SPI_Transmit(handle, ®, 1, 1000);
  24.   HAL_SPI_Receive(handle, bufp, len, 1000);
  25.   HAL_GPIO_WritePin(CS_DHC_GPIO_Port, CS_DHC_Pin, GPIO_PIN_SET);
  26.   return 0;
  27. }

  28. /*
  29. * [url=home.php?mod=space&uid=247401]@brief[/url]  Initialize ISM330DHCX sensor interface
  30. *
  31. * @param  No
  32. *
  33. * [url=home.php?mod=space&uid=266161]@return[/url] No
  34. *
  35. */
  36. static void ism330dhcx_initialize()
  37. {
  38.   ism330dhcx_initialize_basics();
  39. #ifdef ACCELEROMETER
  40.   /* Accelelerometer configuration */
  41.   ism330dhcx_xl_data_rate_set(&dev_ctx, ACCELEROMETER_ODR);
  42.   ism330dhcx_xl_full_scale_set(&dev_ctx, ACCELEROMETER_FS);
  43. #else
  44.   /* Gyroscope configuration */
  45.   ism330dhcx_gy_data_rate_set(&dev_ctx, GYROSCOPE_ODR);
  46.   ism330dhcx_gy_full_scale_set(&dev_ctx, GYROSCOPE_FS);
  47. #endif
  48.   ism330dhcx_initialize_fifo();
  49. }

  50. /*
  51. * @brief  Initialize ISM330DHCX basics
  52. *
  53. * @param  No
  54. *
  55. * [url=home.php?mod=space&uid=266161]@return[/url] No
  56. *
  57. */
  58. static void ism330dhcx_initialize_basics()
  59. {
  60.   /* Check device ID */
  61.   whoamI = 0;

  62.   do {
  63.     /* Wait sensor boot time */
  64.     HAL_Delay(10);
  65.     ism330dhcx_device_id_get(&dev_ctx, &whoamI);
  66.   } while (whoamI != ISM330DHCX_ID);

  67.   /* Restore default configuration */
  68.   ism330dhcx_reset_set(&dev_ctx, PROPERTY_ENABLE);

  69.   do {
  70.     ism330dhcx_reset_get(&dev_ctx, &rst);
  71.   } while (rst);

  72.   /* Start device configuration. */
  73.   ism330dhcx_device_conf_set(&dev_ctx, PROPERTY_ENABLE);
  74. }

  75. /*
  76. * @brief  Initialize ISM330DHCX internal FIFO
  77. *
  78. * @param  No
  79. *
  80. * @return No
  81. *
  82. */
  83. static void ism330dhcx_initialize_fifo()
  84. {
  85. #ifdef ACCELEROMETER
  86.   /* Batch odr config */
  87.   ism330dhcx_fifo_xl_batch_set(&dev_ctx, ACCELEROMETER_ODR);
  88.   ism330dhcx_fifo_gy_batch_set(&dev_ctx, 0);
  89. #else
  90.   /* Batch odr config */
  91.   ism330dhcx_fifo_xl_batch_set(&dev_ctx, 0);
  92.   ism330dhcx_fifo_gy_batch_set(&dev_ctx, GYROSCOPE_ODR);
  93. #endif
  94.   /* FIFO MODE */
  95.   ism330dhcx_fifo_mode_set(&dev_ctx, ISM330DHCX_BYPASS_MODE);
  96.   HAL_Delay(10);
  97.   ism330dhcx_fifo_mode_set(&dev_ctx, ISM330DHCX_STREAM_MODE);
  98.   /* Watermark config */
  99.   if (SAMPLES <= MAX_FIFO_SIZE) {
  100.     ism330dhcx_fifo_watermark_set(&dev_ctx, (uint16_t) SAMPLES);
  101.   }
  102.   else {
  103.     ism330dhcx_fifo_watermark_set(&dev_ctx, (uint16_t) MAX_FIFO_SIZE);
  104.   }
  105.   uint8_t ctrl = 0x08;
  106.   ism330dhcx_write_reg(&dev_ctx, ISM330DHCX_INT1_CTRL, (uint8_t *) &ctrl, 1);
  107. }

  108. /*
  109. * @brief  Get accelerometer or gyroscope data from
  110. *         ISM330DHCX using the internal FIFO buffer
  111. *
  112. * @param  No
  113. *
  114. * @return No
  115. *
  116. */
  117. static void ism330dhcx_get_buffer_from_fifo(uint16_t nb)
  118. {
  119.   static int16_t dummy[3];
  120.   ism330dhcx_fifo_tag_t reg_tag;
  121.   for (uint16_t i = 0; i < nb; i++) {
  122.     /* Read FIFO tag */
  123.     ism330dhcx_fifo_sensor_tag_get(&dev_ctx, ®_tag);
  124.     if(reg_tag == ISM330DHCX_XL_NC_TAG) {
  125.       memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
  126.       ism330dhcx_fifo_out_raw_get(&dev_ctx, (uint8_t *) data_raw_acceleration);
  127.       for(uint8_t j = 0; j < AXIS; j++) {
  128.         neai_buffer[(AXIS * neai_buffer_ptr) + (AXIS * i) + j] = ism330dhcx_convert_accel_data_to_mg(data_raw_acceleration[j]);
  129.       }
  130.     }
  131.     else if(reg_tag == ISM330DHCX_GYRO_NC_TAG) {
  132.       memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
  133.       ism330dhcx_fifo_out_raw_get(&dev_ctx, (uint8_t *) data_raw_angular_rate);
  134.       for(uint8_t j = 0; j < AXIS; j++) {
  135.         neai_buffer[(AXIS * neai_buffer_ptr) + (AXIS * i) + j] = ism330dhcx_convert_gyro_data_to_mdps(data_raw_angular_rate[j]);
  136.       }
  137.     }
  138.     else {
  139.       /* Flush unused samples */
  140.       printf("Bad sensor tag: %d.\n", reg_tag);
  141.       memset(dummy, 0x00, 3 * sizeof(int16_t));
  142.       ism330dhcx_fifo_out_raw_get(&dev_ctx, (uint8_t *) dummy);
  143.     }
  144.   }
  145.   neai_buffer_ptr += nb;
  146.   if (neai_buffer_ptr == SAMPLES) {
  147.     neai_buffer_ptr = 0;
  148.   }
  149. }

  150. /*
  151. * @brief  Convert gyroscope raw data to milli degrees per second (mdps)
  152. *
  153. * @param  gyro_raw_data: which is gyroscope raw data
  154. *                        depending on the full scale selected
  155. *
  156. * @return The converted value in milli degrees per second (mdps)
  157. *
  158. */
  159. static float ism330dhcx_convert_gyro_data_to_mdps(int16_t gyro_raw_data)
  160. {
  161.   float gyro_data_mdps = 0.0;
  162. #ifdef GYROSCOPE
  163.   switch (GYROSCOPE_FS)
  164.   {
  165.   case ISM330DHCX_125dps:
  166.     gyro_data_mdps = ism330dhcx_from_fs125dps_to_mdps(gyro_raw_data);
  167.     break;
  168.   case ISM330DHCX_250dps:
  169.     gyro_data_mdps = ism330dhcx_from_fs250dps_to_mdps(gyro_raw_data);
  170.     break;
  171.   case ISM330DHCX_500dps:
  172.     gyro_data_mdps = ism330dhcx_from_fs500dps_to_mdps(gyro_raw_data);
  173.     break;
  174.   case ISM330DHCX_1000dps:
  175.     gyro_data_mdps = ism330dhcx_from_fs1000dps_to_mdps(gyro_raw_data);
  176.     break;
  177.   case ISM330DHCX_2000dps:
  178.     gyro_data_mdps = ism330dhcx_from_fs2000dps_to_mdps(gyro_raw_data);
  179.     break;
  180.   case ISM330DHCX_4000dps:
  181.     gyro_data_mdps = ism330dhcx_from_fs4000dps_to_mdps(gyro_raw_data);
  182.     break;
  183.   default:
  184.     gyro_data_mdps = 0.0;
  185.     break;
  186.   }
  187. #endif
  188.   return gyro_data_mdps;
  189. }

  190. /*
  191. * @brief  Convert accelerometer raw data to milli-G' (mg)
  192. *
  193. * @param  accel_raw_data: which is accelerometer raw data
  194. *                        depending on the full scale selected
  195. *
  196. * @return The converted value in milli-G' (mg)
  197. *
  198. */
  199. static float ism330dhcx_convert_accel_data_to_mg(int16_t accel_raw_data)
  200. {
  201.   float accel_data_mg = 0.0;
  202. #ifdef ACCELEROMETER
  203.   switch (ACCELEROMETER_FS)
  204.   {
  205.   case ISM330DHCX_2g:
  206.     accel_data_mg = ism330dhcx_from_fs2g_to_mg(accel_raw_data);
  207.     break;
  208.   case ISM330DHCX_4g:
  209.     accel_data_mg = ism330dhcx_from_fs4g_to_mg(accel_raw_data);
  210.     break;
  211.   case ISM330DHCX_8g:
  212.     accel_data_mg = ism330dhcx_from_fs8g_to_mg(accel_raw_data);
  213.     break;
  214.   case ISM330DHCX_16g:
  215.     accel_data_mg = ism330dhcx_from_fs16g_to_mg(accel_raw_data);
  216.     break;
  217.   default:
  218.     accel_data_mg = 0.0;
  219.     break;
  220.   }
  221. #endif
  222.   return accel_data_mg;
  223. }
代码编译,烧录到板卡中即可
5.演示
(审核通过之后上传)




飞思啦 发表于 2024-9-2 17:00 | 显示全部楼层
最近在研究单轴加速度计的单击、双击,这个还是有意思
v26g7l 发表于 2024-12-31 19:22 | 显示全部楼层
ISM330DHCX 是一款强大的传感器,结合了高精度的加速度计和陀螺仪
OKAKAKO 发表于 2024-12-31 22:10 | 显示全部楼层
六轴陀螺仪了解一下程序
LOVEEVER 发表于 2024-12-31 23:02 | 显示全部楼层
驱动六轴陀螺仪学习一下
您需要登录后才可以回帖 登录 | 注册

本版积分规则

16

主题

70

帖子

1

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

16

主题

70

帖子

1

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