VL53L0x TOF激光测距的 stm32 HAL库驱动代码

VL53L0x 是ST公司生成的 TOF激光测距传感器，得益于非常小巧的尺寸，很容易放入手机、无人机中，提供更加精准、稳定的测距方式。

 

本文代码，使用 STM32F4xx的 HAL库。

 

VL53L0x.c

#include "VL53L0x.h"
#include <math.h>
#include "PersonalMath.h"

struct VL53L0xData  VL53L0x_Bottom;		//底部连接在IIC2的VL53L0x的数据


void VL53L0x_init(I2C_HandleTypeDef *hi2c)
{	
	uint8_t VL53L0x_SendData[2] = {0x01};
	uint8_t VL53L0x_RecData[5] ;
	
/*	//Revision ID:
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_IDENTIFICATION_REVISION_ID, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData, 1, 10);
	//Device ID:
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_IDENTIFICATION_MODEL_ID, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData+1, 1, 10);
	//PRE_RANGE_CONFIG_VCSEL_PERIOD =
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData+2, 1, 10);
	//FINAL_RANGE_CONFIG_VCSEL_PERIOD=
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData+3, 1, 10);
	*/
	HAL_I2C_Mem_Write(hi2c, VL53L0x_add, VL53L0X_REG_SYSRANGE_START, I2C_MEMADD_SIZE_8BIT, VL53L0x_SendData, 1, 1);
	//HAL_Delay(500);
	
	VL53L0x_SendData[1] = 100;
	while(VL53L0x_SendData[1]--)
	{
		HAL_Delay(1);
		HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_RESULT_RANGE_STATUS, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData+4, 1, 1);
			if (VL53L0x_RecData[4] & 0x01) break;
	}
}

//检查VL53L0x是否正常
//检查成功返回0
//检查失败返回1
uint8_t VL53L0x_Check(I2C_HandleTypeDef *hi2c)
{	
	uint8_t VL53L0x_SendData[2] = {0x01};
	uint8_t VL53L0x_RecData[5] ;
	
	//Revision ID:
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_IDENTIFICATION_REVISION_ID, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData, 1, 1);
	//Device ID:
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_IDENTIFICATION_MODEL_ID, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData+1, 1, 1);
	//PRE_RANGE_CONFIG_VCSEL_PERIOD =
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData+2, 1, 1);
	//FINAL_RANGE_CONFIG_VCSEL_PERIOD=
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData+3, 1, 1);
	
	HAL_I2C_Mem_Write(hi2c, VL53L0x_add, VL53L0X_REG_SYSRANGE_START, I2C_MEMADD_SIZE_8BIT, VL53L0x_SendData, 1, 1);
	HAL_Delay(500);
	
	VL53L0x_SendData[1] = 100;
	while(VL53L0x_SendData[1]--)
	{
		HAL_Delay(10);
		HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_RESULT_RANGE_STATUS, I2C_MEMADD_SIZE_8BIT, VL53L0x_RecData+4, 1, 1);
			if (VL53L0x_RecData[4] & 0x01) break;
	}
	
	if (VL53L0x_RecData[4] & 0x01) 
	return 0;				//检查成功返回0
	else return 1;	//检查失败返回1
}

//VL53L0x转换一次
void VL53L0x_StartConversion(I2C_HandleTypeDef *hi2c)
{
	uint8_t VL53L0x_SendData[1] = {0x01};
	HAL_I2C_Mem_Write(hi2c, VL53L0x_add, VL53L0X_REG_SYSRANGE_START, I2C_MEMADD_SIZE_8BIT, VL53L0x_SendData, 1, 1);
}

uint16_t makeuint16(int lsb, int msb) 
{
    return ((msb & 0xFF) << 8) | (lsb & 0xFF);
}


