【STEVAL-STWINKT1B测评】4.驱动六轴陀螺仪（Ism330）

只看该作者 · 2024-8-29 11:07

本帖最后由电子烂人于 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.初始化配置：

基础配置和IS3DWB别无二致，只需要改一下对应的引脚即可。

另外INT1/INT2这两个引脚是配置传感器内部寄存器的

经询问ST的FAE，这两个引脚不配置也可以正常工作，只是工作模式固定

具体的IO配置如图：

cube-mems包的配置同前一篇帖子，不再展示
4.代码分析：
定义加速度、陀螺仪的参数：

复制

#ifndef AXIS

  #define AXIS                          3                         /* Axis should be defined between 1 and 3 */

#endif

#ifndef SAMPLES

  #define SAMPLES                       256                       /* Should be between 16 & 4096 */

#endif

#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 */

#define FIFO_FULL                       512                       /* FIFO full size */

/************************************************************ Sensor type part ************************************************************/

#define ACCELEROMETER                                             /* Could be either ACCELEROMETER or GYROSCOPE */

/************************************************************ Sensors configuration part ************************************************************/

#ifdef ACCELEROMETER

  #ifndef ACCELEROMETER_ODR

    #define ACCELEROMETER_ODR           ISM330DHCX_XL_ODR_1666Hz  /* Shoud be between ISM330DHCX_XL_ODR_12Hz5 and ISM330DHCX_XL_ODR_6667Hz */

  #endif

  #ifndef ACCELEROMETER_FS

    #define ACCELEROMETER_FS            ISM330DHCX_2g             /* Should be between ISM330DHCX_2g and ISM330DHCX_8g */

  #endif

#else

  #ifndef GYROSCOPE_ODR

    #define GYROSCOPE_ODR               ISM330DHCX_GY_ODR_1666Hz  /* Shoud be between ISM330DHCX_GY_ODR_12Hz5 and ISM330DHCX_GY_ODR_6667Hz */

  #endif

  #ifndef GYROSCOPE_FS

    #define GYROSCOPE_FS                ISM330DHCX_2000dps        /* Should be between ISM330DHCX_125dps and ISM330DHCX_4000dps */

  #endif

#endif

/************************************************************ Datalogger / NEAI mode part ************************************************************/

#ifndef NEAI_MODE

  #define NEAI_MODE                     0                         /* 0: Datalogger mode, 1: NEAI functions mode */

#endif

#if (NEAI_MODE == 1)

  #ifndef NEAI_LEARN_NB

    #define NEAI_LEARN_NB               20                        /* Number of buffers to be learn by the NEAI library */

  #endif

#endif

定义部分

复制

/* USER CODE BEGIN PV */

static int16_t data_raw_acceleration[3];

static int16_t data_raw_angular_rate[3];

static uint8_t whoamI, rst;

uint8_t neai_similarity = 0, neai_state = 0;

volatile uint8_t drdy = 0;

uint16_t data_left = (uint16_t) SAMPLES, number_read = 0, neai_buffer_ptr = 0, neai_cnt = 0;

float neai_time = 0.0;

static float neai_buffer[AXIS * SAMPLES] = {0.0};

stmdev_ctx_t dev_ctx;





/* USER CODE END PV */

初始化要用到的函数：

复制

/* USER CODE BEGIN PV */

static int16_t data_raw_acceleration[3];

static int16_t data_raw_angular_rate[3];

static uint8_t whoamI, rst;

uint8_t neai_similarity = 0, neai_state = 0;

volatile uint8_t drdy = 0;

uint16_t data_left = (uint16_t) SAMPLES, number_read = 0, neai_buffer_ptr = 0, neai_cnt = 0;

float neai_time = 0.0;

static float neai_buffer[AXIS * SAMPLES] = {0.0};

stmdev_ctx_t dev_ctx;





/* USER CODE END PV */

复制

/* USER CODE BEGIN PFP */

static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len);

static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len);

static void ism330dhcx_initialize(void);

static void ism330dhcx_initialize_basics(void);

static void ism330dhcx_initialize_fifo(void);

static void ism330dhcx_get_buffer_from_fifo(uint16_t nb);

static float ism330dhcx_convert_gyro_data_to_mdps(int16_t gyro_raw_data);

static float ism330dhcx_convert_accel_data_to_mg(int16_t accel_raw_data);



/* USER CODE END PFP */

同前一篇帖子一样，定义platform

复制

