cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三

一、cube.AI实际项目应用

        接篇二,前文都是采用FP-AI-SENSING1案例和配套的B-L475E-IOT01A开发板来阐述的,而实际项目中,我们都是基于自身项目硬件平台来训练模型及部署模型的,我们仅仅需要cube.AI软件包(作为可调用库)来支持我们项目,不会强行采用FP-AI-SENSING1案例去收集数据及配套的B-L475E-IOT01A等硬件平台部署。

        回顾篇一,ST公司支持到如下图芯片型号,

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第1张图片

         在本文中,将采用STM32L496VGT6-ali开发板来部署cube.AI实现人工智能。STM32L496VGT6开发板已经集成了LSM6DSL传感器(三轴加速度计及三轴陀螺仪传感器),项目设想如下:

        1)通过LSM6DSL采集加速度数值(x/y/z三轴加速度)

        2)本文只采集三种姿态(开发板正面朝上,静止不动、左右移动、上下移动三种姿态)时的加速度数值,用来实现分类神经网络,三种姿态作为神经网络模型输出值(分类)

        3)每次输入读取三组加速度值(每组数据是读取x/y/z三轴的三个加速度值),共9个数值作为神经网络模型输入数据

        4)利用STM32L496VGT6开发板上的三个按钮,KEY0为静止不动姿态采集按键,KEY1为左右移动姿态采集按键,KEY2为上下移动姿态采集按键。

        5)通过串口打印输出采集数据信息,并通过串联助手连接获得采集日志并保存成TXT文件

        6)将记录数据文件转换为csv文件,通过keras框架,编写神经网络训练模型python项目,进行神经网络模型训练,并输出.h5训练模型文件

        7)通过cubeMX和cube.AI将h5神经网络模型转换为c语言神经网络模型

        8)将LSM6DSL实时采集数据推送给c语言神经网络模型API,进行神经网络计算,查看输出结果是否符合预期。

二、创建工程

        在CubeIDE上,基于STM32L496VGT6芯片,创建新工程STM32工程,并实现了串口lpuart1调试日志输出,三个按键KEY0~2和三个LED灯LED0~2的功能实现,并实现LSM6DSL传感器采集数据功能(I2C4),请参考本专栏博文:

        1)cubeIDE快速开发流程_ide 程序的开发过程_py_free的博客-CSDN博客

        2)cubeIDE开发, stm32调试信息串口通信输出显示_py_free的博客-CSDN博客_怎么实查看stm32串口输出

        3)cubeIDE开发,I2C协议采集传感器数据(SHTC1、LTR-553ALS、BMP280、LSM6DSL、MMC3680KJ)_ltr553 driver download_py_free的博客-CSDN博客

        现给出简要的配置及源码信息:

2.1 工程配置

        1)内核功能配置及RCC开启外部时钟支持

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第2张图片cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第3张图片

         2)开启LPUART1,并开启其中断支持

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第4张图片cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第5张图片

         3)开启I2C4,并开启其中断功能及DMA功能

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第6张图片

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第7张图片cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第8张图片

        4)配置GPIO引脚(三个按键及三个LED灯)

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第9张图片

         5)配置时钟树

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第10张图片

         6)引脚视图

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第11张图片

         7)工程配置,选择为每个外设生成独立的.h/.c文件

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第12张图片

         生成输出代码

2.2 外设代码设计

        禁用syscalls.c文件(右键进入文件属性设置页面)

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第13张图片

        在工程下,创建源目录ICore,在该目录下,如下图所示,创建子目录及外设驱动源文件

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第14张图片

         源码文件内容如下:

        1)key.h

#ifndef KEY_H_
#define KEY_H_

#include "main.h"
#include "gpio.h"

GPIO_PinState get_key0_val();
GPIO_PinState get_key1_val();
GPIO_PinState get_key2_val();

uint8_t KEY_0(void);
uint8_t KEY_1(void);
uint8_t KEY_2(void);

#endif /* KEY_H_ */

         key.c

#include "key.h"

GPIO_PinState get_key0_val()
{
	return HAL_GPIO_ReadPin(KEY0_GPIO_Port,KEY0_Pin);
};

GPIO_PinState get_key1_val()
{
	return HAL_GPIO_ReadPin(KEY1_GPIO_Port,KEY1_Pin);
};

GPIO_PinState get_key2_val()
{
	return HAL_GPIO_ReadPin(KEY2_GPIO_Port,KEY2_Pin);
};

uint8_t KEY_0(void)
{
	uint8_t a;
	a=0;//如果未进入按键处理,则返回0
	if(HAL_GPIO_ReadPin(KEY0_GPIO_Port,KEY0_Pin)==GPIO_PIN_RESET){//读按键接口的电平
		HAL_Delay(20);//延时去抖动
		if(HAL_GPIO_ReadPin(KEY0_GPIO_Port,KEY0_Pin)==GPIO_PIN_RESET){ //读按键接口的电平
			a=1;//进入按键处理,返回1
		}
	}
	while(HAL_GPIO_ReadPin(KEY0_GPIO_Port,KEY0_Pin)==GPIO_PIN_RESET); //等待按键松开
	return a;
}

uint8_t KEY_1(void)
{
	uint8_t a;
	a=0;//如果未进入按键处理,则返回0
	if(HAL_GPIO_ReadPin(KEY1_GPIO_Port,KEY1_Pin)==GPIO_PIN_RESET){//读按键接口的电平
		HAL_Delay(20);//延时去抖动
		if(HAL_GPIO_ReadPin(KEY1_GPIO_Port,KEY1_Pin)==GPIO_PIN_RESET){ //读按键接口的电平
			a=1;//进入按键处理,返回1
		}
	}
	while(HAL_GPIO_ReadPin(KEY1_GPIO_Port,KEY1_Pin)==GPIO_PIN_RESET); //等待按键松开
	return a;
}

uint8_t KEY_2(void)
{
	uint8_t a;
	a=0;//如果未进入按键处理,则返回0
	if(HAL_GPIO_ReadPin(KEY2_GPIO_Port,KEY2_Pin)==GPIO_PIN_RESET){//读按键接口的电平
		HAL_Delay(20);//延时去抖动
		if(HAL_GPIO_ReadPin(KEY2_GPIO_Port,KEY2_Pin)==GPIO_PIN_RESET){ //读按键接口的电平
			a=1;//进入按键处理,返回1
		}
	}
	while(HAL_GPIO_ReadPin(KEY2_GPIO_Port,KEY2_Pin)==GPIO_PIN_RESET); //等待按键松开
	return a;
}

         2) led.h

#ifndef LED_H_
#define LED_H_
#include "main.h"
#include "gpio.h"

void Toggle_led0();
void Toggle_led1();
void Toggle_led2();

void set_led0_val(GPIO_PinState PinState);
void set_led1_val(GPIO_PinState PinState);
void set_led2_val(GPIO_PinState PinState);

#endif /* LED_H_ */

         led.c

#include "led.h"

void Toggle_led0()
{
	HAL_GPIO_TogglePin(LED0_GPIO_Port,LED0_Pin);
}

void Toggle_led1()
{
	HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
}

void Toggle_led2()
{
	HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
}

void set_led0_val(GPIO_PinState PinState)
{
	HAL_GPIO_WritePin(LED0_GPIO_Port,LED0_Pin,PinState);
};

void set_led1_val(GPIO_PinState PinState)
{
	HAL_GPIO_WritePin(LED1_GPIO_Port,LED1_Pin,PinState);
};

void set_led2_val(GPIO_PinState PinState)
{
	HAL_GPIO_WritePin(LED2_GPIO_Port,LED2_Pin,PinState);
};

         3)print.h

