ST公司 Lis2dh12 三轴加速度传感器,计算加速度值转成角度值

概述

项目上使用了一款Lis2dh12三轴加速度传感器。开发前要准备的工作。

1)要引用官方提供SDK中的lis2dh12_reg.h文件、lis2dh12_reg.c文件,才行。

2)我使用的主控芯片是STM32L051C8T6,硬件IIC与lis2dh12通讯。

3)本次写的是Demo项目,使用工具STM32Cube IDE开发

1、原理图:

ST公司 Lis2dh12 三轴加速度传感器,计算加速度值转成角度值_第1张图片                        ST公司 Lis2dh12 三轴加速度传感器,计算加速度值转成角度值_第2张图片

ST公司 Lis2dh12 三轴加速度传感器,计算加速度值转成角度值_第3张图片

1.1、创建lis2dh12.c文件

#include "lis2dh12.h"
#include "lis2dh12_reg.h"
#include "i2c.h"
#include "stdlib.h"
#include "stdio.h"
#include 
#include 


#define LIS2DH12_FROM_FS_2g_HR_TO_mg(lsb)  (float)((int16_t)lsb>>4) * 1.0f
#define LIS2DH12_FROM_FS_4g_HR_TO_mg(lsb)  (float)((int16_t)lsb>>4) * 2.0f
#define LIS2DH12_FROM_FS_8g_HR_TO_mg(lsb)  (float)((int16_t)lsb>>4) * 4.0f
#define LIS2DH12_FROM_FS_16g_HR_TO_mg(lsb) (float)((int16_t)lsb>>4) * 12.0f 
#define LIS2DH12_FROM_LSB_TO_degC_HR(lsb)  (float)((int16_t)lsb>>6) / 4.0f+25.0f

 
axis_info_t acc_sample;
filter_avg_t acc_data;


/* ============================================================
func name   :  lis2_Delay_us
discription :  lis2 微妙延时
param       :  us
return      :  
Revision    : 
=============================================================== */
void lis2_Delay_us(uint32_t us)
{
	int n = 11;
	while(--us)
	{
	    while(--n);
	    n = 11;
	}
}

/* ============================================================
func name   :  lis2_Delay_ms
discription :  lis2 毫秒延时
param       :  ms
return      :  
Revision    : 
=============================================================== */
void lis2_Delay_ms(uint32_t ms)
{
	int i = 0;
	for(;i0x19 */
//	lis2dh12_iic_write_byte(0x33, 0x03);  /* INT1_DURATION(33h): 1LSB=1/ODR  如果ODR=50HZ   那么1LSB=20ms 设置延时 1s,对应50->0x32 */
  lis2dh12_iic_write_byte(0x33, 0x03);  	/* INT1_DURATION(33h): 1LSB=1/ODR  如果ODR=100HZ  那么1LSB=10ms 设置延时 1s,对应100->0x64 */

	
//	/* INT2 单击中断 */
////	lis2dh12_iic_write_byte(0x24, 0x01);	/* CTRL_REG5(24h):  */
//	lis2dh12_iic_write_byte(0x25, 0xa0);  /* CTRL_REG6(25h): Click interrupt on INT2 pin */
//
//	lis2dh12_iic_write_byte(0x38, 0x15);	/* CLICK_CFG (38h): 单击识别中断使能 */
////	lis2dh12_iic_write_byte(0x39, 0x10);
//	lis2dh12_iic_write_byte(0x3a, 0x7f);  /* CLICK_THS (3Ah): 单击阀值 */
//	lis2dh12_iic_write_byte(0x3b, 0xff);  /* TIME_LIMIT (3Bh): 时间限制窗口6 ODR 1LSB=1/ODR 1LSB=1/100HZ,10ms,设置延时1s,对应100—>0x64*/
//	lis2dh12_iic_write_byte(0x3c, 0xff);  /* TIME_LATENCY (3Ch): 中断电平持续时间1 ODR=10ms */
//	lis2dh12_iic_write_byte(0x3d, 0x01);  /* TIME_WINDOW (3Dh):  单击时间窗口 */
	
	/* Start sensor */
//	lis2dh12_iic_write_byte(0x20, 0x37);
	lis2dh12_iic_write_byte(0x20, 0x5f);  /* CTRL_REG1(20h): Start sensor at ODR 100Hz Low-power mode */
	
	return 0;
}


/* ============================================================
func name   :  get_acc_value
discription :  获取加速度值
param       :  axis_info_t *sample
return      :  void
Revision    : 
=============================================================== */
void get_acc_value(axis_info_t *sample)
{
	uint8_t i = 0;
	uint8_t data[6];
	for (i=0; i<6; i++){
		lis2dh12_iic_read_byte(0x28+i, data+i);		//获取X、y、z轴的数据
		//printf("data[i] %d, \r\n", data[i]);
	}
//  sample->x = abs((int)(LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)data))); 
//	sample->y = abs((int)(LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)(data+2))));
//	sample->z = abs((int)(LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)(data+4))));
	sample->x = LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)data); 
	sample->y = LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)(data+2));
	sample->z = LIS2DH12_FROM_FS_2g_HR_TO_mg(*(int16_t*)(data+4));
}