/* USER CODE BEGIN 1 */

  /* Initialize mems driver interface */

  dev_ctx.write_reg = platform_write;

  dev_ctx.read_reg = platform_read;

  dev_ctx.handle = &hspi3;

  /* USER CODE END 1 */

在主循环中用FIFO采集加速度数据，并用串口打印出来。

复制

while (1)

  {

    uint8_t wtm_flag = 0, status2 = 0;

    uint16_t num = 0;

    if (drdy) {

      /* Reset data ready condition */

      drdy = 0;

      ism330dhcx_read_reg(&dev_ctx, ISM330DHCX_FIFO_STATUS2, &status2, 1);

      wtm_flag = status2 >> 7;

      if (wtm_flag) {

        ism330dhcx_fifo_data_level_get(&dev_ctx, &num);

        if (data_left < num) {

          num = data_left;

        }

        ism330dhcx_get_buffer_from_fifo(num);

        data_left -= num;

        number_read += num;

        if (data_left == 0) {

          ism330dhcx_fifo_mode_set(&dev_ctx, ISM330DHCX_BYPASS_MODE);

#if NEAI_MODE

          uint32_t cycles_cnt = 0;

          if (neai_cnt < NEAI_LEARN_NB) {

            neai_cnt++;

            KIN1_ResetCycleCounter();

            neai_state = neai_anomalydetection_learn(neai_buffer);

            cycles_cnt = KIN1_GetCycleCounter();

            neai_time = (cycles_cnt * 1000000.0) / HAL_RCC_GetSysClockFreq();

            printf("Learn: %d / %d. NEAI learn return: %d. Cycles counter: %ld = %.1f µs at %ld Hz.\n",

                  neai_cnt, NEAI_LEARN_NB, neai_state, cycles_cnt, neai_time, HAL_RCC_GetSysClockFreq());

          }

          else {

            KIN1_ResetCycleCounter();

            neai_state = neai_anomalydetection_detect(neai_buffer, &neai_similarity);

            cycles_cnt = KIN1_GetCycleCounter();

            neai_time = (cycles_cnt * 1000000.0) / HAL_RCC_GetSysClockFreq();

            printf("Similarity: %d / 100. NEAI detect return: %d. Cycles counter: %ld = %.1f µs at %ld Hz.\n",

                  neai_similarity, neai_state, cycles_cnt, neai_time, HAL_RCC_GetSysClockFreq());

          }

#else

          for (uint16_t i = 0; i < AXIS * SAMPLES; i++) {

            printf("%.3f ", neai_buffer[i]);

          }

          printf("\n");

#endif

          data_left = (uint16_t) SAMPLES;

          number_read = 0;

          memset(neai_buffer, 0x00, AXIS * SAMPLES * sizeof(float));

          if (SAMPLES <= MAX_FIFO_SIZE) {

            ism330dhcx_fifo_watermark_set(&dev_ctx, (uint16_t) SAMPLES);

          }

          else {

            ism330dhcx_fifo_watermark_set(&dev_ctx, (uint16_t) MAX_FIFO_SIZE);

          }

          ism330dhcx_fifo_mode_set(&dev_ctx, ISM330DHCX_STREAM_MODE);

        }

        else if (data_left < MAX_FIFO_SIZE) {

          ism330dhcx_fifo_watermark_set(&dev_ctx, data_left);

        }

      }

    }

    /* USER CODE END WHILE */



    /* USER CODE BEGIN 3 */

  }

  /* USER CODE END 3 */

另外把串口数据打印部分重定义一下：

复制

int __io_putchar(int ch)

{

 uint8_t c[1];

 c[0] = ch & 0x00FF;

 HAL_UART_Transmit(&huart2, &*c, 1, 10);

 return ch;

}

其他几个函数如写入，读取等直接使用例程：

复制

static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)

{

  HAL_GPIO_WritePin(CS_DHC_GPIO_Port, CS_DHC_Pin, GPIO_PIN_RESET);

  HAL_SPI_Transmit(handle, ®, 1, 1000);

  HAL_SPI_Transmit(handle, (uint8_t*) bufp, len, 1000);

  HAL_GPIO_WritePin(CS_DHC_GPIO_Port, CS_DHC_Pin, GPIO_PIN_SET);

  return 0;

}