#ifndef INC_RETARGET_H_
#define INC_RETARGET_H_

#include "stm32l4xx_hal.h"
#include "stdio.h"//用于printf函数串口重映射
#include 

void ResetPrintInit(UART_HandleTypeDef  *huart);

int _isatty(int fd);
int _write(int fd, char* ptr, int len);
int _close(int fd);
int _lseek(int fd, int ptr, int dir);
int _read(int fd, char* ptr, int len);
int _fstat(int fd, struct stat* st);

#endif /* INC_RETARGET_H_ */

         print.c

#include <_ansi.h>
#include <_syslist.h>
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include "print.h"

#if !defined(OS_USE_SEMIHOSTING)
#define STDIN_FILENO  0
#define STDOUT_FILENO 1
#define STDERR_FILENO 2

UART_HandleTypeDef *gHuart;

void ResetPrintInit(UART_HandleTypeDef *huart)  {
  gHuart = huart;
  /* Disable I/O buffering for STDOUT  stream, so that
   * chars are sent out as soon as they are  printed. */
  setvbuf(stdout, NULL, _IONBF, 0);
}
int _isatty(int fd) {
  if (fd >= STDIN_FILENO && fd <=  STDERR_FILENO)
    return 1;
  errno = EBADF;
  return 0;
}
int _write(int fd, char* ptr, int len) {
  HAL_StatusTypeDef hstatus;
  if (fd == STDOUT_FILENO || fd ==  STDERR_FILENO) {
    hstatus = HAL_UART_Transmit(gHuart,  (uint8_t *) ptr, len, HAL_MAX_DELAY);
    if (hstatus == HAL_OK)
      return len;
    else
      return EIO;
  }
  errno = EBADF;
  return -1;
}
int _close(int fd) {
  if (fd >= STDIN_FILENO && fd <=  STDERR_FILENO)
    return 0;
  errno = EBADF;
  return -1;
}
int _lseek(int fd, int ptr, int dir) {
  (void) fd;
  (void) ptr;
  (void) dir;
  errno = EBADF;
  return -1;
}
int _read(int fd, char* ptr, int len) {
  HAL_StatusTypeDef hstatus;
  if (fd == STDIN_FILENO) {
    hstatus = HAL_UART_Receive(gHuart,  (uint8_t *) ptr, 1, HAL_MAX_DELAY);
    if (hstatus == HAL_OK)
      return 1;
    else
      return EIO;
  }
  errno = EBADF;
  return -1;
}
int _fstat(int fd, struct stat* st) {
  if (fd >= STDIN_FILENO && fd <=  STDERR_FILENO) {
    st->st_mode = S_IFCHR;
    return 0;
  }
  errno = EBADF;
  return 0;
}

#endif //#if !defined(OS_USE_SEMIHOSTING)

        4) usart.h

#ifndef INC_USART_H_
#define INC_USART_H_

#include "stm32l4xx_hal.h" //HAL库文件声明
#include //用于字符串处理的库
#include "../print/print.h"//用于printf函数串口重映射

extern UART_HandleTypeDef hlpuart1;//声明LPUSART的HAL库结构体

#define HLPUSART_REC_LEN  256//定义LPUSART最大接收字节数

extern uint8_t  HLPUSART_RX_BUF[HLPUSART_REC_LEN];//接收缓冲,最大HLPUSART_REC_LEN个字节.末字节为换行符
extern uint16_t HLPUSART_RX_STA;//接收状态标记
extern uint8_t HLPUSART_NewData;//当前串口中断接收的1个字节数据的缓存


void  HAL_UART_RxCpltCallback(UART_HandleTypeDef  *huart);//串口中断回调函数声明

#endif /* INC_USART_H_ */

        usart.c

#include "usart.h"

uint8_t  HLPUSART_RX_BUF[HLPUSART_REC_LEN];//接收缓冲,最大HLPUSART_REC_LEN个字节.末字节为换行符
/*
 * bit15:接收到回车(0x0d)时设置HLPUSART_RX_STA|=0x8000;
 * bit14:接收溢出标志,数据超出缓存长度时,设置HLPUSART_RX_STA|=0x4000;
 * bit13:预留
 * bit12:预留
 * bit11~0:接收到的有效字节数目(0~4095)
 */
uint16_t HLPUSART_RX_STA=0;接收状态标记//bit15:接收完成标志,bit14:接收到回车(0x0d),bit13~0:接收到的有效字节数目
uint8_t HLPUSART_NewData;//当前串口中断接收的1个字节数据的缓存

