sundp
/
STS_RR_R125
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

revised GPIO, XSHUT fully support 3 TOF sensors

This commit is contained in:
Yunhorn 2023-07-13 16:34:21 +08:00
parent 1c393d1043
commit 0f665e497d
2 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
TOF/App/X-WL55_WLE5_53L0X.c
View File

@ -205,15 +205,16 @@ void XWL55_WLE5_53L0X_ResetId(uint8_t DevNo, int state)
break; break;
case XNUCLEO53L0X_CENTER: case XNUCLEO53L0X_CENTER:
HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, ((state == 1)?GPIO_PIN_SET:GPIO_PIN_RESET)); break; HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, ((state == 1)?GPIO_PIN_SET:GPIO_PIN_RESET));
break;
case XNUCLEO53L0X_RIGHT: case XNUCLEO53L0X_RIGHT:
HAL_GPIO_WritePin(TOF_R_XSHUT_GPIO_Port, TOF_R_XSHUT_Pin, ((state == 1)?GPIO_PIN_SET:GPIO_PIN_RESET)); break; HAL_GPIO_WritePin(TOF_R_XSHUT_GPIO_Port, TOF_R_XSHUT_Pin, ((state == 1)?GPIO_PIN_SET:GPIO_PIN_RESET));
default: break;
default:
break; break;
} }
} }
/** /**

32
TOF/App/app_tof_vl53l0x_range.c
View File

@ -111,8 +111,8 @@ void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig);
void HandleError(int err){ void HandleError(int err){
//char msg[16]; //char msg[16];
//sprintf(msg,"Er%d", err); //sprintf(msg,"Er%d", err);
APP_LOG(TS_OFF, VLEVEL_L,"Er%d\r\n",err); APP_LOG(TS_OFF, VLEVEL_L,"Er %04u\r\n",err);
while(1){}; //while(1){};
} }
@ -135,7 +135,7 @@ int sts_tof_vl53l0x_DetectSensors(void)
XWL55_WLE5_53L0X_ResetId(i,0); XWL55_WLE5_53L0X_ResetId(i,0);
} }
//XWL55_WLE5_53L0X_SetIntrStateId(0,0); XWL55_WLE5_53L0X_SetIntrStateId(0,0);
/* detect all sensors (even on-board)*/ /* detect all sensors (even on-board)*/
for (i=0; i < MAX_TOF_COUNT; i++) for (i=0; i < MAX_TOF_COUNT; i++)
@ -145,7 +145,7 @@ int sts_tof_vl53l0x_DetectSensors(void)
pDev->I2cDevAddr = 0x52; pDev->I2cDevAddr = 0x52;
pDev->Present = 0; pDev->Present = 0;
XWL55_WLE5_53L0X_ResetId(pDev->Id, 1); XWL55_WLE5_53L0X_ResetId(pDev->Id, 1);
HAL_Delay(2); HAL_Delay(30);
FinalAddress = 0x52+(i+1)*2; FinalAddress = 0x52+(i+1)*2;
APP_LOG(TS_OFF, VLEVEL_L, "\r\n Detect TOF sensors #%u with I2CDevAddr=0x%02x \r\n", i, pDev->I2cDevAddr); APP_LOG(TS_OFF, VLEVEL_L, "\r\n Detect TOF sensors #%u with I2CDevAddr=0x%02x \r\n", i, pDev->I2cDevAddr);
@ -218,9 +218,12 @@ void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
uint8_t preRangeVcselPeriod = 14; uint8_t preRangeVcselPeriod = 14;
uint8_t finalRangeVcselPeriod = 10; uint8_t finalRangeVcselPeriod = 10;
// uart_printf("\r\n######### start setup for single shot \r\n"); // uart_printf("\r\n######### start setup for single shot \r\n");
for( i=0; i<3; i++){ for( i=0; i< MAX_TOF_COUNT; i++){
if( VL53L0XDevs[i].Present){ if( VL53L0XDevs[i].Present){
APP_LOG(TS_OFF, VLEVEL_L, "\r\n ###### Starting Range #%u sensor \r\n",i);
status=VL53L0X_StaticInit(&VL53L0XDevs[i]); status=VL53L0X_StaticInit(&VL53L0XDevs[i]);
if( status ){ if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_StaticInit failed\n"); APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_StaticInit failed\n");
@ -353,35 +356,26 @@ void STS_TOF_VL53L0X_Range_Process(void)
{ {
if (VL53L0XDevs[i].Present ==1) if (VL53L0XDevs[i].Present ==1)
{ {
APP_LOG(TS_OFF, VLEVEL_L, "\r\nStart Ranging #%u sensor, position mask 0x%02x \r\n", i, nDevMask);
status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData); status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData);
APP_LOG(TS_OFF, VLEVEL_L, "\r\nRanging status =0x%04x \r\n", status);
if( status ==0 ) if( status ==0 )
{ {
sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData); sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData);
/* Display distance in mm */
APP_LOG(TS_OFF, VLEVEL_L, "\r\nSet New Range status =%u \r\n", RangingMeasurementData.RangeStatus);
if( RangingMeasurementData.RangeStatus == 0 ) if( RangingMeasurementData.RangeStatus == 0 )
{ {
sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange; sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange;
nDevMask |= (1 << i); nDevMask |= (1 << i);
sensor_data_ready |= 1; sensor_data_ready |= 1;
//uart_printf("## MeasureData Distance = %4d mm \r\n", (int)Distance_data,(int) VL53L0XDevs.LeakyRange);
} else } else
{ {
HandleError(ERR_DEMO_RANGE_ONE); HandleError(ERR_DEMO_RANGE_ONE);
} }
// ########## two conditions // ########## two conditions
// ########## 1) return status ==0, // ########## 1) return status ==0,
// ########## 2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!! // ########## 2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
} }
} }
HAL_Delay(30);
} // for i < MAX_TOF_COUNT } // for i < MAX_TOF_COUNT
APP_LOG(TS_OFF, VLEVEL_L, "\r\n## Measured Range: \r\nTOF #0 = %4u mm, \r\nTOF #1 = %4u mm, \r\nTOF #2 = %4u mm\r\n", APP_LOG(TS_OFF, VLEVEL_L, "\r\n## Measured Range: \r\nTOF #0 = %4u mm, \r\nTOF #1 = %4u mm, \r\nTOF #2 = %4u mm\r\n",