/* ============================================================
func name   :  filter_calculate
discription :  均值滤波器---滤波
			   读取xyz数据存入均值滤波器,存满进行计算,滤波后样本存入sample
param       :  filter_avg_t *filter, axis_info_t *sample
return      :  void
Revision    : 
=============================================================== */
void filter_calculate(filter_avg_t *filter, axis_info_t *sample)
{
	uint8_t i = 0;
	short x_sum = 0, y_sum = 0, z_sum = 0;	
	
	for (i=0; iinfo[i].x = sample->x;
		filter->info[i].y = sample->y;
		filter->info[i].z = sample->z;
		
		x_sum += filter->info[i].x;
		y_sum += filter->info[i].y;
		z_sum += filter->info[i].z;
		
		printf("acc_x:%d,  acc_y:%d,  acc_z:%d \n",filter->info[i].x,filter->info[i].y,filter->info[i].z);
	}
	sample->x = x_sum / FILTER_CNT;
	sample->y = y_sum / FILTER_CNT;
	sample->z = z_sum / FILTER_CNT;	
	printf("\r\n acc_info.acc_x:%d, acc_info.acc_y:%d, acc_info.acc_z:%d \r\n",sample->x, sample->y, sample->z);
}
 
 
/* ============================================================
func name   :  new_angle_calculate
discription :  计算新角度
param       :  axis_info_t *sample
return      :  void
Revision    : 
=============================================================== */
void new_angle_calculate(axis_info_t *sample)
{
	sample->new_angle_x = atan((short)sample->x/(short)sqrt(pow(sample->y, 2)+pow(sample->z, 2))) * DEGREE_CAL;
	sample->new_angle_y = atan((short)sample->y/(short)sqrt(pow(sample->x, 2)+pow(sample->z, 2))) * DEGREE_CAL;
	sample->new_angle_z = atan((short)sample->z/(short)sqrt(pow(sample->x, 2)+pow(sample->y, 2))) * DEGREE_CAL;
	if (sample->new_angle_z < 0)
	{
		sample->new_angle_x = 180-sample->new_angle_x;
		sample->new_angle_y = 180-sample->new_angle_y;
	}
	printf("sample->new_angle_x:%d, sample->new_angle_y:%d, sample->new_angle_z:%d \r\n",sample->new_angle_x, sample->new_angle_y, sample->new_angle_z);
}

/* ============================================================
func name   :  old_angle_calculate
discription :  计算旧角度
param       :  axis_info_t *sample
return      :  void
Revision    : 
=============================================================== */
void old_angle_calculate(axis_info_t *sample)
{
	sample->old_angle_x = atan((short)sample->x/(short)sqrt(pow(sample->y, 2)+pow(sample->z, 2))) * DEGREE_CAL;
	sample->old_angle_y = atan((short)sample->y/(short)sqrt(pow(sample->x, 2)+pow(sample->z, 2))) * DEGREE_CAL;
	sample->old_angle_z = atan((short)sample->z/(short)sqrt(pow(sample->x, 2)+pow(sample->y, 2))) * DEGREE_CAL;
	if (sample->old_angle_z < 0)
	{
		sample->old_angle_x = 180-sample->old_angle_x;
		sample->old_angle_y = 180-sample->old_angle_y;
	}
	printf("sample->old_angle_x:%d, sample->old_angle_y:%d, sample->old_angle_z:%d \r\n",sample->old_angle_x, sample->old_angle_y, sample->old_angle_z);
}

1.2、在此重点说明,如果想调传感器的中断灵敏度,注意:关注1、INT1_THS(32h),2、INT1_DURATION(33h)这两个寄存器即可。

INT1_THS(32h)

ST公司 Lis2dh12 三轴加速度传感器,计算加速度值转成角度值_第4张图片

1.3、计算阀值:主要是看配置中断的量程是哪个。

(1)如果量程 FS=2g 每1LSB=16mg  
那么寄存器配置INT1_THS=2 那么阈值=2*16mg
如果INT1_THS=5 那么阈值=5*16mg
 
(2)如果量程FS=4g,那么每1LSB=32mg 
INT1_THS =5 阈值就是5*32mg=160mg

ST公司 Lis2dh12 三轴加速度传感器,计算加速度值转成角度值_第5张图片

看配置的ODR是哪个,来计算延时的时间。

1LSB=1/ODR  如果ODR=50HZ  那么1LSB=20ms

/*
500mg = 30°
250mg = 15°
160mg = 10°
我现在对应的量程是FS=2g,INT1_THS=10,那么阀值=10*16mg
 
1LSB=1/ODR  如果ODR=50HZ  那么1LSB=20ms
50LSB   = 1s
100LSB = 2s 
*/

1.4、创建lis2dh12.h文件

#ifndef __LIS2DH12_H
#define __LIS2DH12_H

#ifdef __cplusplus
extern "C"
{
#endif

#include "stdint.h"

#define DEGREE_CAL 180.0/3.1416
#define FILTER_CNT 4


typedef struct {
  short x;
  short y;
  short z;
  short new_angle_x;
  short new_angle_y;
  short new_angle_z;
  short old_angle_x;
  short old_angle_y;
  short old_angle_z;
}axis_info_t;


typedef struct filter_avg{
  axis_info_t info[FILTER_CNT];
  unsigned char count;
}filter_avg_t;

extern axis_info_t acc_sample;
extern filter_avg_t acc_data;

void filter_calculate(filter_avg_t *filter, axis_info_t *sample);
void old_angle_calculate(axis_info_t *sample);
void new_angle_calculate(axis_info_t *sample);



#endif

1.5、main.c文件

/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"

#include "lis2dh12.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

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

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* 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_I2C1_Init();
  MX_USART1_UART_Init();
  /* USER CODE BEGIN 2 */
  filter_calculate(&acc_data, &acc_sample);
  old_angle_calculate(&acc_sample);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	 HAL_Delay(1000);
	 filter_calculate(&acc_data, &acc_sample);
	 new_angle_calculate(&acc_sample);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

1.6、demo运行结果如下: 

ST公司 Lis2dh12 三轴加速度传感器,计算加速度值转成角度值_第6张图片

代码:Git

你可能感兴趣的:(传感器)