void  HAL_UART_RxCpltCallback(UART_HandleTypeDef  *huart)//串口中断回调函数
{
	if(huart ==&hlpuart1)//判断中断来源(串口1:USB转串口)
    {
		if(HLPUSART_NewData==0x0d){//回车标记
     	  HLPUSART_RX_STA|=0x8000;//标记接到回车
		}else{
			if((HLPUSART_RX_STA&0X0FFF)

        5) LSM6DSL.h

#ifndef _LSM6DSL_H_
#define _LSM6DSL_H_

#include "main.h"

void LSM6DSL_init();
//
uint8_t LSM6DSL_acc_st_open(void);
uint8_t LSM6DSL_acc_st_close(void);

uint8_t LSM6DSL_gyro_st_open(void);
uint8_t LSM6DSL_gyro_st_close(void);

uint8_t LSM6DSL_acc_read(int32_t *x_data,int32_t *y_data,int32_t *z_data);
uint8_t LSM6DSL_gyro_read(int32_t *x_data,int32_t *y_data,int32_t *z_data);

#endif /* LSM6DSL_LSM6DSL_H_ */

        LSM6DSL.c,实现传感器的ID检验、软重置、模式设置、数据读取及转换功能。

#include 
#include "LSM6DSL.h"

extern I2C_HandleTypeDef hi2c4;

#define LSM6DSL_I2C_ADDR1 (0x6A)
#define LSM6DSL_I2C_ADDR2 (0x6B)
#define LSM6DSL_I2C_ADDR_TRANS(n) ((n) << 1)
#define LSM6DSL_I2C_ADDR LSM6DSL_I2C_ADDR_TRANS(LSM6DSL_I2C_ADDR2)

#define LSM6DSL_ACC_GYRO_FUNC_CFG_ACCESS 0x01
#define LSM6DSL_ACC_GYRO_SENSOR_SYNC_TIME 0x04
#define LSM6DSL_ACC_GYRO_SENSOR_RES_RATIO 0x05
#define LSM6DSL_ACC_GYRO_FIFO_CTRL1 0x06
#define LSM6DSL_ACC_GYRO_FIFO_CTRL2 0x07
#define LSM6DSL_ACC_GYRO_FIFO_CTRL3 0x08
#define LSM6DSL_ACC_GYRO_FIFO_CTRL4 0x09
#define LSM6DSL_ACC_GYRO_FIFO_CTRL5 0x0A
#define LSM6DSL_ACC_GYRO_DRDY_PULSE_CFG_G 0x0B
#define LSM6DSL_ACC_GYRO_INT1_CTRL 0x0D
#define LSM6DSL_ACC_GYRO_INT2_CTRL 0x0E
#define LSM6DSL_ACC_GYRO_WHO_AM_I_REG 0x0F
#define LSM6DSL_ACC_GYRO_CTRL1_XL 0x10
#define LSM6DSL_ACC_GYRO_CTRL2_G 0x11
#define LSM6DSL_ACC_GYRO_CTRL3_C 0x12
#define LSM6DSL_ACC_GYRO_CTRL4_C 0x13
#define LSM6DSL_ACC_GYRO_CTRL5_C 0x14
#define LSM6DSL_ACC_GYRO_CTRL6_C 0x15
#define LSM6DSL_ACC_GYRO_CTRL7_G 0x16
#define LSM6DSL_ACC_GYRO_CTRL8_XL 0x17
#define LSM6DSL_ACC_GYRO_CTRL9_XL 0x18
#define LSM6DSL_ACC_GYRO_CTRL10_C 0x19


#define LSM6DSL_ACC_GYRO_MASTER_CONFIG 0x1A
#define LSM6DSL_ACC_GYRO_WAKE_UP_SRC 0x1B
#define LSM6DSL_ACC_GYRO_TAP_SRC 0x1C
#define LSM6DSL_ACC_GYRO_D6D_SRC 0x1D
#define LSM6DSL_ACC_GYRO_STATUS_REG 0x1E

#define LSM6DSL_ACC_GYRO_OUT_TEMP_L 0x20
#define LSM6DSL_ACC_GYRO_OUT_TEMP_H 0x21
#define LSM6DSL_ACC_GYRO_OUTX_L_G 0x22
#define LSM6DSL_ACC_GYRO_OUTX_H_G 0x23
#define LSM6DSL_ACC_GYRO_OUTY_L_G 0x24
#define LSM6DSL_ACC_GYRO_OUTY_H_G 0x25
#define LSM6DSL_ACC_GYRO_OUTZ_L_G 0x26
#define LSM6DSL_ACC_GYRO_OUTZ_H_G 0x27
#define LSM6DSL_ACC_GYRO_OUTX_L_XL 0x28
#define LSM6DSL_ACC_GYRO_OUTX_H_XL 0x29
#define LSM6DSL_ACC_GYRO_OUTY_L_XL 0x2A
#define LSM6DSL_ACC_GYRO_OUTY_H_XL 0x2B
#define LSM6DSL_ACC_GYRO_OUTZ_L_XL 0x2C
#define LSM6DSL_ACC_GYRO_OUTZ_H_XL 0x2D
#define LSM6DSL_ACC_GYRO_SENSORHUB1_REG 0x2E
#define LSM6DSL_ACC_GYRO_SENSORHUB2_REG 0x2F
#define LSM6DSL_ACC_GYRO_SENSORHUB3_REG 0x30
#define LSM6DSL_ACC_GYRO_SENSORHUB4_REG 0x31
#define LSM6DSL_ACC_GYRO_SENSORHUB5_REG 0x32
#define LSM6DSL_ACC_GYRO_SENSORHUB6_REG 0x33
#define LSM6DSL_ACC_GYRO_SENSORHUB7_REG 0x34
#define LSM6DSL_ACC_GYRO_SENSORHUB8_REG 0x35
#define LSM6DSL_ACC_GYRO_SENSORHUB9_REG 0x36
#define LSM6DSL_ACC_GYRO_SENSORHUB10_REG 0x37
#define LSM6DSL_ACC_GYRO_SENSORHUB11_REG 0x38
#define LSM6DSL_ACC_GYRO_SENSORHUB12_REG 0x39
#define LSM6DSL_ACC_GYRO_FIFO_STATUS1 0x3A
#define LSM6DSL_ACC_GYRO_FIFO_STATUS2 0x3B
#define LSM6DSL_ACC_GYRO_FIFO_STATUS3 0x3C
#define LSM6DSL_ACC_GYRO_FIFO_STATUS4 0x3D
#define LSM6DSL_ACC_GYRO_FIFO_DATA_OUT_L 0x3E
#define LSM6DSL_ACC_GYRO_FIFO_DATA_OUT_H 0x3F
#define LSM6DSL_ACC_GYRO_TIMESTAMP0_REG 0x40
#define LSM6DSL_ACC_GYRO_TIMESTAMP1_REG 0x41
#define LSM6DSL_ACC_GYRO_TIMESTAMP2_REG 0x42

#define LSM6DSL_ACC_GYRO_TIMESTAMP_L 0x49
#define LSM6DSL_ACC_GYRO_TIMESTAMP_H 0x4A

#define LSM6DSL_ACC_GYRO_STEP_COUNTER_L 0x4B
#define LSM6DSL_ACC_GYRO_STEP_COUNTER_H 0x4C

#define LSM6DSL_ACC_GYRO_SENSORHUB13_REG 0x4D
#define LSM6DSL_ACC_GYRO_SENSORHUB14_REG 0x4E
#define LSM6DSL_ACC_GYRO_SENSORHUB15_REG 0x4F
#define LSM6DSL_ACC_GYRO_SENSORHUB16_REG 0x50
#define LSM6DSL_ACC_GYRO_SENSORHUB17_REG 0x51
#define LSM6DSL_ACC_GYRO_SENSORHUB18_REG 0x52

#define LSM6DSL_ACC_GYRO_FUNC_SRC 0x53
#define LSM6DSL_ACC_GYRO_TAP_CFG1 0x58
#define LSM6DSL_ACC_GYRO_TAP_THS_6D 0x59
#define LSM6DSL_ACC_GYRO_INT_DUR2 0x5A
#define LSM6DSL_ACC_GYRO_WAKE_UP_THS 0x5B
#define LSM6DSL_ACC_GYRO_WAKE_UP_DUR 0x5C
#define LSM6DSL_ACC_GYRO_FREE_FALL 0x5D
#define LSM6DSL_ACC_GYRO_MD1_CFG 0x5E
#define LSM6DSL_ACC_GYRO_MD2_CFG 0x5F

#define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_X_L 0x66
#define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_X_H 0x67
#define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_Y_L 0x68
#define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_Y_H 0x69
#define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_Z_L 0x6A
#define LSM6DSL_ACC_GYRO_OUT_MAG_RAW_Z_H 0x6B

#define LSM6DSL_ACC_GYRO_X_OFS_USR 0x73
#define LSM6DSL_ACC_GYRO_Y_OFS_USR 0x74
#define LSM6DSL_ACC_GYRO_Z_OFS_USR 0x75

#define LSM6DSL_CHIP_ID_VALUE (0x6A)

#define LSM6DSL_RESET_VALUE (0x1)
#define LSM6DSL_RESET_MSK (0X1)
#define LSM6DSL_RESET_POS (0)

#define LSM6DSL_ACC_ODR_POWER_DOWN (0X00)
#define LSM6DSL_ACC_ODR_1_6_HZ (0X0B)
#define LSM6DSL_ACC_ODR_12_5_HZ (0x01)
#define LSM6DSL_ACC_ODR_26_HZ (0x02)
#define LSM6DSL_ACC_ODR_52_HZ (0x03)
#define LSM6DSL_ACC_ODR_104_HZ (0x04)
#define LSM6DSL_ACC_ODR_208_HZ (0x05)
#define LSM6DSL_ACC_ODR_416_HZ (0x06)
#define LSM6DSL_ACC_ODR_833_HZ (0x07)
#define LSM6DSL_ACC_ODR_1_66_KHZ (0x08)
#define LSM6DSL_ACC_ODR_3_33_KHZ (0x09)
#define LSM6DSL_ACC_ODR_6_66_KHZ (0x0A)
#define LSM6DSL_ACC_ODR_MSK (0XF0)
#define LSM6DSL_ACC_ODR_POS (4)

#define LSM6DSL_GYRO_ODR_POWER_DOWN (0X00)
#define LSM6DSL_GYRO_ODR_12_5_HZ (0x01)
#define LSM6DSL_GYRO_ODR_26_HZ (0x02)
#define LSM6DSL_GYRO_ODR_52_HZ (0x03)
#define LSM6DSL_GYRO_ODR_104_HZ (0x04)
#define LSM6DSL_GYRO_ODR_208_HZ (0x05)
#define LSM6DSL_GYRO_ODR_416_HZ (0x06)
#define LSM6DSL_GYRO_ODR_833_HZ (0x07)
#define LSM6DSL_GYRO_ODR_1_66_KHZ (0x08)
#define LSM6DSL_GYRO_ODR_3_33_KHZ (0x09)
#define LSM6DSL_GYRO_ODR_6_66_KHZ (0x0A)
#define LSM6DSL_GYRO_ODR_MSK (0XF0)
#define LSM6DSL_GYRO_ODR_POS (4)

#define LSM6DSL_ACC_RANGE_2G (0x0)
#define LSM6DSL_ACC_RANGE_4G (0x2)
#define LSM6DSL_ACC_RANGE_8G (0x3)
#define LSM6DSL_ACC_RANGE_16G (0x1)
#define LSM6DSL_ACC_RANGE_MSK (0X0C)
#define LSM6DSL_ACC_RANGE_POS (2)

#define LSM6DSL_ACC_SENSITIVITY_2G (61)
#define LSM6DSL_ACC_SENSITIVITY_4G (122)
#define LSM6DSL_ACC_SENSITIVITY_8G (244)
#define LSM6DSL_ACC_SENSITIVITY_16G (488)

#define LSM6DSL_GYRO_RANGE_245 (0x0)
#define LSM6DSL_GYRO_RANGE_500 (0x1)
#define LSM6DSL_GYRO_RANGE_1000 (0x2)
#define LSM6DSL_GYRO_RANGE_2000 (0x3)
#define LSM6DSL_GYRO_RANGE_MSK (0X0C)
#define LSM6DSL_GYRO_RANGE_POS (2)

#define LSM6DSL_GYRO_SENSITIVITY_245DPS (8750)
#define LSM6DSL_GYRO_SENSITIVITY_500DPS (17500)
#define LSM6DSL_GYRO_SENSITIVITY_1000DPS (35000)
#define LSM6DSL_GYRO_SENSITIVITY_2000DPS (70000)

#define LSM6DSL_SHIFT_EIGHT_BITS (8)
#define LSM6DSL_16_BIT_SHIFT (0xFF)
#define LSM6DSL_ACC_MUL (1000)
#define LSM6DSL_GYRO_MUL (1)

#define LSM6DSL_ACC_DEFAULT_ODR_100HZ (100)
#define LSM6DSL_GYRO_DEFAULT_ODR_100HZ (100)

#define LSM6DSL_GET_BITSLICE(regvar, bitname) \
    ((regvar & bitname##_MSK) >> bitname##_POS)

#define LSM6DSL_SET_BITSLICE(regvar, bitname, val) \
    ((regvar & ~bitname##_MSK) | ((val << bitname##_POS) & bitname##_MSK))

typedef enum {
    ACC_RANGE_2G,
    ACC_RANGE_4G,
    ACC_RANGE_8G,
    ACC_RANGE_16G,
    ACC_RANGE_6G,
    ACC_RANGE_12G,
    ACC_RANGE_24G,
    ACC_RANGE_100G,
    ACC_RANGE_200G,
    ACC_RANGE_400G,
    ACC_RANGE_MAX
} acc_range_e;

typedef enum {
    GYRO_RANGE_125DPS,
    GYRO_RANGE_250DPS,
    GYRO_RANGE_500DPS,
    GYRO_RANGE_1000DPS,
    GYRO_RANGE_2000DPS,
    GYRO_RANGE_MAX
} gyro_range_e;

static int32_t lsm6dsl_acc_factor[ACC_RANGE_MAX] = {
    LSM6DSL_ACC_SENSITIVITY_2G, LSM6DSL_ACC_SENSITIVITY_4G,
    LSM6DSL_ACC_SENSITIVITY_8G, LSM6DSL_ACC_SENSITIVITY_16G
};
static int32_t lsm6dsl_gyro_factor[GYRO_RANGE_MAX] = {
    0, LSM6DSL_GYRO_SENSITIVITY_245DPS, LSM6DSL_GYRO_SENSITIVITY_500DPS,
    LSM6DSL_GYRO_SENSITIVITY_1000DPS, LSM6DSL_GYRO_SENSITIVITY_2000DPS
};

typedef enum {
    DEV_POWER_OFF = 0,
    DEV_POWER_ON,
    DEV_SLEEP,
    DEV_SUSPEND,
    DEV_DEEP_SUSPEND,
} LSM6DSL_power_mode;

static int32_t cur_acc_factor  = 0;
static int32_t cur_gyro_factor = 0;

uint8_t LSM6DSL_ID_check()
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	uint8_t addr_val[3] = {LSM6DSL_ACC_GYRO_WHO_AM_I_REG,0x00,LSM6DSL_CHIP_ID_VALUE};
	hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("get LSM6DSL ID error\r\n");
		return 1;
	}
	if(addr_val[1]!=addr_val[2]){
		printf("LSM6DSL validate_id is error\r\n");
		return 1;
	}
	printf("LSM6DSL_id:%02X\r\n",addr_val[1]);
	return 0;
}

uint8_t LSM6DSL_soft_reset()
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*first read*/
	uint8_t addr_val[2] = {LSM6DSL_ACC_GYRO_CTRL3_C,0x00};
	hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("get LSM6DSL ACC_GYRO_CTRL3_C error\r\n");
		return 1;
	}
	printf("LSM6DSL ACC_GYRO_CTRL3_C old:%02X\r\n",addr_val[1]);
	addr_val[1] |= LSM6DSL_RESET_VALUE;
	printf("LSM6DSL ACC_GYRO_CTRL3_C new:%02X\r\n",addr_val[1]);
	hi2c2_status = HAL_I2C_Mem_Write(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("set LSM6DSL ACC_GYRO_CTRL3_C error\r\n");
		return 1;
	}
	printf("successfully LSM6DSL soft reset\r\n");
	return 0;
}
/*
 * 以正数为例,最大可到32767,如果是Accelerometer数据,量程为2g的情况下,
 * 32768个刻度,一个刻度代表:2g/32768 = 2000mg/32767 = 0.061035mg
 * 例如:如果读出数据为16384,则加速度:16384x0.061035mg = 1000mg = 1g
 */
uint8_t LSM6DSL_acc_set_range(uint32_t range)
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*first read*/
	uint8_t addr_val[2] = {LSM6DSL_ACC_GYRO_CTRL1_XL,0x00};
	hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("get LSM6DSL acc range error\r\n");
		return 1;
	}
	uint8_t tmp   = 0;
    switch (range) {
    	case ACC_RANGE_2G: {
            tmp = LSM6DSL_ACC_RANGE_2G;
        } break;

        case ACC_RANGE_4G: {
            tmp = LSM6DSL_ACC_RANGE_4G;
        } break;

        case ACC_RANGE_8G: {
            tmp = LSM6DSL_ACC_RANGE_8G;
        } break;

        case ACC_RANGE_16G: {
            tmp = LSM6DSL_ACC_RANGE_16G;
        } break;

        default:
            break;
    }
    addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_ACC_RANGE, tmp);
    hi2c2_status = HAL_I2C_Mem_Write(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("set LSM6DSL acc range error\r\n");
		return 1;
	}
	if (range <= ACC_RANGE_16G) {
        cur_acc_factor = lsm6dsl_acc_factor[range];
    }
	printf("successfully LSM6DSL set acc range\r\n");
	return 0;
}

