STM32多路VL53L0激光测距程序（含校准）CPU小白 -

#include "vl53l0.h"
VL53L0X_Dev_t vl53l0x_dev0;
VL53L0X_Dev_t vl53l0x_dev1;
VL53L0X_Dev_t vl53l0x_dev2;
VL53L0X_Dev_t vl53l0x_dev3;
VL53L0X_DeviceInfo_t vl53l0x_dev_info;
VL53L0X_RangingMeasurementData_t vl53l0x_data;
VL53L0X_Error vl53l0x_status;
void print_pal_error(VL53L0X_Error Status)
{
char buf[VL53L0X_MAX_STRING_LENGTH];
VL53L0X_GetPalErrorString(Status, buf); //得到错误码与字符串
printf("API Status: %i : %s\n", Status, buf);
}
void print_range_status(VL53L0X_RangingMeasurementData_t* pdata)
{
char buf[VL53L0X_MAX_STRING_LENGTH];
uint8_t RangeStatus;
// New Range Status: data is valid when pdata->RangeStatus = 0
RangeStatus = pdata->RangeStatus;
VL53L0X_GetRangeStatusString(RangeStatus, buf);
printf("Range Status: %i : %s\n", RangeStatus, buf);
}
uint16_t vl53l0x_start_single_test(VL53L0X_Dev_t *pdev,
VL53L0X_RangingMeasurementData_t *pdata)
{
int i=0,sum=0;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
if(vl53l0x_status != VL53L0X_ERROR_NONE)
return vl53l0x_status;
status = VL53L0X_PerformSingleRangingMeasurement(pdev, pdata); //VL53L0X执行单一测量范围
if(status != VL53L0X_ERROR_NONE){
printf("error:Call of VL53L0X_PerformSingleRangingMeasurement\n");
return status;
}
for(i=0;i<5;i++)
sum+=pdata->RangeMilliMeter;
pdata->RangeMilliMeter=sum/5;
return pdata->RangeMilliMeter;
}
/******************************************************************
函数功能：配置VL53L0X设备I2C地址
入口参数：dev:设备I2C参数结构体
newaddr:设备新I2C地址
返回值：错误状态
******************************************************************/
VL53L0X_Error vl53l0x_Addr_set(VL53L0X_Dev_t *dev,uint8_t newaddr)
{
uint16_t Id;
uint8_t FinalAddress;
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
u8 sta=0x00;
FinalAddress = newaddr;
if(FinalAddress==dev->I2cDevAddr)//新设备I2C地址与旧地址一致,直接退出
return VL53L0X_ERROR_NONE;
//在进行第一个寄存器访问之前设置I2C标准模式(400Khz)
Status = VL53L0X_WrByte(dev,0x88,0x00);
if(Status!=VL53L0X_ERROR_NONE)
{
sta=0x01;//设置I2C标准模式出错
goto set_error;
}
//尝试使用默认的0x52地址读取一个寄存器
Status = VL53L0X_RdWord(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
if(Status!=VL53L0X_ERROR_NONE)
{
sta=0x02;//读取寄存器出错
goto set_error;
}
if(Id == 0xEEAA)
{
//设置设备新的I2C地址
Status = VL53L0X_SetDeviceAddress(dev,FinalAddress);
if(Status!=VL53L0X_ERROR_NONE)
{
sta=0x03;//设置I2C地址出错
goto set_error;
}
//修改参数结构体的I2C地址
dev->I2cDevAddr = FinalAddress;
//检查新的I2C地址读写是否正常
Status = VL53L0X_RdWord(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
if(Status!=VL53L0X_ERROR_NONE)
{
sta=0x04;//新I2C地址读写出错
goto set_error;
}
}
set_error:
if(Status!=VL53L0X_ERROR_NONE)
{
print_pal_error(Status);//打印错误信息
}
if(sta!=0)
printf("sta:0x%x\r\n",sta);
return Status;
}
/******************************************************************
函数功能：VL53L0初始化
入口参数：VL53L0X_Dev_t *pMyDevice ,u8 vl53l0_x_id
设备结构体设备ID
返回值：错误状态
******************************************************************/
VL53L0X_Error vl53l0x_InitX(VL53L0X_Dev_t *pMyDevice ,u8 vl53l0_x_id)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE; //初始值赋值为0
pMyDevice->I2cDevAddr = 0x52; //iic地址 0x52是默认地址,要初始化必须先写0x52,才能初始化，之后再通过软件修改
pMyDevice->comms_type = 1; //选择IIC还是SPI iic=1；SPI=0
pMyDevice->comms_speed_khz = 400; //iic速率
//正点原子的VL53L0用户手册上写明了再次使能时地址会恢复为0x52,所以只能使能一次，设置好地址即可，这里是核心
switch(vl53l0_x_id)
{
case 0:
I2C_X0_HIGH();
Delay_Ms(1);
vl53l0x_Addr_set(pMyDevice,0x54);//设置第一个VL53L0X传感器I2C地址
break;
case 1:
I2C_X1_HIGH();
Delay_Ms(1);
vl53l0x_Addr_set(pMyDevice,0x56);//设置第二个VL53L0X传感器I2C地址
break;
case 2:
I2C_X2_HIGH();
Delay_Ms(1);
vl53l0x_Addr_set(pMyDevice,0x58);
break;
case 3:
I2C_X3_HIGH();
Delay_Ms(1);
vl53l0x_Addr_set(pMyDevice,0x5a);
break;
}
Status = VL53L0X_DataInit(pMyDevice); //设备的初始化，赋默认值
if(Status != VL53L0X_ERROR_NONE){ //判断如果状态不为0 打印错误信息
print_pal_error(Status);
return Status; //返回错误值可通过此值DEBUG查找错误位置
}
Status = VL53L0X_GetDeviceInfo(pMyDevice, &vl53l0x_dev_info); //读取给定设备的设备信息
if(Status != VL53L0X_ERROR_NONE){
print_pal_error(Status);
return Status;
}
/*
printf("VL53L0X_GetDeviceInfo:\n");
printf("Device Name : %s\n", vl53l0x_dev_info.Name); //设备名
printf("Device Type : %s\n", vl53l0x_dev_info.Type); //产品类型VL53L0X = 1, VL53L1 = 2
printf("Device ID : %s\n", vl53l0x_dev_info.ProductId); // 设备ID
printf("ProductRevisionMajor : %d\n", vl53l0x_dev_info.ProductRevisionMajor);
printf("ProductRevisionMinor : %d\n", vl53l0x_dev_info.ProductRevisionMinor);
if ((vl53l0x_dev_info.ProductRevisionMajor != 1) && (vl53l0x_dev_info.ProductRevisionMinor != 1)){
printf("Error expected cut 1.1 but found cut %d.%d\n",
vl53l0x_dev_info.ProductRevisionMajor, vl53l0x_dev_info.ProductRevisionMinor);
Status = VL53L0X_ERROR_NOT_SUPPORTED;
print_pal_error(Status);
return Status;
} */
// Device Initialization
Status = VL53L0X_StaticInit(pMyDevice); //基本设备初始化
if(Status != VL53L0X_ERROR_NONE){ //判断是否初始化成功失败打印错误信息
printf ("Call of VL53L0X_StaticInit\n");
print_pal_error(Status);
return Status;
}
return Status;
}
/******************************************************************
函数功能：sPANS参考校准
入口参数：VL53L0X_Dev_t *pMyDevice ,Device_Message *pDev
设备结构体设备信息
返回值：错误状态
******************************************************************/
VL53L0X_Error PerformRefSpad_CalibrationX(VL53L0X_Dev_t *pMyDevice,Device_Message *pDev)
{
uint32_t refSpadCount;
uint8_t isApertureSpads;
VL53L0X_Error Status = VL53L0X_ERROR_NONE; //执行sPANS参考校准
Status = VL53L0X_PerformRefSpadManagement(pMyDevice,&refSpadCount, &isApertureSpads);
if(Status != VL53L0X_ERROR_NONE){
printf ("Call of VL53L0X_PerformRefSpadManagement\n");
printf ("refSpadCount = %d, isApertureSpads = %d\n", refSpadCount, isApertureSpads);
print_pal_error(Status);
return Status;
}
pDev->refSpadCount = refSpadCount;
pDev->isApertureSpads = isApertureSpads;
return Status;
}
/******************************************************************
函数功能：Ref校准
入口参数：VL53L0X_Dev_t *pMyDevice ,Device_Message *pDev
设备结构体设备信息
返回值：错误状态
******************************************************************/
VL53L0X_Error PerformRef_CalibrationX(VL53L0X_Dev_t *pMyDevice,Device_Message *pDev)
{
uint8_t VhvSettings;
uint8_t PhaseCal;
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
// Device Initialization //执行温度参考校准
Status = VL53L0X_PerformRefCalibration(pMyDevice, &VhvSettings,&PhaseCal);
if(Status != VL53L0X_ERROR_NONE){
printf ("Call of VL53L0X_PerformRefCalibration\n");
print_pal_error(Status);
return Status;
}
pDev->VhvSettings = VhvSettings; //更新结构体
pDev->PhaseCal = PhaseCal;
return Status;
}
/******************************************************************
函数功能：Offset校准
入口参数：VL53L0X_Dev_t *pMyDevice ,Device_Message *pDev
设备结构体设备信息
返回值：错误状态
******************************************************************/
VL53L0X_Error Offset_CalibrationX(VL53L0X_Dev_t *pMyDevice,Device_Message *pDev)
{
int32_t OffsetMicroMeter;
VL53L0X_Error Status = VL53L0X_ERROR_NONE; //初始值赋值为0
Status=VL53L0X_PerformOffsetCalibration(pMyDevice,100<<16,&OffsetMicroMeter); //偏移校准，把白色物体放在100mm处进行，输出偏移校准值
if(Status != VL53L0X_ERROR_NONE)
{
print_pal_error(Status);
return Status;
}
pDev->OffsetMicroMeter = OffsetMicroMeter;
return Status;
}
/******************************************************************
函数功能：XTalk校准
入口参数：VL53L0X_Dev_t *pMyDevice ,u8 vl53l0_x_id
设备结构体设备ID
返回值：错误状态
******************************************************************/
VL53L0X_Error XTalk_CalibrationX(VL53L0X_Dev_t *pMyDevice,Device_Message *pDev)
{
FixPoint1616_t XTalkCompensation;
VL53L0X_Error Status = VL53L0X_ERROR_NONE; //初始值赋值为0
Status=VL53L0X_PerformXTalkCalibration(pMyDevice,250<<16,&XTalkCompensation); //偏移校准，把白色物体放在100mm处进行，输出偏移校准值
if(Status != VL53L0X_ERROR_NONE)
{
print_pal_error(Status);
return Status;
}
pDev->XTalkCompensation = XTalkCompensation;
return Status;
}
//校准参数设置
VL53L0X_Error vl53l0x_Set_InitX(VL53L0X_Dev_t *pMyDevice,Device_Message *pDev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
//SPAD校准参数设置
Status = VL53L0X_SetReferenceSpads(pMyDevice,pDev->refSpadCount,pDev->isApertureSpads);
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
//温度校准参数设置
Status = VL53L0X_SetRefCalibration(pMyDevice,pDev->VhvSettings,pDev->PhaseCal);
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
//偏移校准参数设置
Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(pMyDevice,pDev->OffsetMicroMeter);
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
//盖玻片校准参数设置
Status = VL53L0X_SetXTalkCompensationRateMegaCps(pMyDevice,pDev->XTalkCompensation);
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
//使能串扰校准
Status = VL53L0X_SetXTalkCompensationEnable(pMyDevice,ENABLE);
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
return Status;
}
VL53L0X_Error vl53l0x_Set_ModeX(VL53L0X_Dev_t *pMyDevice)
{
// no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode 设置单一测量模式
if(Status != VL53L0X_ERROR_NONE){
printf ("Call of VL53L0X_SetDeviceMode\n");
print_pal_error(Status);
return Status;
}
// Enable/Disable Sigma and Signal check
if(Status == VL53L0X_ERROR_NONE){Status = VL53L0X_SetLimitCheckEnable(pMyDevice, //启用/禁用特定限制检查
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
}
if(Status == VL53L0X_ERROR_NONE){Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
}
if(Status == VL53L0X_ERROR_NONE){ Status = VL53L0X_SetLimitCheckValue(pMyDevice,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
(FixPoint1616_t)(0.25*65536));
}
if(Status == VL53L0X_ERROR_NONE){ Status = VL53L0X_SetLimitCheckValue(pMyDevice,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
(FixPoint1616_t)(18*65536));
}
if(Status == VL53L0X_ERROR_NONE){ Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice, //设置微秒时间
200000);
}
if(Status != VL53L0X_ERROR_NONE){
printf ("Sigma and Signal check error\n");
print_pal_error(Status);
return Status;
}
return Status; //返回0
}
/******************************************************************
函数功能：设备初始化·
入口参数：无
返回值：错误参数
******************************************************************/
VL53L0X_Error vl53l0x_Init(void)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE; //初始值赋值为0
//初始化一定按照这个顺序执行，否则不成功
VL53L0X_i2c_init();
vl53l0x_InitX(&vl53l0x_dev0,0);
vl53l0x_InitX(&vl53l0x_dev1,1);
vl53l0x_InitX(&vl53l0x_dev2,2);
vl53l0x_InitX(&vl53l0x_dev3,3);
return Status; //返回0
}
/******************************************************************
函数功能：参数赋值程序
入口参数：无
返回值：错误参数
******************************************************************/
VL53L0X_Error vl53l0x_Set_Init(void)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE; //初始值赋值为0
vl53l0x_Set_InitX(&vl53l0x_dev0,&Device0_Calibration_Message);
vl53l0x_Set_InitX(&vl53l0x_dev1,&Device1_Calibration_Message);
vl53l0x_Set_InitX(&vl53l0x_dev2,&Device2_Calibration_Message);
vl53l0x_Set_InitX(&vl53l0x_dev3,&Device3_Calibration_Message);
if(Status != VL53L0X_ERROR_NONE)
{
print_pal_error(Status);
return Status;
}
return Status;
}
/******************************************************************
函数功能：设置模式
入口参数：无
返回值：错误参数
******************************************************************/
VL53L0X_Error vl53l0x_Set_Mode(void)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
Status = vl53l0x_Set_ModeX(&vl53l0x_dev0);
Status = vl53l0x_Set_ModeX(&vl53l0x_dev1);
Status = vl53l0x_Set_ModeX(&vl53l0x_dev2);
Status = vl53l0x_Set_ModeX(&vl53l0x_dev3);
if(Status != VL53L0X_ERROR_NONE){
printf ("Call of VL53L0X_SetDeviceMode\n");
print_pal_error(Status);
return Status;
}
return Status;
}
/******************************************************************
函数功能：校准程序
入口参数：无
返回值：错误参数
******************************************************************/
VL53L0X_Error vl53l0x_Calibration_Init(void)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
if(Host_Configuration.calibration_marker == 0xaa)
{
Status = vl53l0x_Set_Init();
USB_USART_SendData(Host_Configuration.calibration_marker); //设备数据设定
}
else
{ usb_printf("等待偏移校准！");
while(1){Dispose_Usb_Data();
if(Host_Configuration.calibration_marker == 0x0a){break;}}; //等待偏移校准
Status = PerformRefSpad_Calibration();
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
Status = PerformRef_Calibration();
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
Status = Offset_Calibration();
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
usb_printf("等待载玻片校准！");
while(1){Dispose_Usb_Data();
if(Host_Configuration.calibration_marker == 0xaa){break;}}; //等待载玻片校准
Status = XTalk_Calibration();
if(Status!=VL53L0X_ERROR_NONE) { return Status; }
usb_printf("校准成功！");
Save_Device_Data_To_Flash(&Device0_Calibration_Message,0); //保存校准后参数
Save_Device_Data_To_Flash(&Device1_Calibration_Message,1);
Save_Device_Data_To_Flash(&Device2_Calibration_Message,2);
Save_Device_Data_To_Flash(&Device3_Calibration_Message,3);
}
vl53l0x_Set_Mode();
usb_printf("设备启动！");
return Status;
}
//SPAD校准程序
VL53L0X_Error PerformRefSpad_Calibration(void)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
//SPAD校准
PerformRefSpad_CalibrationX(&vl53l0x_dev0,&Device0_Calibration_Message);
PerformRefSpad_CalibrationX(&vl53l0x_dev1,&Device1_Calibration_Message);
PerformRefSpad_CalibrationX(&vl53l0x_dev2,&Device2_Calibration_Message);
PerformRefSpad_CalibrationX(&vl53l0x_dev3,&Device3_Calibration_Message);
if(Status != VL53L0X_ERROR_NONE)
{
print_pal_error(Status);
return Status;
}
return Status;
}
//温度校准程序
VL53L0X_Error PerformRef_Calibration(void)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
//温度校准
PerformRef_CalibrationX(&vl53l0x_dev0,&Device0_Calibration_Message);
PerformRef_CalibrationX(&vl53l0x_dev1,&Device1_Calibration_Message);
PerformRef_CalibrationX(&vl53l0x_dev2,&Device2_Calibration_Message);
PerformRef_CalibrationX(&vl53l0x_dev3,&Device3_Calibration_Message);
if(Status != VL53L0X_ERROR_NONE)
{
print_pal_error(Status);
return Status;
}
return Status;
}
//偏移校准程序
VL53L0X_Error Offset_Calibration(void)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE; //初始值赋值为0
//偏移校准
Offset_CalibrationX(&vl53l0x_dev0,&Device0_Calibration_Message);
Offset_CalibrationX(&vl53l0x_dev1,&Device1_Calibration_Message);
Offset_CalibrationX(&vl53l0x_dev2,&Device2_Calibration_Message);
Offset_CalibrationX(&vl53l0x_dev3,&Device3_Calibration_Message);
if(Status != VL53L0X_ERROR_NONE)
{
print_pal_error(Status);
return Status;
}
return Status;
}
//串扰校准程序
VL53L0X_Error XTalk_Calibration(void)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
//串扰校准
XTalk_CalibrationX(&vl53l0x_dev0,&Device0_Calibration_Message);
XTalk_CalibrationX(&vl53l0x_dev1,&Device1_Calibration_Message);
XTalk_CalibrationX(&vl53l0x_dev2,&Device2_Calibration_Message);
XTalk_CalibrationX(&vl53l0x_dev3,&Device3_Calibration_Message);
if(Status != VL53L0X_ERROR_NONE)
{
print_pal_error(Status);
return Status;
}
return Status;
}
/******************************************************************
函数功能：获取四个传感器测量的高度
入口参数：times ：滤波次数
返回值：无
******************************************************************/
void Vl53l0x_Get_Distance(u8 times) //times不易过大为好
{
static u8 m=0;
Data_Mover(vl5310_1_value_filter,LEFT,times);
Data_Mover(vl5310_2_value_filter,LEFT,times);
Data_Mover(vl5310_3_value_filter,LEFT,times);
Data_Mover(vl5310_4_value_filter,LEFT,times);
for(;m<times;m++) //读取距离数据
{
vl5310_1_value_filter[m]= vl53l0x_start_single_test(&vl53l0x_dev0, &vl53l0x_data);
vl5310_2_value_filter[m]= vl53l0x_start_single_test(&vl53l0x_dev1, &vl53l0x_data);
vl5310_3_value_filter[m]= vl53l0x_start_single_test(&vl53l0x_dev2, &vl53l0x_data);
vl5310_4_value_filter[m]= vl53l0x_start_single_test(&vl53l0x_dev3, &vl53l0x_data);
}
m--; //下次执行此函数时只用读取一次即可
Send_Information.first_vl53l0_value = Buf_Add(vl5310_1_value_filter,times)/times; //更新传感器的值
Send_Information.second_vl53l0_value= Buf_Add(vl5310_2_value_filter,times)/times;
Send_Information.third_vl53l0_value = Buf_Add(vl5310_3_value_filter,times)/times;
Send_Information.fourth_vl53l0_value= Buf_Add(vl5310_4_value_filter,times)/times;
}

复制代码