/* 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 #include "X-WL55_WLE5_53L0X.h" #include "vl53l0x_api.h" #include #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****/