VL53L5CX Multi-Zone Sensor get x,y,z of points relative to origin?

[VBhat.1]

Hi, I was trying to create a small 8x8 / 4x4 point cloud using a VL53L5CX sensor, and I can get the depth data for each zone, but I was wondering if there was a way to get x,y,z from this information? Like if there was a way to know the vertical and horizontal angle for every zone on the VL53L5CX and then calculate the x,y,z location of the points using that and the depth data.

Any help would be appreciated!

[John E KVAM]

Yes there is.

You can assume (in 8x8 mode) that the origin is exactly perpendicular to the sensor.

The code below should do what you want.

Sorry we didn't do the 4x4 case, but you can work out below what we did and the 4x4 case should be clear.

• john

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include "vl53l5cx_api.h"
#include "math.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 */
int status;
volatile int IntCount;
uint8_t p_data_ready;
VL53L5CX_Configuration 	Dev;
VL53L5CX_ResultsData 	Results;
uint8_t resolution, isAlive;
uint16_t idx;
 
const double VL53L5_Zone_Pitch8x8[64] = {
		59.00,64.00,67.50,70.00,70.00,67.50,64.00,59.00,
		64.00,70.00,72.90,74.90,74.90,72.90,70.00,64.00,
		67.50,72.90,77.40,80.50,80.50,77.40,72.90,67.50,
		70.00,74.90,80.50,85.75,85.75,80.50,74.90,70.00,
		70.00,74.90,80.50,85.75,85.75,80.50,74.90,70.00,
		67.50,72.90,77.40,80.50,80.50,77.40,72.90,67.50,
		64.00,70.00,72.90,74.90,74.90,72.90,70.00,64.00,
		59.00,64.00,67.50,70.00,70.00,67.50,64.00,59.00
};
 
const double VL53L5_Zone_Yaw8x8[64] = {
		135.00,125.40,113.20, 98.13, 81.87, 66.80, 54.60, 45.00,
		144.60,135.00,120.96,101.31, 78.69, 59.04, 45.00, 35.40,
		156.80,149.04,135.00,108.45, 71.55, 45.00, 30.96, 23.20,
		171.87,168.69,161.55,135.00, 45.00, 18.45, 11.31,  8.13,
		188.13,191.31,198.45,225.00,315.00,341.55,348.69,351.87,
		203.20,210.96,225.00,251.55,288.45,315.00,329.04,336.80,
		203.20,225.00,239.04,258.69,281.31,300.96,315.00,324.60,
		225.00,234.60,246.80,261.87,278.13,293.20,305.40,315.00
};
 
 
PlaneEquation_t PlaneEquation;
XYZ_ZoneCoordinates_t XYZ_ZoneCoordinates;
 
double SinOfPitch[64], CosOfPitch[64], SinOfYaw[64], CosOfYaw[64];
/* USER CODE END PV */
 
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
 
 
 
uint8_t ComputeSinCosTables(void)
{
	//This function will save the math processing time of the code.  If the user wishes to not
	//perform this function, these tables can be generated and saved as a constant.
	uint8_t ZoneNum;
	for (ZoneNum = 0; ZoneNum < 64; ZoneNum++)
	{
		SinOfPitch[ZoneNum] = sin((VL53L5_Zone_Pitch8x8[ZoneNum])*Pi/180);
		CosOfPitch[ZoneNum] = cos((VL53L5_Zone_Pitch8x8[ZoneNum])*Pi/180);
		SinOfYaw[ZoneNum] = sin(VL53L5_Zone_Yaw8x8[ZoneNum]*Pi/180);
		CosOfYaw[ZoneNum] = cos(VL53L5_Zone_Yaw8x8[ZoneNum]*Pi/180);
	}
 
	return 0;
}
 
uint8_t ConvertDist2XYZCoords8x8(VL53L5CX_ResultsData *ResultsData, XYZ_ZoneCoordinates_t *XYZ_ZoneCoordinates)
{
	uint8_t ZoneNum;
	float Hyp;
	for (ZoneNum = 0; ZoneNum < 64; ZoneNum++)
	{
		if ((ResultsData->nb_target_detected[ZoneNum] > 0) && (ResultsData->distance_mm[ZoneNum] > 0) && ((ResultsData->target_status[ZoneNum] == 5) || (ResultsData->target_status[ZoneNum] == 6) || (ResultsData->target_status[ZoneNum] == 9)) )
		{
			Hyp = ResultsData->distance_mm[ZoneNum]/SinOfPitch[ZoneNum];
			XYZ_ZoneCoordinates->Xpos[ZoneNum] = CosOfYaw[ZoneNum]*CosOfPitch[ZoneNum]*Hyp;
			XYZ_ZoneCoordinates->Ypos[ZoneNum] = SinOfYaw[ZoneNum]*CosOfPitch[ZoneNum]*Hyp;
			XYZ_ZoneCoordinates->Zpos[ZoneNum] = ResultsData->distance_mm[ZoneNum];
		}
		else
		{
			XYZ_ZoneCoordinates->Xpos[ZoneNum] = 0;
			XYZ_ZoneCoordinates->Ypos[ZoneNum] = 0;
			XYZ_ZoneCoordinates->Zpos[ZoneNum] = 0;
		}
	}
	return 0;
}
 