static uint8_t acc_st_lsm6dsl_hz2odr(uint32_t hz)
{
    if (hz > 3330)
        return LSM6DSL_ACC_ODR_6_66_KHZ;
    else if (hz > 1660)
        return LSM6DSL_ACC_ODR_3_33_KHZ;
    else if (hz > 833)
        return LSM6DSL_ACC_ODR_1_66_KHZ;
    else if (hz > 416)
        return LSM6DSL_ACC_ODR_833_HZ;
    else if (hz > 208)
        return LSM6DSL_ACC_ODR_416_HZ;
    else if (hz > 104)
        return LSM6DSL_ACC_ODR_208_HZ;
    else if (hz > 52)
        return LSM6DSL_ACC_ODR_104_HZ;
    else if (hz > 26)
        return LSM6DSL_ACC_ODR_52_HZ;
    else if (hz > 13)
        return LSM6DSL_ACC_ODR_26_HZ;
    else if (hz >= 2)
        return LSM6DSL_ACC_ODR_12_5_HZ;
    else
        return LSM6DSL_ACC_ODR_1_6_HZ;
}

uint8_t LSM6DSL_acc_set_odr(uint32_t hz)
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*first read*/
	uint8_t addr_val[2] = {LSM6DSL_ACC_GYRO_CTRL1_XL,0x00};
	hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("get LSM6DSL acc odr error\r\n");
		return 1;
	}
	uint8_t odr   = acc_st_lsm6dsl_hz2odr(hz);
	addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_ACC_ODR, odr);
	hi2c2_status = HAL_I2C_Mem_Write(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("set LSM6DSL acc odr error\r\n");
		return 1;
	}
	printf("successfully LSM6DSL set acc odr\r\n");
	return 0;
}

