From ecef96275b0125e22feed8d5dd458701d8e7b534 Mon Sep 17 00:00:00 2001
From: YunHorn Technology <dp.s@yunhorn.com>
Date: Fri, 7 Jul 2023 18:51:19 +0800
Subject: [PATCH] revise i2c address handle

---
 TOF/App/app_tof.c               |  4 ++++
 TOF/App/app_tof_vl53l0x_range.c | 23 ++++++++++++++++-------
 2 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/TOF/App/app_tof.c b/TOF/App/app_tof.c
index c222f44..11d7282 100644
--- a/TOF/App/app_tof.c
+++ b/TOF/App/app_tof.c
@@ -102,7 +102,11 @@ void MX_TOF_Process(void)
 #endif
 
 #ifdef	STS_USE_TOF_VL53L0X
+	STS_SENSOR_Power_ON(1);
+	HAL_Delay(500);
 	STS_TOF_VL53L0X_Range_Process();
+	HAL_Delay(500);
+	STS_SENSOR_Power_OFF(1);
 #endif
 
 #if defined(STS_TOF_VL53L1X_PeopleCount) && defined(STS_USE_TOF_VL53L1X)
diff --git a/TOF/App/app_tof_vl53l0x_range.c b/TOF/App/app_tof_vl53l0x_range.c
index 5187e1a..e7c03a6 100644
--- a/TOF/App/app_tof_vl53l0x_range.c
+++ b/TOF/App/app_tof_vl53l0x_range.c
@@ -24,7 +24,7 @@
 /* USER CODE BEGIN Includes */
 #include <string.h>
 #include "X-WL55_WLE5_53L0X.h"
-
+#include "main.h"
 #include "vl53l0x_api.h"
 #include <limits.h>
 #include "sys_app.h"
@@ -130,16 +130,23 @@ int sts_tof_vl53l0x_DetectSensors(void)
     int FinalAddress;
     nDevPresent = 0;
 
+    HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, GPIO_PIN_RESET);
+    HAL_Delay(30);
+    HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, GPIO_PIN_SET);
+    HAL_Delay(30);
+    //HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, GPIO_PIN_SET);
+    //HAL_Delay(100);
+
     /* detect all sensors (even on-board)*/
     for (i=0; i < MAX_TOF_COUNT; i++)
     {
     VL53L0X_Dev_t *pDev;
     pDev = &VL53L0XDevs[i];
-    pDev->I2cDevAddr = 0x52;
+    pDev->I2cDevAddr = 0x52+(i*2);
     pDev->Present = 0;
         //status = XNUCLEO53L0A1_ResetId(pDev->Id, 1);
         //HAL_Delay(2);
-   FinalAddress=0x52 +(i+1)*2;
+   FinalAddress=0x52 +(i)*2;
 
    APP_LOG(TS_OFF, VLEVEL_L, "\r\n Detect TOF sensors #%u with I2CDevAddr=0x%02x \r\n", i, pDev->I2cDevAddr);
 
@@ -150,7 +157,7 @@ int sts_tof_vl53l0x_DetectSensors(void)
 					
        /* Try to read one register using default 0x52 address */
        status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
-       	   	   	   	   	   	   APP_LOG(TS_OFF, VLEVEL_L, "#%u read id = %04x \r\n",i, Id);
+       APP_LOG(TS_OFF, VLEVEL_L, "#%u read id = %04x I2C ADDR=0x%4X\r\n",i, Id, pDev->I2cDevAddr);
        if (status) {
              APP_LOG(TS_OFF, VLEVEL_L, "# Read id fail\n");
              break;
@@ -173,10 +180,11 @@ int sts_tof_vl53l0x_DetectSensors(void)
         	   APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_DataInit %d fail\n");
         	   break;
            }
-           APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X %d Present and initiated to final 0x%x\n", pDev->Id, pDev->I2cDevAddr);
+
            nDevPresent++;
            nDevMask |= 1 << i;
            pDev->Present = 1;
+           APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X %d Present and initiated to final 0x%4X Mask=0b%8b\r\n", pDev->Id, pDev->I2cDevAddr, nDevMask);
        }
        else {
     	   APP_LOG(TS_OFF, VLEVEL_L, "# unknown ID %x\n", Id);
@@ -325,9 +333,10 @@ void STS_TOF_VL53L0X_Range_Process(void)
   int status=0, i;
   RangingConfig_e RangingConfig = HIGH_ACCURACY; //HIGH_ACCURACY;		//LONG_RANGE;
   XWL55_WLE5_53L0X_Init();
-  if ((nDevMask ==0) && (nSensorPresent ==0)){
+  if ((nDevMask ==0) || (nSensorPresent ==0))
+  {
 	  nSensorPresent = sts_tof_vl53l0x_DetectSensors();					// confirm sensor online
-	  APP_LOG(TS_OFF, VLEVEL_M, "sensor online \r\n");
+	  APP_LOG(TS_OFF, VLEVEL_L, "%u pcs sensor(s) online \r\n", nSensorPresent);
   }
 
   if (nSensorPresent > 0)