sundp
/
STS_RR_R125
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity
STS_RR_R125/TOF/App/app_tof_vl53l0x_range.c

368 lines
11 KiB
C
Raw Blame History

/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file app_tof_vl53l0x_range.c
* @author Yunhorn (r) Technology Limited Application Team
* @brief Application of the LRWAN Middleware
******************************************************************************
* @attention
*
* Copyright (c) 2023 Yunhorn Technology Limited.
* Copyright (c) 2023 Shenzhen Yunhorn Technology Co., Ltd.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "stm32xxx_hal.h"
/* USER CODE BEGIN Includes */
#include <string.h>
#include "X-WL55_WLE5_53L0X.h"
#include "vl53l0x_api.h"
#include <limits.h>
#include "sys_app.h"
/**
* @defgroup Configuration Static configuration
* @{
*/
/** @} */ /* config group */
#ifndef MIN
# define MIN(a,b) ((a) < (b) ? (a) : (b))
#endif
/**
* @defgroup ErrCode Errors code shown on display
* @{
*/
#define ERR_DETECT -1
#define ERR_DEMO_RANGE_ONE 1
#define ERR_DEMO_RANGE_MULTI 2
extern I2C_HandleTypeDef hi2c2;
#define X_WL55_WLE5_53L0X_hi2c hi2c2
/** }@} */ /* defgroup ErrCode */
/* USER CODE END Includes */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/
typedef enum {
LONG_RANGE = 0, /*!< Long range mode */
HIGH_SPEED = 1, /*!< High speed mode */
HIGH_ACCURACY = 2, /*!< High accuracy mode */
} RangingConfig_e;
/**
* Global ranging struct
*/
VL53L0X_RangingMeasurementData_t RangingMeasurementData;
/** leaky factor for filtered range
*
* r(n) = averaged_r(n-1)*leaky +r(n)(1-leaky)
*
* */
int LeakyFactorFix8 = (int)( 0.6 *256);
/** How many device detect set by @a DetectSensors()*/
int nDevPresent=0;
volatile int sts_tof_distance_data;
extern volatile uint8_t sensor_data_ready;
VL53L0X_Dev_t VL53L0XDevs={
.I2cHandle=&X_WL55_WLE5_53L0X_hi2c,
.I2cDevAddr=0x52
};
/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/
//void ResetAndDetectSensor(int SetDisplay);
/* USER CODE END PFP */
/* USER CODE BEGIN 0 */
/**
* Handle Error
*
* Set err on display and loop forever
* @param err Error case code
*/
void HandleError(int err){
char msg[16];
sprintf(msg,"Er%d", err);
while(1){};
}
/**
* Reset all sensor then goes true presence detection
*
* All present devices are data initiated and assigned with their final address
* @return
*/
int sts_tof_vl53l0x_DetectSensors(void)
{
uint16_t Id;
int status;
int FinalAddress;
/* detect all sensors (even on-board)*/
VL53L0X_Dev_t *pDev;
pDev = &VL53L0XDevs;
pDev->I2cDevAddr = 0x52;
pDev->Present = 0;
//status = XNUCLEO53L0A1_ResetId(pDev->Id, 1);
//HAL_Delay(2);
FinalAddress=0x52;
do {
/* Set I2C standard mode (400 KHz) before doing the first register access */
//if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_WrByte(pDev, 0x88, 0x00);
/* Try to read one register using default 0x52 address */
status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
if (status) {
APP_LOG(TS_OFF, VLEVEL_L, "# Read id fail\n");
break;
}
if (Id == 0xEEAA) {
/* Sensor is found => Change its I2C address to final one */
status = VL53L0X_SetDeviceAddress(pDev,FinalAddress);
if (status != 0) {
APP_LOG(TS_OFF, VLEVEL_L, "#i VL53L0X_SetDeviceAddress fail\n");
break;
}
pDev->I2cDevAddr = FinalAddress;
/* Check all is OK with the new I2C address and initialize the sensor */
status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
status = VL53L0X_DataInit(pDev);
if( status == 0 ){
pDev->Present = 1;
}
else{
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_DataInit %d fail\n");
break;
}
//APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X Present and initiated to final 0x%x\n", pDev->I2cDevAddr);
pDev->Present = 1;
}
else {
APP_LOG(TS_OFF, VLEVEL_L, "# unknown ID %x\n", Id);
status = 1;
}
/* if fail r can't use for any reason then put the device back to reset */
//if (status) {
//XNUCLEO53L0A1_ResetId(i, 0);
//}
} while(0);
/* Display detected sensor(s) */
return Id;
}
/**
* Setup all detected sensors for single shot mode and setup ranging configuration
*/
void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
{
int status=VL53L0X_ERROR_NONE;
uint8_t VhvSettings;
uint8_t PhaseCal;
uint32_t refSpadCount;
uint8_t isApertureSpads;
FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25*65536);
FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18*65536);
uint32_t timingBudget = 33000;
uint8_t preRangeVcselPeriod = 14;
uint8_t finalRangeVcselPeriod = 10;
// uart_printf("\r\n######### start setup for single shot \r\n");
status=VL53L0X_StaticInit(&VL53L0XDevs);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_StaticInit failed\n");
}
status = VL53L0X_PerformRefCalibration(&VL53L0XDevs, &VhvSettings, &PhaseCal);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefCalibration failed\n");
}
status = VL53L0X_PerformRefSpadManagement(&VL53L0XDevs, &refSpadCount, &isApertureSpads);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefSpadManagement failed\n");
}
status = VL53L0X_SetDeviceMode(&VL53L0XDevs, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetDeviceMode failed\n");
}
status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckEnable failed\n");
}
status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); // Enable Signa limit
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckEnable failed\n");
}
/* Ranging configuration */
switch(rangingConfig) {
case LONG_RANGE:
signalLimit = (FixPoint1616_t)(0.1*65536);
sigmaLimit = (FixPoint1616_t)(60*65536);
timingBudget = 33000;
preRangeVcselPeriod = 18;
finalRangeVcselPeriod = 14;
break;
case HIGH_ACCURACY:
signalLimit = (FixPoint1616_t)(0.25*65536);
sigmaLimit = (FixPoint1616_t)(18*65536);
timingBudget = 200000;
preRangeVcselPeriod = 14;
finalRangeVcselPeriod = 10;
break;
case HIGH_SPEED:
signalLimit = (FixPoint1616_t)(0.25*65536);
sigmaLimit = (FixPoint1616_t)(32*65536);
timingBudget = 20000;
preRangeVcselPeriod = 14;
finalRangeVcselPeriod = 10;
break;
default:
APP_LOG(TS_OFF, VLEVEL_L, "Not Supported");
break;
}
status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckValue failed\n");
}
status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckValue failed\n");
}
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&VL53L0XDevs, timingBudget);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetMeasurementTimingBudgetMicroSeconds failed\n");
}
status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs, VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetVcselPulsePeriod failed\n");
}
status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetVcselPulsePeriod failed\n");
}
status = VL53L0X_PerformRefCalibration(&VL53L0XDevs, &VhvSettings, &PhaseCal);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefCalibration failed\n");
}
VL53L0XDevs.LeakyFirst =1;
}
/* Store new ranging data into the device structure, apply leaky integrator if needed */
void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange){
if( pRange->RangeStatus == 0 ){
if( pDev->LeakyFirst ){
pDev->LeakyFirst = 0;
pDev->LeakyRange = pRange->RangeMilliMeter;
}
else{
pDev->LeakyRange = (pDev->LeakyRange*LeakyFactorFix8 + (256-LeakyFactorFix8)*pRange->RangeMilliMeter)>>8;
}
}
else{
pDev->LeakyFirst = 1;
}
}
VL53L0X_Error sts_tof_vl53l0x_range(VL53L0X_Dev_t *dev,VL53L0X_RangingMeasurementData_t *pdata, RangingConfig_e rangingConfig)
{
//int over=0;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
/* Setup all sensors in Single Shot mode */
sts_tof_vl53l0x_SetupSingleShot(rangingConfig);
/* Call All-In-One blocking API function */
status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs, &RangingMeasurementData);
if( status ==0 ){
sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs,&RangingMeasurementData);
/* Display distance in mm */
if( RangingMeasurementData.RangeStatus == 0 ){
sts_tof_distance_data = (int)VL53L0XDevs.LeakyRange;
//uart_printf("## MeasureData Distance = %4d mm \r\n", (int)Distance_data,(int) VL53L0XDevs.LeakyRange);
}
}
else{
HandleError(ERR_DEMO_RANGE_ONE);
}
return status;
}
/* USER CODE END 0 */
void STS_TOF_VL53L0X_Range_Process(void)
{
int Status=0;
RangingConfig_e RangingConfig = HIGH_ACCURACY; //HIGH_ACCURACY; //LONG_RANGE;
XWL55_WLE5_53L0X_Init();
Status = sts_tof_vl53l0x_DetectSensors(); // confirm sensor online
//if (Status ==1 )
//APP_LOG(TS_OFF, VLEVEL_M, "sensor online \r\n");
// RangingConfig == HIGH ACCURACY, LONG RANGE, HIGH SPEED
Status = sts_tof_vl53l0x_range(&VL53L0XDevs,&RangingMeasurementData, RangingConfig);
// ########## two conditions
// ########## 1) return status ==0,
// ########## 2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
if ((Status == 0) && (RangingMeasurementData.RangeStatus == 0 ))
{
//sts_tof_distance_data = (int) VL53L0XDevs.LeakyRange;
APP_LOG(TS_OFF, VLEVEL_M, "## Measured Range = %4d mm\r\n", (int)sts_tof_distance_data);
sensor_data_ready = 1;
}
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Reference in New Issue View Git Blame Copy Permalink