uint8_t LSM6DSL_acc_power_mode(LSM6DSL_power_mode mode)
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*first read*/
	uint8_t addr_val[2] = {LSM6DSL_ACC_GYRO_CTRL1_XL,0x00};
	hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("get LSM6DSL acc power_mode error\r\n");
		return 1;
	}
	switch (mode) {
		case DEV_POWER_ON: {
			addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_ACC_ODR,LSM6DSL_ACC_ODR_12_5_HZ);
		}
		break;
		case DEV_POWER_OFF: {
			addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_ACC_ODR,LSM6DSL_ACC_ODR_POWER_DOWN);
		}
		break;
		case DEV_SLEEP: {
			addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_ACC_ODR,LSM6DSL_ACC_ODR_12_5_HZ);
		}
		break;
		default:
			break;
	}
	hi2c2_status = HAL_I2C_Mem_Write(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("set LSM6DSL acc power_mode error\r\n");
		return 1;
	}
	printf("successfully LSM6DSL acc power_mode\r\n");
	return 0;
}

uint8_t LSM6DSL_acc_st_open(void)
{
	uint8_t ret = 0;

    ret = LSM6DSL_acc_power_mode( DEV_POWER_ON);
    if (ret>0) {
        return ret;
    }

    ret = LSM6DSL_acc_set_range(ACC_RANGE_8G);
    if (ret>0) {
        return ret;
    }

    ret = LSM6DSL_acc_set_odr(LSM6DSL_ACC_DEFAULT_ODR_100HZ);
    if (ret>0) {
        return ret;
    }
    printf("successfully LSM6DSL acc open\r\n");
    return 0;
}

uint8_t LSM6DSL_acc_st_close(void)
{
	uint8_t ret = 0;
    ret = LSM6DSL_acc_power_mode(DEV_POWER_OFF);
    if (ret>0) {
        return ret;
    }
    printf("successfully LSM6DSL acc close\r\n");
    return 0;
}

//LSM6DSL的满刻度加速度范围为±2/±4/±8/±16 g,角速度范围为±125/±250/±500/±1000/±2000 dps。
uint8_t LSM6DSL_gyro_set_range(uint32_t range)
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*first read*/
	uint8_t addr_val[2] = {LSM6DSL_ACC_GYRO_CTRL2_G,0x00};
	hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("get LSM6DSL gyro range error\r\n");
		return 1;
	}
	uint8_t tmp   = 0;
	switch (range) {
    	case GYRO_RANGE_250DPS: {
            tmp = LSM6DSL_GYRO_RANGE_245;
        } break;

        case GYRO_RANGE_500DPS: {
            tmp = LSM6DSL_GYRO_RANGE_500;
        } break;

        case GYRO_RANGE_1000DPS: {
            tmp = LSM6DSL_GYRO_RANGE_1000;
        } break;

        case GYRO_RANGE_2000DPS: {
            tmp = LSM6DSL_GYRO_RANGE_2000;
        } break;

        default:
            break;
	}
	addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_GYRO_RANGE, tmp);
    hi2c2_status = HAL_I2C_Mem_Write(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("set LSM6DSL gyro range error\r\n");
		return 1;
	}
	if ((range >= GYRO_RANGE_250DPS) && (range <= GYRO_RANGE_2000DPS)) {
        cur_gyro_factor = lsm6dsl_gyro_factor[range];
    }
	printf("successfully LSM6DSL set gyro range\r\n");
	return 0;
}

static uint8_t gyro_st_lsm6dsl_hz2odr(uint32_t hz)
{
    if (hz > 3330)
        return LSM6DSL_GYRO_ODR_6_66_KHZ;
    else if (hz > 1660)
        return LSM6DSL_GYRO_ODR_3_33_KHZ;
    else if (hz > 833)
        return LSM6DSL_GYRO_ODR_1_66_KHZ;
    else if (hz > 416)
        return LSM6DSL_GYRO_ODR_833_HZ;
    else if (hz > 208)
        return LSM6DSL_GYRO_ODR_416_HZ;
    else if (hz > 104)
        return LSM6DSL_GYRO_ODR_208_HZ;
    else if (hz > 52)
        return LSM6DSL_GYRO_ODR_104_HZ;
    else if (hz > 26)
        return LSM6DSL_GYRO_ODR_52_HZ;
    else if (hz > 13)
        return LSM6DSL_GYRO_ODR_26_HZ;
    else
        return LSM6DSL_GYRO_ODR_12_5_HZ;
}

uint8_t LSM6DSL_gyro_set_odr(uint32_t hz)
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*first read*/
	uint8_t addr_val[2] = {LSM6DSL_ACC_GYRO_CTRL2_G,0x00};
	hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("get LSM6DSL gyro odr error\r\n");
		return 1;
	}
	uint8_t odr = gyro_st_lsm6dsl_hz2odr(hz);
	addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_GYRO_ODR, odr);
	hi2c2_status = HAL_I2C_Mem_Write(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("set LSM6DSL gyro odr error\r\n");
		return 1;
	}
	printf("successfully LSM6DSL set gyro odr\r\n");
	return 0;
}

uint8_t LSM6DSL_gyro_power_mode(LSM6DSL_power_mode mode)
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*first read*/
	uint8_t addr_val[2] = {LSM6DSL_ACC_GYRO_CTRL2_G,0x00};
	hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("get LSM6DSL gyro power_mode error\r\n");
		return 1;
	}
	switch (mode) {
		case DEV_POWER_ON: {
			addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_GYRO_ODR,LSM6DSL_GYRO_ODR_12_5_HZ);
			break;
		}
		case DEV_POWER_OFF: {
			addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_GYRO_ODR,LSM6DSL_GYRO_ODR_POWER_DOWN);
			break;
		}
		case DEV_SLEEP: {
			addr_val[1] = LSM6DSL_SET_BITSLICE(addr_val[1], LSM6DSL_GYRO_ODR,LSM6DSL_GYRO_ODR_12_5_HZ);
			break;
		}
		default:
			break;
	}
	hi2c2_status = HAL_I2C_Mem_Write(&hi2c4,LSM6DSL_I2C_ADDR,addr_val[0],1,&addr_val[1],1,1000);
	if(HAL_OK!=hi2c2_status){
		printf("set LSM6DSL gyro power_mode error\r\n");
		return 1;
	}
	printf("successfully LSM6DSL gyro power_mode\r\n");
	return 0;
}

