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固态激光雷达嘛。