/*

 * [url=home.php?mod=space&uid=247401]@brief[/url]  Read generic device register (platform dependent)

 *

 * @param  handle    customizable argument. In this examples is used in

 *                   order to select the correct sensor bus handler.

 * @param  reg       register to read

 * @param  bufp      pointer to buffer that store the data read

 * @param  len       number of consecutive register to read

 *

 */

static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)

{

  reg |= 0x80;

  HAL_GPIO_WritePin(CS_DHC_GPIO_Port, CS_DHC_Pin, GPIO_PIN_RESET);

  HAL_SPI_Transmit(handle, ®, 1, 1000);

  HAL_SPI_Receive(handle, bufp, len, 1000);

  HAL_GPIO_WritePin(CS_DHC_GPIO_Port, CS_DHC_Pin, GPIO_PIN_SET);

  return 0;

}



/*

 * [url=home.php?mod=space&uid=247401]@brief[/url]  Initialize ISM330DHCX sensor interface

 *

 * @param  No

 *

 * [url=home.php?mod=space&uid=266161]@return[/url] No

 *

 */

static void ism330dhcx_initialize()

{

  ism330dhcx_initialize_basics();

#ifdef ACCELEROMETER

  /* Accelelerometer configuration */

  ism330dhcx_xl_data_rate_set(&dev_ctx, ACCELEROMETER_ODR);

  ism330dhcx_xl_full_scale_set(&dev_ctx, ACCELEROMETER_FS);

#else

  /* Gyroscope configuration */

  ism330dhcx_gy_data_rate_set(&dev_ctx, GYROSCOPE_ODR);

  ism330dhcx_gy_full_scale_set(&dev_ctx, GYROSCOPE_FS);

#endif

  ism330dhcx_initialize_fifo();

}



/*

 * @brief  Initialize ISM330DHCX basics

 *

 * @param  No

 *

 * [url=home.php?mod=space&uid=266161]@return[/url] No

 *

 */

static void ism330dhcx_initialize_basics()

{

  /* Check device ID */

  whoamI = 0;



  do {

    /* Wait sensor boot time */

    HAL_Delay(10);

    ism330dhcx_device_id_get(&dev_ctx, &whoamI);

  } while (whoamI != ISM330DHCX_ID);



  /* Restore default configuration */

  ism330dhcx_reset_set(&dev_ctx, PROPERTY_ENABLE);



  do {

    ism330dhcx_reset_get(&dev_ctx, &rst);

  } while (rst);



  /* Start device configuration. */

  ism330dhcx_device_conf_set(&dev_ctx, PROPERTY_ENABLE);

}



/*

 * @brief  Initialize ISM330DHCX internal FIFO

 *

 * @param  No

 *

 * @return No

 *

 */

static void ism330dhcx_initialize_fifo()

{

#ifdef ACCELEROMETER

  /* Batch odr config */

  ism330dhcx_fifo_xl_batch_set(&dev_ctx, ACCELEROMETER_ODR);

  ism330dhcx_fifo_gy_batch_set(&dev_ctx, 0);

#else

  /* Batch odr config */

  ism330dhcx_fifo_xl_batch_set(&dev_ctx, 0);

  ism330dhcx_fifo_gy_batch_set(&dev_ctx, GYROSCOPE_ODR);

#endif

  /* FIFO MODE */

  ism330dhcx_fifo_mode_set(&dev_ctx, ISM330DHCX_BYPASS_MODE);

  HAL_Delay(10);

  ism330dhcx_fifo_mode_set(&dev_ctx, ISM330DHCX_STREAM_MODE);

  /* Watermark config */

  if (SAMPLES <= MAX_FIFO_SIZE) {

    ism330dhcx_fifo_watermark_set(&dev_ctx, (uint16_t) SAMPLES);

  }

  else {

    ism330dhcx_fifo_watermark_set(&dev_ctx, (uint16_t) MAX_FIFO_SIZE);

  }

  uint8_t ctrl = 0x08;

  ism330dhcx_write_reg(&dev_ctx, ISM330DHCX_INT1_CTRL, (uint8_t *) &ctrl, 1);

}



/*

 * @brief  Get accelerometer or gyroscope data from

 *         ISM330DHCX using the internal FIFO buffer

 *

 * @param  No

 *

 * @return No

 *

 */

static void ism330dhcx_get_buffer_from_fifo(uint16_t nb)