uint8_t LSM6DSL_gyro_st_open(void)
{
	uint8_t ret = 0;
    ret	= LSM6DSL_gyro_power_mode(DEV_POWER_ON);
    if (ret>0) {
        return 1;
    }

    ret = LSM6DSL_gyro_set_range(GYRO_RANGE_1000DPS);
    if (ret>0) {
        return 1;
    }

    ret = LSM6DSL_gyro_set_odr(LSM6DSL_GYRO_DEFAULT_ODR_100HZ);
    if (ret>0) {
        return 1;
    }
    printf("successfully LSM6DSL gyro open\r\n");
    return 0;
}

uint8_t LSM6DSL_gyro_st_close(void)
{
	uint8_t ret = 0;
    ret	= LSM6DSL_gyro_power_mode(DEV_POWER_OFF);
    if (ret>0) {
        return 1;
    }
    printf("successfully LSM6DSL gyro close\r\n");
    return 0;
}

void LSM6DSL_init()
{
	if(LSM6DSL_ID_check()>0)
		return;
	if(LSM6DSL_soft_reset()>0)
		return;
	if(LSM6DSL_acc_power_mode(DEV_POWER_OFF)>0)
		return;
	if(LSM6DSL_gyro_power_mode(DEV_POWER_OFF)>0)
			return;
	printf("successfully LSM6DSL init\r\n");
}

#define DATA_AXIS_X 0
#define DATA_AXIS_Y 1
#define DATA_AXIS_Z 2

uint8_t LSM6DSL_acc_read(int32_t *x_data,int32_t *y_data,int32_t *z_data)
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*read 0X28,0X29,0X2A,0X2B,0X2C,0X2D*/
	uint8_t addr[6] = {LSM6DSL_ACC_GYRO_OUTX_L_XL,LSM6DSL_ACC_GYRO_OUTX_H_XL,
			LSM6DSL_ACC_GYRO_OUTY_L_XL,LSM6DSL_ACC_GYRO_OUTY_H_XL,
			LSM6DSL_ACC_GYRO_OUTZ_L_XL,LSM6DSL_ACC_GYRO_OUTZ_H_XL};
	uint8_t val[6] = {0};
	for(uint8_t i=0; i<6; i++){
		hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr[i],1,&val[i],1,1000);
		if(HAL_OK!=hi2c2_status){
			printf("get LSM6DSL acc_read[0X%02X] error\r\n",addr[i]);
			return 1;
		}
	}
//	printf("read acc reg_data 1:%02X, 2:%02X, 3:%02X, 4:%02X, 5:%02X ,6:%02X\r\n"
//		    		,val[0],val[1],val[2],val[3],val[4],val[5]);
	int32_t data[3] = {0};
	data[DATA_AXIS_X] = (int16_t)((((int16_t)((int8_t)val[1])) << LSM6DSL_SHIFT_EIGHT_BITS) | (val[0]));
	data[DATA_AXIS_Y] = (int16_t)((((int16_t)((int8_t)val[3])) << LSM6DSL_SHIFT_EIGHT_BITS) | (val[2]));
	data[DATA_AXIS_Z] = (int16_t)((((int16_t)((int8_t)val[5])) << LSM6DSL_SHIFT_EIGHT_BITS) | (val[4]));
    if (cur_acc_factor != 0)
    {
        data[DATA_AXIS_X] = (data[DATA_AXIS_X] * cur_acc_factor) / LSM6DSL_ACC_MUL;
        data[DATA_AXIS_Y] = (data[DATA_AXIS_Y] * cur_acc_factor) / LSM6DSL_ACC_MUL;
        data[DATA_AXIS_Z] = (data[DATA_AXIS_Z] * cur_acc_factor) / LSM6DSL_ACC_MUL;
    }
//    printf("read acc cur_acc_factor:%ld, X:%ld,Y:%ld,Z:%ld\r\n"
//    		,cur_acc_factor,data[0],data[1],data[2]);
    *x_data = data[DATA_AXIS_X];
    *y_data = data[DATA_AXIS_Y];
    *z_data = data[DATA_AXIS_Z];
	return 0;
}

uint8_t LSM6DSL_gyro_read(int32_t *x_data,int32_t *y_data,int32_t *z_data)
{
	HAL_StatusTypeDef hi2c2_status = 0x00;
	/*read 0X22,0X23,0X24,0X25,0X26,0X27*/
	uint8_t addr[6] = {LSM6DSL_ACC_GYRO_OUTX_L_G,LSM6DSL_ACC_GYRO_OUTX_H_G,
			LSM6DSL_ACC_GYRO_OUTY_L_G,LSM6DSL_ACC_GYRO_OUTY_H_G,
			LSM6DSL_ACC_GYRO_OUTZ_L_G,LSM6DSL_ACC_GYRO_OUTZ_H_G};
	uint8_t val[6] = {0};
	for(uint8_t i=0; i<6; i++){
		hi2c2_status = HAL_I2C_Mem_Read(&hi2c4,LSM6DSL_I2C_ADDR,addr[i],1,&val[i],1,1000);
		if(HAL_OK!=hi2c2_status){
			printf("get LSM6DSL gyro_read[0X%02X] error\r\n",addr[i]);
			return 1;
		}
	}
//	printf("read gyro reg_data 1:%02X, 2:%02X, 3:%02X, 4:%02X, 5:%02X ,6:%02X\r\n"
//	    		,val[0],val[1],val[2],val[3],val[4],val[5]);
	int32_t data[3] = {0};
	data[DATA_AXIS_X] = (int16_t)((((int32_t)((int8_t)val[1])) << LSM6DSL_SHIFT_EIGHT_BITS) | (val[0]));
	data[DATA_AXIS_Y] = (int16_t)((((int32_t)((int8_t)val[3])) << LSM6DSL_SHIFT_EIGHT_BITS) | (val[2]));
	data[DATA_AXIS_Z] = (int16_t)((((int32_t)((int8_t)val[5])) << LSM6DSL_SHIFT_EIGHT_BITS) | (val[4]));

    if (cur_gyro_factor != 0) {
        data[DATA_AXIS_X] = (data[DATA_AXIS_X] * cur_gyro_factor) / LSM6DSL_GYRO_MUL;
        data[DATA_AXIS_Y] = (data[DATA_AXIS_Y] * cur_gyro_factor) / LSM6DSL_GYRO_MUL;
        data[DATA_AXIS_Z] = (data[DATA_AXIS_Z] * cur_gyro_factor) / LSM6DSL_GYRO_MUL;
    }
//    printf("read gyro cur_gyro_factor:%ld, X:%ld,Y:%ld,Z:%ld\r\n"
//    		,cur_gyro_factor,data[0],data[1],data[2]);
    *x_data = data[DATA_AXIS_X];
    *y_data = data[DATA_AXIS_Y];
    *z_data = data[DATA_AXIS_Z];
	return 0;
}

