[STM32L5] 【STM32L562E-DK试用】3D加速度计和陀螺仪测试

#define SENSOR_BUS hi2c1



/* Private macro -------------------------------------------------------------*/

#define    BOOT_TIME      10



/* Private variables ---------------------------------------------------------*/

static int16_t data_raw_acceleration[3];

static int16_t data_raw_angular_rate[3];

static int16_t data_raw_temperature;

static float_t acceleration_mg[3];

static float_t angular_rate_mdps[3];

static float_t temperature_degC;

static uint8_t whoamI, rst;

static uint8_t buffer[1000];

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

                              uint16_t len)

{

  HAL_I2C_Mem_Write(handle, LSM6DSO_I2C_ADD_L, reg,

                    I2C_MEMADD_SIZE_8BIT, (uint8_t*) bufp, len, 1000);

  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)

{

  HAL_I2C_Mem_Read(handle, LSM6DSO_I2C_ADD_L, reg,

                   I2C_MEMADD_SIZE_8BIT, bufp, len, 1000);

  return 0;

}

  stmdev_ctx_t dev_ctx;

  /* Initialize mems driver interface */

  dev_ctx.write_reg = platform_write;

  dev_ctx.read_reg = platform_read;

  dev_ctx.mdelay = platform_delay;

  dev_ctx.handle = &SENSOR_BUS;

  /* Init test platform */

  /* Wait sensor boot time */

  platform_delay(BOOT_TIME);

  /* Check device ID */

  lsm6dso_device_id_get(&dev_ctx, &whoamI);

  printf("LSM6DSO_ID=0x%x,whoamI=0x%x",LSM6DSO_ID,whoamI);

  if (whoamI != LSM6DSO_ID)

    while (1);



  /* Restore default configuration */

  lsm6dso_reset_set(&dev_ctx, PROPERTY_ENABLE);



  do {

    lsm6dso_reset_get(&dev_ctx, &rst);

  } while (rst);



  /* Disable I3C interface */

  lsm6dso_i3c_disable_set(&dev_ctx, LSM6DSO_I3C_DISABLE);

  /* Enable Block Data Update */

  lsm6dso_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

  /* Set Output Data Rate */

  lsm6dso_xl_data_rate_set(&dev_ctx, LSM6DSO_XL_ODR_12Hz5);

  lsm6dso_gy_data_rate_set(&dev_ctx, LSM6DSO_GY_ODR_12Hz5);

  /* Set full scale */

  lsm6dso_xl_full_scale_set(&dev_ctx, LSM6DSO_2g);

  lsm6dso_gy_full_scale_set(&dev_ctx, LSM6DSO_2000dps);

  /* Configure filtering chain(No aux interface)

   * Accelerometer - LPF1 + LPF2 path

   */

  lsm6dso_xl_hp_path_on_out_set(&dev_ctx, LSM6DSO_LP_ODR_DIV_100);

  lsm6dso_xl_filter_lp2_set(&dev_ctx, PROPERTY_ENABLE);

  LCD_UTILS_Drv_t lcdDrv;

  /* Initialize the LCD */

  BSP_LCD_Init(0, LCD_ORIENTATION_PORTRAIT);

  /* Set UTIL_LCD functions */

  lcdDrv.DrawBitmap  = BSP_LCD_DrawBitmap;

  lcdDrv.FillRGBRect = BSP_LCD_FillRGBRect;

  lcdDrv.DrawHLine   = BSP_LCD_DrawHLine;

  lcdDrv.DrawVLine   = BSP_LCD_DrawVLine;

  lcdDrv.FillRect    = BSP_LCD_FillRect;

  lcdDrv.GetPixel    = BSP_LCD_ReadPixel;

  lcdDrv.SetPixel    = BSP_LCD_WritePixel;

  lcdDrv.GetXSize    = BSP_LCD_GetXSize;

  lcdDrv.GetYSize    = BSP_LCD_GetYSize;

  lcdDrv.SetLayer    = BSP_LCD_SetActiveLayer;

  lcdDrv.GetFormat   = BSP_LCD_GetFormat;

  UTIL_LCD_SetFuncDriver(&lcdDrv);

  /* Clear the LCD */

  UTIL_LCD_Clear(UTIL_LCD_COLOR_WHITE);

  BSP_LCD_DisplayOn(0);

  UTIL_LCD_SetTextColor(UTIL_LCD_COLOR_BLUE);

  UTIL_LCD_SetBackColor(UTIL_LCD_COLOR_WHITE);

  UTIL_LCD_SetFont(&Font24);

  while (1)

  {

            uint8_t reg;

            /* Read output only if new xl value is available */

            lsm6dso_xl_flag_data_ready_get(&dev_ctx, ®);



            if (reg) {

              /* Read acceleration field data */

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

              lsm6dso_acceleration_raw_get(&dev_ctx, data_raw_acceleration);

              acceleration_mg[0] =

                lsm6dso_from_fs2_to_mg(data_raw_acceleration[0]);

              acceleration_mg[1] =

                lsm6dso_from_fs2_to_mg(data_raw_acceleration[1]);

              acceleration_mg[2] =

                lsm6dso_from_fs2_to_mg(data_raw_acceleration[2]);

//              printf("Acceleration [mg]:%4.2f,%4.2f,%4.2f\r\n",

//                      acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);

            }

            sprintf((char *)buffer," accX:%4.2f    ",acceleration_mg[0]);

            UTIL_LCD_DisplayStringAt(0, 10, (uint8_t *)buffer, LEFT_MODE);

            memset(buffer,0,sizeof(buffer));

            sprintf((char *)buffer," accY:%4.2f    ",acceleration_mg[1]);

                UTIL_LCD_DisplayStringAt(0, 40, (uint8_t *)buffer, LEFT_MODE);

                memset(buffer,0,sizeof(buffer));

                sprintf((char *)buffer," accZ:%4.2f    ",acceleration_mg[2]);

                UTIL_LCD_DisplayStringAt(0, 70, (uint8_t *)buffer, LEFT_MODE);

                memset(buffer,0,sizeof(buffer));

            lsm6dso_gy_flag_data_ready_get(&dev_ctx, ®);



            if (reg) {

              /* Read angular rate field data */

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

              lsm6dso_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate);

              angular_rate_mdps[0] =

                lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[0]);

              angular_rate_mdps[1] =

                lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[1]);

              angular_rate_mdps[2] =

                lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[2]);