//VL53L0x读取距离等数据反馈信息
void VL53L0x_ReadDistance(I2C_HandleTypeDef *hi2c, struct VL53L0xData *VL_temp)
{
	//记录历史有效值
	VL_temp->dist_last = VL_temp->distValid;
	
	HAL_I2C_Mem_Read(hi2c, VL53L0x_add, VL53L0X_REG_RESULT_RANGE_STATUS, I2C_MEMADD_SIZE_8BIT, VL_temp->vtemp, 12, 1);
	
	VL_temp->acnt = makeuint16(VL_temp->vtemp[7], VL_temp->vtemp[6]);
	VL_temp->scnt = makeuint16(VL_temp->vtemp[9], VL_temp->vtemp[8]);
	VL_temp->dist = makeuint16(VL_temp->vtemp[11], VL_temp->vtemp[10]);
	VL_temp->DeviceRangeStatusInternal = ((VL_temp->vtemp[0] & 0x78) >> 3);

	
	//提取有效值
	if(VL_temp->dist <= 0x0014)		//距离数据无效
		VL_temp->distValid = VL_temp->dist_last;
	else 	//有效
		VL_temp->distValid = VL_temp->dist;
	
/*	下面的代码具体干嘛的时间长忘记了，功能是根据对地倾角求得飞机的高度，大家可以直接删掉 */
	
	uint8_t i,j;
	
	for(i = 14;i>0;i--)
	{
		VL_temp->dist_buff[i] = VL_temp->dist_buff[i-1];
	}
	VL_temp->dist_buff[0] = VL_temp->distValid;
	
	i =0;
	j=0;
	for(i=0;i<6;i++)
	{
		if(VL_temp->dist_buff[i] >=2000)
			j++;
	}
	
	if(j >= 2)		// 1/3的溢出率
		VL_temp->distValidFinal = 2000;
	else		//从数组里筛选出最近的有效值
	{
		i=0;
		for(i=0;i<15;i++)
		{
			if(VL_temp->dist_buff[i] <2000)
				break;
		}
		VL_temp->distValidFinal = VL_temp->dist_buff[i];
	}
	
	VL_temp->GroundDis_last = VL_temp->GroundDis;
	VL_temp->GroundDis = (float)VL_temp->distValidFinal /1000;
	VL_temp->GroundDis = VL_temp->GroundDis *cos(ABS((PostureAngle.Pitch /180 *3.14159))) *cos(ABS((PostureAngle.Roll /180 *3.14159)));
}

 

VL53L0x.h

#ifndef __VL53L0X_H
#define __VL53L0X_H			    
#include "stm32f4xx_hal.h"
#include "i2c.h"

#include "Structure.h"
#include "Algorithm_filter.h"


#define VL_CS1_Enable 		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_2,(GPIO_PinState)1);	//左侧VL53L0X的PB2拉高使能
#define VL_CS1_Disable 		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_2,(GPIO_PinState)0);	//左侧VL53L0X的PB2拉低失能

#define VL_CS2_Enable 		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1,(GPIO_PinState)1);	//右侧VL53L0X的PB1拉高使能
#define VL_CS2_Disable 		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1,(GPIO_PinState)0);	//右侧VL53L0X的PB1拉低失能


//寄存器操作函数	 
#define VL53L0x_add 		0x52			//VL53L0x的IIC器件地址

#define VL53L0X_REG_IDENTIFICATION_MODEL_ID         0xc0
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID      0xc2
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD   0x50
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define VL53L0X_REG_SYSRANGE_START                  0x00
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS         0x13
#define VL53L0X_REG_RESULT_RANGE_STATUS             0x14


struct VL53L0xData {
	uint8_t vtemp[12];
	uint16_t acnt;		//环境统计,激光光强
	uint16_t scnt;		//信号数
	uint16_t dist;		//距离，单位mm	最原始数据
	uint8_t DeviceRangeStatusInternal;
	
	uint16_t dist_last;		//历史有效值，用来判断
	
	uint16_t distValid;		//原始距离值提取后的有效值
	uint16_t dist_buff[15];		//测距的滑窗缓存
	uint16_t distValidFinal;
	
	float GroundDis;
	float GroundDis_last;
	
	float Speed;
	float Speed_last;
	
	uint16_t distFilted;	//卡尔曼滤波后的距离值
	uint16_t distFilted_1;//卡尔曼后 + 一阶低通滤波
	
	uint8_t Flag_OverRange;	//超出量程标志位

/*	uint16_t dist_Original;	//原始距离值
	uint16_t dist_OriginalValid;	//原始距离值提取后的有效值
	uint16_t dist_OriginalValid_lsat;*/
};

extern struct VL53L0xData  VL53L0x_Bottom;		//底部连接在IIC2的VL53L0x的数据



void VL53L0x_init(I2C_HandleTypeDef *hi2c);

//检查VL53L0x是否正常
uint8_t VL53L0x_Check(I2C_HandleTypeDef *hi2c);

//VL53L0x转换一次
void VL53L0x_StartConversion(I2C_HandleTypeDef *hi2c);

//VL53L0x读取距离等数据反馈信息
void VL53L0x_ReadDistance(I2C_HandleTypeDef *hi2c, struct VL53L0xData *VL_temp);

void VL53L0x_DataFitting(void);

#endif

 

I2C2 在 STM32CubeMX 的配置：

 

感慨一小下

压了三年的箱底，可算翻出来了。

当时这款传感器比较新，ST提供的驱动是linux的，CubeMX和HAL库也刚步入正轨，所以借鉴了好多。Arduino的，博客的。

以前懒得码字，可现在学东西多路总容易忘记以前的东西，拿博客当云笔记用，发表出来兴许还能帮到别人，岂不美哉。

更好的 VL53L1x 早已上市，十分好这条产品线。等测距速度提上去，再与MEMS振镜结合，这不就Mini固态激光雷达嘛。