2.3 传感器数据采集与输出源码设计

        在main.c文件中,添加各个外设驱动头文件支持

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "../../ICore/key/key.h"
#include "../../ICore/led/led.h"
#include "../../ICore/print/print.h"
#include "../../ICore/usart/usart.h"
#include "../../ICore/LSM6DSL/LSM6DSL.h"
/* USER CODE END Includes */

        打印实时采集的三轴加速度信息

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void out_print(int32_t acc_x, int32_t acc_y, int32_t acc_z)
{
	if(acc_x>0)
		printf("%d.%d, ",(acc_x*98)/10000,((acc_x*98)%10000)/100);
	else
		printf("%d.%d, ",(acc_x*98)/10000,((-acc_x*98)%10000)/100);
	if(acc_y>0)
		printf("%d.%d, ",(acc_y*98)/10000,((acc_y*98)%10000)/100);
	else
		printf("%d.%d, ",(acc_y*98)/10000,((-acc_y*98)%10000)/100);
	if(acc_z>0)
		printf("%d.%d, ",(acc_z*98)/10000,((acc_z*98)%10000)/100);
	else
		printf("%d.%d, ",(acc_z*98)/10000,((-acc_z*98)%10000)/100);
}
/* USER CODE END 0 */

        在main函数中,初始化各个外设