//              printf("Angular rate [mdps]:%4.2f,%4.2f,%4.2f\r\n",

//                      angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);

            }

            sprintf((char *)buffer," angP:%4.2f    ",angular_rate_mdps[0]);

            UTIL_LCD_DisplayStringAt(0, 100, (uint8_t *)buffer, LEFT_MODE);

            memset(buffer,0,sizeof(buffer));

            sprintf((char *)buffer," angY:%4.2f    ",angular_rate_mdps[1]);

                UTIL_LCD_DisplayStringAt(0, 130, (uint8_t *)buffer, LEFT_MODE);

                memset(buffer,0,sizeof(buffer));

                sprintf((char *)buffer," angR:%4.2f    ",angular_rate_mdps[2]);

                UTIL_LCD_DisplayStringAt(0, 160, (uint8_t *)buffer, LEFT_MODE);

                memset(buffer,0,sizeof(buffer));



            lsm6dso_temp_flag_data_ready_get(&dev_ctx, ®);



            if (reg) {

              /* Read temperature data */

              memset(&data_raw_temperature, 0x00, sizeof(int16_t));

              lsm6dso_temperature_raw_get(&dev_ctx, &data_raw_temperature);

              temperature_degC =

                lsm6dso_from_lsb_to_celsius(data_raw_temperature);

//              printf("Temperature [degC]:%6.2f\r\n", temperature_degC);

            }

                sprintf((char *)buffer," Temp:%6.2f    ",temperature_degC);

                UTIL_LCD_DisplayStringAt(0, 190, (uint8_t *)buffer, LEFT_MODE);

                memset(buffer,0,sizeof(buffer));

    /* USER CODE END WHILE */



    /* USER CODE BEGIN 3 */

  }