{

  static int16_t dummy[3];

  ism330dhcx_fifo_tag_t reg_tag;

  for (uint16_t i = 0; i < nb; i++) {

    /* Read FIFO tag */

    ism330dhcx_fifo_sensor_tag_get(&dev_ctx, ®_tag);

    if(reg_tag == ISM330DHCX_XL_NC_TAG) {

      memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));

      ism330dhcx_fifo_out_raw_get(&dev_ctx, (uint8_t *) data_raw_acceleration);

      for(uint8_t j = 0; j < AXIS; j++) {

        neai_buffer[(AXIS * neai_buffer_ptr) + (AXIS * i) + j] = ism330dhcx_convert_accel_data_to_mg(data_raw_acceleration[j]);

      }

    }

    else if(reg_tag == ISM330DHCX_GYRO_NC_TAG) {

      memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));

      ism330dhcx_fifo_out_raw_get(&dev_ctx, (uint8_t *) data_raw_angular_rate);

      for(uint8_t j = 0; j < AXIS; j++) {

        neai_buffer[(AXIS * neai_buffer_ptr) + (AXIS * i) + j] = ism330dhcx_convert_gyro_data_to_mdps(data_raw_angular_rate[j]);

      }

    }

    else {

      /* Flush unused samples */

      printf("Bad sensor tag: %d.\n", reg_tag);

      memset(dummy, 0x00, 3 * sizeof(int16_t));

      ism330dhcx_fifo_out_raw_get(&dev_ctx, (uint8_t *) dummy);

    }

  }

  neai_buffer_ptr += nb;

  if (neai_buffer_ptr == SAMPLES) {

    neai_buffer_ptr = 0;

  }

}



/*

 * @brief  Convert gyroscope raw data to milli degrees per second (mdps)

 *

 * @param  gyro_raw_data: which is gyroscope raw data

 *                        depending on the full scale selected

 *

 * @return The converted value in milli degrees per second (mdps)

 *

 */

static float ism330dhcx_convert_gyro_data_to_mdps(int16_t gyro_raw_data)

{

  float gyro_data_mdps = 0.0;

#ifdef GYROSCOPE

  switch (GYROSCOPE_FS)

  {

  case ISM330DHCX_125dps:

    gyro_data_mdps = ism330dhcx_from_fs125dps_to_mdps(gyro_raw_data);

    break;

  case ISM330DHCX_250dps:

    gyro_data_mdps = ism330dhcx_from_fs250dps_to_mdps(gyro_raw_data);

    break;

  case ISM330DHCX_500dps:

    gyro_data_mdps = ism330dhcx_from_fs500dps_to_mdps(gyro_raw_data);

    break;

  case ISM330DHCX_1000dps:

    gyro_data_mdps = ism330dhcx_from_fs1000dps_to_mdps(gyro_raw_data);

    break;

  case ISM330DHCX_2000dps:

    gyro_data_mdps = ism330dhcx_from_fs2000dps_to_mdps(gyro_raw_data);

    break;

  case ISM330DHCX_4000dps:

    gyro_data_mdps = ism330dhcx_from_fs4000dps_to_mdps(gyro_raw_data);

    break;

  default:

    gyro_data_mdps = 0.0;

    break;

  }

#endif

  return gyro_data_mdps;

}



/*

 * @brief  Convert accelerometer raw data to milli-G' (mg)

 *

 * @param  accel_raw_data: which is accelerometer raw data

 *                        depending on the full scale selected

 *

 * @return The converted value in milli-G' (mg)

 *

 */

static float ism330dhcx_convert_accel_data_to_mg(int16_t accel_raw_data)

{

  float accel_data_mg = 0.0;

#ifdef ACCELEROMETER

  switch (ACCELEROMETER_FS)

  {

  case ISM330DHCX_2g:

    accel_data_mg = ism330dhcx_from_fs2g_to_mg(accel_raw_data);

    break;

  case ISM330DHCX_4g:

    accel_data_mg = ism330dhcx_from_fs4g_to_mg(accel_raw_data);

    break;

  case ISM330DHCX_8g:

    accel_data_mg = ism330dhcx_from_fs8g_to_mg(accel_raw_data);

    break;

  case ISM330DHCX_16g:

    accel_data_mg = ism330dhcx_from_fs16g_to_mg(accel_raw_data);

    break;

  default:

    accel_data_mg = 0.0;

    break;

  }

#endif

  return accel_data_mg;

}

代码编译，烧录到板卡中即可
5.演示
(审核通过之后上传)

只看该作者 · 2024-9-2 17:00

最近在研究单轴加速度计的单击、双击，这个还是有意思