int main(void)
{
  /* USER CODE BEGIN 1 */
    int32_t acc_x,acc_y,acc_z;
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_I2C4_Init();
  MX_LPUART1_UART_Init();
  /* USER CODE BEGIN 2 */
  ResetPrintInit(&hlpuart1);
  HAL_UART_Receive_IT(&hlpuart1,(uint8_t *)&HLPUSART_NewData, 1); //再开启接收中断
  HLPUSART_RX_STA = 0;
  //LSM6DSL
  LSM6DSL_init();
  LSM6DSL_acc_st_open();
  acc_x = acc_y = acc_z = 0;
  uint8_t menu = 0;
  uint8_t step_size = 3;
  /* USER CODE END 2 */

        在main函数循环体内,实现根据按键采集传感器数据(开发板正面朝上):

        1)保持开发板在桌面不动,按键KEY0按下时,采集静止不动姿态时的三轴加速度,并每采集三次,输出一次姿态结果[1,0,0],再次按下KEY0时停止采集

        2)保持开发板在桌面左右移动,按键KEY1按下时,采集左右移动姿态时的三轴加速度,并每采集三次,输出一次姿态结果[0,1,0],再次按下KEY1时停止采集

        3)保持开发板在桌面上上下移动(垂直方向),按键KEY2按下时,采集左右移动姿态时的三轴加速度,并每采集三次,输出一次姿态结果[0,0,1],再次按下KEY2时停止采集

 /* USER CODE BEGIN WHILE */
  while (1)
  {
	  if(HLPUSART_RX_STA&0xC000){//溢出或换行,重新开始
  		  printf("%.*s\r\n",HLPUSART_RX_STA&0X0FFF, HLPUSART_RX_BUF);
  		  HLPUSART_RX_STA=0;//接收错误,重新开始
  		  HAL_Delay(100);//等待
  	  }
	  if(KEY_0())
	  {
		  if(menu&0x01)
			  menu &= 0XFE;	//取消静止不动数据刷新
		  else{
			  menu |= 0X01; //开启静止不动数据刷新
		  }
		  menu &= 0XF9; //取消其他数据刷新
	  }
	  if(KEY_1())
	  {
		  if(menu&0x02)
			  menu &= 0XFD;	//取消左右移动数据刷新
		  else{
			  menu |= 0X02; //开启左右移动数据刷新
		  }
		  menu &= 0XFA; //取消其他数据刷新

	  }
	  if(KEY_2())
	  {
		  if(menu&0x04)
			  menu &= 0XFB; //取消上下移动数据刷新
		  else{
			  menu |= 0X04; //开启上下移动数据刷新
		  }
		  menu &= 0XFC;	//取消其他数据刷新

	  }
	  if(menu&0x01)//静止不动
  	  {
		  for(uint8_t i =0; i

2.4 编辑下载程序,采集数据

        编译程序及加载到开发板

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第15张图片

         打开串口助手,连接开发板,先清空屏幕,然后按上述功能操作进行数据采集,每种姿态采集大概一分钟的数据,完成后保存数据。

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第16张图片

         创建目录My_HAR_Study,将保存的txt文件拷贝到该目录,并将该文件修改为.csv后缀

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第17张图片

 三、模型训练

        在该目录下,创建myrun.py文件,内容如下:

#模型训练文件 myrun.py 训练 epochs 1000次
# myrun.py
'''
开发板(正面朝上)姿态检测
静止不动、左右移动、上下移动

输入层 -> 隐藏层 -> 输出层
'''

# 导入工具包
import pandas as pd
import numpy as np
from keras.models import Sequential
from keras.layers import Dense, Dropout
from keras.optimizers import SGD

# %% 读取数据
data = pd.read_csv('SaveWindows2023_1_28_16-31-14.csv', sep=',', header=None)
data_x = data.loc[:, 0:8]  # 取1~9列所有数据
data_y = data.loc[:, 9:11]
data_y.astype(int)
#
print("-x-")
print(data_x[0:2])
print("-y-")
print(data_y[0:2])

# %% 建立模型
model = Sequential()
# Dense(64) 是一个具有 64 个隐藏神经元的全连接层。
# 在第一层必须指定所期望的输入数据尺寸:
# 在这里,是一个 9 维的向量。
model.add(Dense(64, activation='relu', input_dim=9))
model.add(Dense(32, activation='relu'))
model.add(Dense(3, activation='softmax'))

sgd = SGD(lr=0.01, decay=1e-6, momentum=0.9, nesterov=True)
model.compile(loss='categorical_crossentropy',
              optimizer=sgd,
              metrics=['accuracy'])

model.fit(data_x, data_y,
          epochs=1000,
          batch_size=72)
score = model.evaluate(data_x, data_y, batch_size=72)

# 保存模型
model.save('myhar.h5')

         当前目录启动命令行工具,运行python3 .\myrun.py命令,

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第18张图片

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第19张图片

四、cube.AI配置及c模型生成

        回到数据采集工程(stm32L496VGT6_AI),双击.ioc打开cubeMX配置页面。

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第20张图片

         添加my_har模型,选择刚刚生成的keras模型文件(.h5),注意值生成模型,不需要应用程序。

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第21张图片

         分析结果显示,模型精度很差,意料之中,毕竟神经网络层只进行了简单设计,实现不了那么复杂姿态识别,但验证模型没有错误,支持转换,可以用来演示完开发流程就OK。

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第22张图片

         根据分析稍微调整一下heap和stack大小

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第23张图片

         生成输出代码如下图所示。

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第24张图片

五、模型调用及测试

        在项目属性设置页面,开启float支持

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第25张图片

          在main.c源文件中,添加AI模型库的头文件

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include 
#include "../../ICore/key/key.h"
#include "../../ICore/led/led.h"
#include "../../ICore/print/print.h"
#include "../../ICore/usart/usart.h"
#include "../../ICore/LSM6DSL/LSM6DSL.h"
#include "../../X-CUBE-AI/app/my_har.h"
#include "../../X-CUBE-AI/app/my_har_data.h"
/* USER CODE END Includes */

        在main.c源文件中,添加AI模型库支持函数

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void out_print(int32_t acc_x, int32_t acc_y, int32_t acc_z)
{
	if(acc_x>0)
		printf("%d.%d, ",(acc_x*98)/10000,((acc_x*98)%10000)/100);
	else
		printf("%d.%d, ",(acc_x*98)/10000,((-acc_x*98)%10000)/100);
	if(acc_y>0)
		printf("%d.%d, ",(acc_y*98)/10000,((acc_y*98)%10000)/100);
	else
		printf("%d.%d, ",(acc_y*98)/10000,((-acc_y*98)%10000)/100);
	if(acc_z>0)
		printf("%d.%d, ",(acc_z*98)/10000,((acc_z*98)%10000)/100);
	else
		printf("%d.%d, ",(acc_z*98)/10000,((-acc_z*98)%10000)/100);
}

/* Global handle to reference the instantiated C-model */
static ai_handle network = AI_HANDLE_NULL;

/* Global c-array to handle the activations buffer */
AI_ALIGNED(32)
static ai_u8 activations[AI_MY_HAR_DATA_ACTIVATIONS_SIZE];

AI_ALIGNED(32)
static ai_float in_data[AI_MY_HAR_IN_1_SIZE];

AI_ALIGNED(32)
static ai_float out_data[AI_MY_HAR_OUT_1_SIZE];

/* Array of pointer to manage the model's input/output tensors */
static ai_buffer *ai_input;
static ai_buffer *ai_output;
static ai_buffer_format fmt_input;
static ai_buffer_format fmt_output;

#define NSIZE 3

void buf_print(void)
{
	printf("in_data:");
	for (int i=0; idata)
		  printf(" @0x%X/%d \n",
	        (int)buffer->data,
	        (int)AI_BUFFER_BYTE_SIZE(AI_BUFFER_SIZE(buffer), fmt)
	    );
	  else
		  printf(" (User Domain)/%d \n",
	        (int)AI_BUFFER_BYTE_SIZE(AI_BUFFER_SIZE(buffer), fmt)
	    );
}

void aiPrintNetworkInfo(const ai_network_report report)
{
	printf("Model name      : %s\n", report.model_name);
	printf(" model signature : %s\n", report.model_signature);
	printf(" model datetime     : %s\r\n", report.model_datetime);
	printf(" compile datetime   : %s\r\n", report.compile_datetime);
	printf(" runtime version    : %d.%d.%d\r\n",
	      report.runtime_version.major,
	      report.runtime_version.minor,
	      report.runtime_version.micro);
	if (report.tool_revision[0])
		printf(" Tool revision      : %s\r\n", (report.tool_revision[0])?report.tool_revision:"");
	printf(" tools version      : %d.%d.%d\r\n",
	      report.tool_version.major,
	      report.tool_version.minor,
	      report.tool_version.micro);
	printf(" complexity         : %lu MACC\r\n", (unsigned long)report.n_macc);
	printf(" c-nodes            : %d\r\n", (int)report.n_nodes);

	printf(" map_activations    : %d\r\n", report.map_activations.size);
	  for (int idx=0; idx0){
		sprintf(buf_srt,"%d.%d, ",(acc_x*98)/10000,((acc_x*98)%10000)/100);
		((ai_float*)in_data)[NSIZE*index] =(float)atof(buf_srt);
	}else{
		sprintf(buf_srt,"%d.%d, ",(acc_x*98)/10000,((-acc_x*98)%10000)/100);
		((ai_float*)in_data)[NSIZE*index] =(float)atof(buf_srt);
	}
	if(acc_y>0){
		sprintf(buf_srt,"%d.%d, ",(acc_y*98)/10000,((acc_y*98)%10000)/100);
		((ai_float*)in_data)[NSIZE*index+1] =(float)atof(buf_srt);
	}else{
		sprintf(buf_srt,"%d.%d, ",(acc_y*98)/10000,((-acc_y*98)%10000)/100);
		((ai_float*)in_data)[NSIZE*index+1] =(float)atof(buf_srt);
	}
	if(acc_z>0){
		sprintf(buf_srt,"%d.%d, ",(acc_z*98)/10000,((acc_z*98)%10000)/100);
		((ai_float*)in_data)[NSIZE*index+2] =(float)atof(buf_srt);
	}else{
		sprintf(buf_srt,"%d.%d, ",(acc_z*98)/10000,((-acc_z*98)%10000)/100);
		((ai_float*)in_data)[NSIZE*index+2] =(float)atof(buf_srt);
	}
	return 0;
}
/*
 * Run inference
 */
int aiRun(const void *in_data, void *out_data) {
  ai_i32 n_batch;
  ai_error err;

  /* 1 - Update IO handlers with the data payload */
  ai_input[0].data = AI_HANDLE_PTR(in_data);
  ai_output[0].data = AI_HANDLE_PTR(out_data);

  /* 2 - Perform the inference */
  n_batch = ai_my_har_run(network, &ai_input[0], &ai_output[0]);
  if (n_batch != 1) {
	  err = ai_my_har_get_error(network);
	  printf("ai_error_type:%d,ai_error_code:%d\r\n",err.type,err.code);
  };

  return 0;
}

/* USER CODE END 0 */

        在main函数中初始化ai模型

  /* USER CODE BEGIN 2 */
  ResetPrintInit(&hlpuart1);
  HAL_UART_Receive_IT(&hlpuart1,(uint8_t *)&HLPUSART_NewData, 1); //再开启接收中断
  HLPUSART_RX_STA = 0;
  //LSM6DSL
  LSM6DSL_init();
  LSM6DSL_acc_st_open();
  acc_x = acc_y = acc_z = 0;
  uint8_t menu = 0;
  uint8_t step_size = NSIZE;
  //
  aiInit();
  buf_print();
  /* USER CODE END 2 */

        在main函数循环体中,通过串口lpuart1调试发送test,开启将实时数据推送给ai模型

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	  if(HLPUSART_RX_STA&0xC000){//溢出或换行,重新开始
  		  printf("%.*s\r\n",HLPUSART_RX_STA&0X0FFF, HLPUSART_RX_BUF);
  		if(strstr((const char*)HLPUSART_RX_BUF,(const char*)"test"))
		  {
  		      menu = 0x08;
		  }
  		  HLPUSART_RX_STA=0;//接收错误,重新开始
  		  HAL_Delay(100);//等待
  	  }
	  if(KEY_0())
	  {
		  if(menu&0x01)
			  menu &= 0XFE;	//取消静止不动数据刷新
		  else{
			  menu |= 0X01; //开启静止不动数据刷新
		  }
		  menu &= 0XF9; //取消其他数据刷新
	  }
	  if(KEY_1())
	  {
		  if(menu&0x02)
			  menu &= 0XFD;	//取消左右移动数据刷新
		  else{
			  menu |= 0X02; //开启左右移动数据刷新
		  }
		  menu &= 0XFA; //取消其他数据刷新

	  }
	  if(KEY_2())
	  {
		  if(menu&0x04)
			  menu &= 0XFB; //取消上下移动数据刷新
		  else{
			  menu |= 0X04; //开启上下移动数据刷新
		  }
		  menu &= 0XFC;	//取消其他数据刷新

	  }
	  if(menu&0x01)//静止不动
  	  {
		  for(uint8_t i =0; i

        编译及下载程序

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第26张图片

         串口助手通过lpuart1连接开发板,发送“test”,开启AI计算,静止不动开发板情况如下:

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第27张图片

         左右移动开发板测试输出:

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第28张图片

         上下移动(垂直方向)开发板测试输出:

cubeIDE开发, stm32人工智能开发应用实践(Cube.AI).篇三_第29张图片

        通过测试可以看出,基本能识别开发板的行为,若需要更准确的识别,更好数据采集方法,也可以更多姿态行为模式计算(开发板不同朝向、倾斜度等)

你可能感兴趣的:(STM32开发案例,stm32,cubeIDE,cubeMX,cube.AI,嵌入式人工智能)