uint8_t PrintXYZCoords(XYZ_ZoneCoordinates_t *XYZ_ZoneCoordinates)
{
	uint8_t i, j;
	printf("XYZ Coordinates for the target in each zone\n");
	for (i = 0; i < 8; i++) \
	{
		for (j = 0; j < 8; j++)
		{
			idx = (i * 8 + j);
			printf("%5.0f, %5.0f, %5.0f |",XYZ_ZoneCoordinates->Xpos[idx],XYZ_ZoneCoordinates->Ypos[idx],XYZ_ZoneCoordinates->Zpos[idx]);
		}
		printf("\n");
	}
	printf("\n");
 
	return 0;
}

[MMcAl.1]

Could you provide code examples for the model with larger FOV: VL53L7CX?

Thanks in advance!

[John E KVAM]

you alread have it!

The L7 is identical in every way - save for the optics that make the FoV.

So the L5 code you have will work.

Or you can download a new version and there is an #ifdef that selects between the L5 and the L7.

The calculation of the radial to perpendicular has come different coeffecents.

But that is the only difference.

Chip ID might be different as well.

(Look for the #define - if you don't see it, get an updated driver.)

• john

[MMcAl.1]

Thanks for your fast reply.

If I understand you correctly, the updated coefficients could be found somewhere in sample code. Can you give me a hint where I can find this specific code to convert the TOF data to a point cloud? I am not able to find it in the downloads available online: STSW-IMG036, STSW-IMG037, STSW-IMG038 (https://www.st.com/en/imaging-and-photonics-solutions/vl53l7cx.html#tools-software).

Thanks!

[John E KVAM]

I don't think the guy who wrote that L5 code translated it into the L7. Although I can ask him.

Not sure how quickly I can get that for you however.

This is NOT something that's in the STSW-IMG packages.

[John E KVAM]

In the code above insert the following conditional.

But the guy who did the work did minimal verification. It may not be perfect, but it's very close.

(At least it matches the data I took.)

So use this and if it's not perfect, let me know.

• john

#ifdef VL53L5
const double VL53L5_Zone_Pitch8x8[64] = {
		59.00,64.00,67.50,70.00,70.00,67.50,64.00,59.00,
		64.00,70.00,72.90,74.90,74.90,72.90,70.00,64.00,
		67.50,72.90,77.40,80.50,80.50,77.40,72.90,67.50,
		70.00,74.90,80.50,85.75,85.75,80.50,74.90,70.00,
		70.00,74.90,80.50,85.75,85.75,80.50,74.90,70.00,
		67.50,72.90,77.40,80.50,80.50,77.40,72.90,67.50,
		64.00,70.00,72.90,74.90,74.90,72.90,70.00,64.00,
		59.00,64.00,67.50,70.00,70.00,67.50,64.00,59.00
};
#else	//VL53L7
const double VL53L5_Zone_Pitch8x8[64] = {
		52.00,56.00,61.00,64.00,64.00,61.00,56.00,52.00,
		56.00,62.00,67.00,70.00,70.00,67.00,62.00,56.00,
		61.00,67.00,76.00,78.00,78.00,76.00,67.00,61.00,
		64.00,70.00,78.00,84.00,84.00,78.00,70.00,64.00,
		64.00,70.00,78.00,84.00,84.00,78.00,70.00,64.00,
		61.00,67.00,76.00,78.00,78.00,76.00,67.00,61.00,
		56.00,62.00,67.00,70.00,70.00,67.00,62.00,56.00,
		52.00,56.00,61.00,64.00,64.00,61.00,56.00,52.00
};
#endif

[MMcAl.1]

Hi john,

thank you very much for your fast response!

At first sight, the point cloud looks good!

[DSchi.4]

Hi John,

Thanks for sharing this code. If I try to compile the code, I can't find the declaration for

1. PlaneEquation_t PlaneEquation;
2. XYZ_ZoneCoordinates_t XYZ_ZoneCoordinates;

In which API is the type defined?

Best regards,

Daniel

[John E KVAM]

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : VL53L5_Plane_Algo.h
  * @brief          : Header for main.c file.
  *                   This file contains the common defines of the application.
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
 
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __VL53L5_Plane_Algo_H
#define __VL53L5_Plane_Algo_H
 
#ifdef __cplusplus
extern "C" {
#endif
 
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "vl53l5cx_api.h"
#include <stdio.h>
#include "math.h"
 
 
 
#define Pi 3.14156
 
typedef struct
{
	double Ax;
	double By;
	double Cz;
	double Offset;
	double XZ_Angle;
	double YZ_Angle;
}PlaneEquation_t;
 
typedef struct
{
	double Xpos[64];
	double Ypos[64];
	double Zpos[64];
}XYZ_ZoneCoordinates_t;
 
uint8_t Plane_Detection8x8(XYZ_ZoneCoordinates_t *XYZ, PlaneEquation_t *PlaneEq, uint16_t Accuracy_mm);
uint8_t ConvertDist2XYZCoords8x8(VL53L5CX_ResultsData *ResultsData, XYZ_ZoneCoordinates_t *XYZ_ZoneCoordinates);
uint8_t PrintResults(VL53L5CX_ResultsData *ResultsData);
uint8_t PrintXYZCoords(XYZ_ZoneCoordinates_t *XYZ_ZoneCoordinates);
uint8_t ComputeSinCosTables(void);
 
 
#endif /* __VL53L5_Plane_Algo_H */

I took this from some older code. Not sure if you should use the entire include or not.

But it does answer the question you asked.