diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf index 35a1ed0..3c0989b 100644 Binary files a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf and b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf differ diff --git a/STS/Core/Src/yunhorn_sts_process.c b/STS/Core/Src/yunhorn_sts_process.c index 31a25e6..78cfb0c 100644 --- a/STS/Core/Src/yunhorn_sts_process.c +++ b/STS/Core/Src/yunhorn_sts_process.c @@ -1041,6 +1041,7 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s //OnSystemReset(); break; case 'S': /* "YZS": Self Function Testing */ + STS_SENSOR_Function_Test_Process(); break; @@ -1067,14 +1068,16 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s outbuf[i++] = (uint8_t)sts_version; outbuf[i++] = (uint8_t)sts_hardware_ver; outbuf[i++] = (uint8_t)(99*((GetBatteryLevel()/254)&0xff)); - outbuf[i++] = (uint8_t)(4); //four bytes for distance + outbuf[i++] = (uint8_t)(2); //two bytes for distance #if defined(VL53LX)||defined(VL53L0) +#if 0 outbuf[i++] = (uint8_t)(sts_sensor_install_height/1000+0x30)&0xff; outbuf[i++] = (uint8_t)((sts_sensor_install_height/100)%10+0x30)&0xff; outbuf[i++] = (uint8_t)((sts_sensor_install_height/10)%10+0x30)&0xff; outbuf[i++] = (uint8_t)((sts_sensor_install_height/1)%10+0x30)&0xff; - //outbuf[i++] = (uint8_t)(sts_sensor_install_height>>8)&0xFF; - //outbuf[i++] = (uint8_t)(sts_sensor_install_height)&0xFF; +#endif + outbuf[i++] = (uint8_t)(sts_sensor_install_height>>8)&0xFF; + outbuf[i++] = (uint8_t)(sts_sensor_install_height)&0xFF; #endif STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf); @@ -1732,8 +1735,12 @@ void STS_SENSOR_Distance_Test_Process(void) #if defined(VL53LX)||defined(L8) //MX_TOF_Init(); //MX_TOF_Process(); - sts_sensor_install_height = (uint16_t)MX_TOF_Ranging_Process(); + int status=0; + uint16_t range_distance =0; + status = sts_tof_vl53lx_range_distance(&range_distance); + sts_sensor_install_height = range_distance; APP_LOG(TS_OFF, VLEVEL_M, "\n STS SENSOR INSTALLATION HEIGHT =%d mm\n\r", (uint16_t)sts_sensor_install_height); + #endif #if defined(VL53L0) STS_TOF_VL53L0X_Range_Process(); @@ -1762,14 +1769,20 @@ void STS_SENSOR_Function_Test_Process(void) tstbuf[i++] = (uint8_t) (99*GetBatteryLevel()/254)&0xff; #if (defined(STS_P2)||defined(STS_T6)) - STS_SENSOR_MEMS_Get_ID(&sensor_id); + //STS_SENSOR_MEMS_Get_ID(&sensor_id); + + APP_LOG(TS_OFF, VLEVEL_M, "\r\nSensor_id=%04x\r\n", sensor_id); if (((sensor_id & 0xff)!= 0xCC) && (((sensor_id >>8) & 0xFF)!=0xEA)) // no VL53L1X found { tstbuf[i++] = (uint8_t) 'X'; // Slave MEMS Not Avaliable } else { - STS_SENSOR_Distance_Test_Process(); + //STS_SENSOR_Distance_Test_Process(); + int status=0; + uint16_t range_distance =0; + status = sts_tof_vl53lx_range_distance(&range_distance); + sts_sensor_install_height = range_distance; APP_LOG(TS_OFF, VLEVEL_M, "\nSensor Install Height =%4d mm\n", sts_sensor_install_height); tstbuf[i++] = (uint8_t) (2)&0xff; //length of following data tstbuf[i++] = (uint8_t) (sts_sensor_install_height>>8)&0xff; // MSB of sensor height @@ -1785,12 +1798,16 @@ void STS_SENSOR_Function_Test_Process(void) if (sts_sensor_install_height >= 2000) sts_door_jam_profile ++; //DOOR_JAM_2400 + if (sts_sensor_install_height >= 2400) sts_door_jam_profile ++; //DOOR_JAM_3000 + if (sts_sensor_install_height >= 3000) sts_door_jam_profile ++; //DOOR_JAM_3500 + if (sts_sensor_install_height >= 3500) sts_door_jam_profile ++; //DOOR_JAM_4000 + APP_LOG(TS_OFF, VLEVEL_M, "\r\nDOOR JAM PROFILE=%d \r\n", sts_door_jam_profile); } #endif #ifdef YUNHORN_STS_O6_ENABLED @@ -1817,8 +1834,8 @@ void STS_SENSOR_Function_Test_Process(void) #endif //MX_TOF_Process(); #ifdef STS_P2 - MX_TOF_Init(); - MX_TOF_Ranging_Process(); + //MX_TOF_Init(); + //MX_TOF_Ranging_Process(); #endif #ifdef VL53L0 PME_ON; diff --git a/STS/TOF/App/app_tof.c b/STS/TOF/App/app_tof.c index 226a4d3..601d9ba 100644 --- a/STS/TOF/App/app_tof.c +++ b/STS/TOF/App/app_tof.c @@ -134,7 +134,7 @@ uint16_t MX_TOF_Ranging_Process(void) #if (defined(STS_P2)||defined(STS_T6)) uint16_t range_distance=0; - uint8_t range_mode = 2; //STS_TOF_LONG_RANGE; + uint8_t range_mode = STS_TOF_LONG_RANGE; //STS_TOF_LONG_RANGE; STS_TOF_VL53LX_Range_Process(range_mode, &range_distance); APP_LOG(TS_OFF, VLEVEL_M, "\n VL53L1 Range distance =%u mm \n\r", (uint16_t)range_distance); @@ -483,10 +483,13 @@ void BSP_PB_Callback(Button_TypeDef Button) uint8_t sts_vl53lx_ranging(uint16_t *ranged_distance, uint8_t range_mode, uint16_t distance_threshold_mm, uint16_t inter_measurement_ms, uint16_t macro_timing, uint16_t roi_width, uint16_t sigma_mm, uint16_t signal_kcps) { + uint8_t status=0, dev=0x52; + uint16_t estimated_distance_mm=0; +#if 0 uint8_t status=0, dev=0x52; uint16_t estimated_distance_mm=0; status = VL53L1X_SensorInit(dev); - status += VL53L1X_SetDistanceMode(dev, 2); /* 1=short, 2=long, DISTANCE_MODE */ + status += VL53L1X_SetDistanceMode(dev, range_mode); /* 1=short, 2=long, DISTANCE_MODE */ status += VL53L1X_SetTimingBudgetInMs(dev, 100); /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */ status += VL53L1X_SetInterMeasurementInMs(dev, 50); status += VL53L1X_SetROI(dev, 8, 8); /* minimum ROI 4,4 */ @@ -499,8 +502,9 @@ uint16_t roi_width, uint16_t sigma_mm, uint16_t signal_kcps) APP_LOG(TS_OFF, VLEVEL_L,"Start Range failed\n"); return (-1); } - +#endif status = VL53L1X_GetDistance(dev, &estimated_distance_mm); + status = VL53L1X_ClearInterrupt(dev); /* clear interrupt has to be called to enable next interrupt*/ APP_LOG(TS_OFF,VLEVEL_M,"Target detected! Distance =%d mm \r\n", estimated_distance_mm ); *ranged_distance = estimated_distance_mm; status = VL53L1X_StopRanging(dev); diff --git a/STS/TOF/App/app_tof_peoplecount.c b/STS/TOF/App/app_tof_peoplecount.c index 7f58865..a1b0bfb 100644 --- a/STS/TOF/App/app_tof_peoplecount.c +++ b/STS/TOF/App/app_tof_peoplecount.c @@ -39,7 +39,7 @@ volatile sts_zone_center_by_rows_of_spads_t zone_center[4]={ extern volatile uint8_t sts_door_jam_profile; volatile uint8_t sts_presence_state=0; static int PresenceState=0, PrevPresenceState=0; -volatile uint8_t sts_distance_mode = DOOR_JAM_2400; //sts_door_jam_profile; //DOOR_JAM_3000; +volatile uint8_t sts_distance_mode = DOOR_JAM_2400; //DOOR_JAM_2400; //sts_door_jam_profile; //DOOR_JAM_3000; #endif #if (defined(STS_P2)) volatile uint8_t sts_people_count_number_changed = 0; @@ -117,12 +117,13 @@ int ProcessPresenceDetectionData(int16_t Distance, uint8_t zone, uint8_t RangeSt MinDistance = Distances[zone][i]; } } - + APP_LOG(TS_OFF, VLEVEL_M, "\r\nMin Distance =%d ", MinDistance); if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) { // Someone is in ! CurrentZoneStatus = SOMEONE; + APP_LOG(TS_OFF, VLEVEL_M, "---> Someone is in"); } - + APP_LOG(TS_OFF, VLEVEL_M, "\r\n"); //return CurrentZoneStatus; #if 1 // left zone @@ -275,11 +276,13 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu MinDistance = Distances[zone][i]; } } - + APP_LOG(TS_OFF, VLEVEL_M, "\r\n MinDistance & dist_threshold ---%d %d", MinDistance,ppc_cfg[sts_distance_mode].dist_threshold); if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) { // Someone is in ! CurrentZoneStatus = SOMEONE; + APP_LOG(TS_OFF, VLEVEL_M, ">SOMEONE IN"); } + APP_LOG(TS_OFF, VLEVEL_M, "\r\n"); // left zone if (zone == LEFT) { @@ -352,6 +355,9 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu APP_LOG(TS_OFF, VLEVEL_M,"Walk In, People Count=%d\n", PeopleCount); sts_people_count_sensor_data.Walk_In_People_Count ++; sts_people_count_number_changed = 1; + //LED1_ON; //TODO XXX, CHANGE TO (LED_IN_ON && LED_OUT_OFF); + LED1_TOGGLE; + } else if ((PathTrack[1] == 2) && (PathTrack[2] == 3) && (PathTrack[3] == 1)) { // This an exit PeopleCount --; @@ -360,11 +366,14 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu DistancesTableSize[1] = 0; APP_LOG(TS_OFF, VLEVEL_M,"Walk Out, People Count=%d\n", PeopleCount); sts_people_count_sensor_data.Walk_Out_People_Count ++; + //LED1_ON; //TODO XXX, CHANGE TO (LED_IN_OFF && LED_OUT_ON); + LED1_TOGGLE; sts_people_count_number_changed = 1; } else { // reset the table filling size also in case of unexpected path DistancesTableSize[0] = 0; DistancesTableSize[1] = 0; + APP_LOG(TS_OFF, VLEVEL_M,"Walk Around ie. Wrong path\n"); sts_people_count_sensor_data.Walk_Around_People_Count ++; } @@ -394,6 +403,7 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu #endif #endif } + //printf ("Inside PeopleCount = %d , returned\n", PeopleCount); // output debug data to main host machine return(PeopleCount); @@ -692,6 +702,10 @@ int sts_tof_vl53lx_peoplecount_init(void) /* Initialize and configure the device according to people counting need */ status = VL53L1X_SensorInit(dev); + sts_distance_mode = sts_door_jam_profile; + APP_LOG(TS_OFF, VLEVEL_M, "\r\n Door Jam Profile=%d, distance Mode =%d \r\n", sts_door_jam_profile, sts_distance_mode); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n MaxDistance=%d, \nMinDistance=%d \ndist_threshold=%d \n timing_budget=%d \n distance_mode=%d \r\n", + ppc_cfg[sts_distance_mode].max_distance, ppc_cfg[sts_distance_mode].min_distance, ppc_cfg[sts_distance_mode].dist_threshold,ppc_cfg[sts_distance_mode].timing_budget,ppc_cfg[sts_distance_mode].distance_mode); status += VL53L1X_SetDistanceMode(dev, ppc_cfg[sts_distance_mode].distance_mode); /* 1=short, 2=long, DISTANCE_MODE */ status += VL53L1X_SetTimingBudgetInMs(dev, ppc_cfg[sts_distance_mode].timing_budget); /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */ status += VL53L1X_SetInterMeasurementInMs(dev, ppc_cfg[sts_distance_mode].timing_budget); @@ -756,6 +770,7 @@ int sts_tof_vl53lx_peoplecount_start(void) if ((RangeStatus == 0) || (RangeStatus == 4) || (RangeStatus == 7)) { if (Distance <= ppc_cfg[sts_distance_mode].min_distance) //MIN_DISTANCE) // wraparound case see the explanation at the constants definition place Distance = ppc_cfg[sts_distance_mode].max_distance + ppc_cfg[sts_distance_mode].min_distance; //MAX_DISTANCE + MIN_DISTANCE; + APP_LOG(TS_OFF, VLEVEL_M, "\r\nRangeStatus=%d Distance=%d \r\n", RangeStatus, Distance); } else // severe error cases Distance = ppc_cfg[sts_distance_mode].max_distance; //MAX_DISTANCE; @@ -764,9 +779,36 @@ int sts_tof_vl53lx_peoplecount_start(void) //printf("\nPplCounter =%d \n\r", PplCounter); //sprintf(DisplayStr, "%4d", PplCounter); // only use for special EVK with display //XNUCLEO53L1A1_SetDisplayString(DisplayStr); - //APP_LOG(TS_OFF, VLEVEL_M,"%d,%d,%d\n", Zone, Distance, Signal); + //APP_LOG(TS_OFF, VLEVEL_M,"Zone, Distance, Signal = %d,%d,%d\n", Zone, Distance, Signal); Zone++; Zone = Zone%2; return 0; } +int sts_tof_vl53lx_range_distance(uint16_t *range_distance) +{ + + uint16_t wordData=0, Distance=0; + uint8_t RangeStatus; + uint8_t dataReady=0, status=0, dev=0x52; + + /* read and display data */ + + while (dataReady == 0) { + status = VL53L1X_CheckForDataReady(dev, &dataReady); + HAL_Delay(1); + wordData++; + if (wordData > 30) return -1; // 50 ms timer, so make this 60% to fail back + } + dataReady = 0; + //status += VL53L1X_GetRangeStatus(dev, &RangeStatus); + status += VL53L1X_GetDistance(dev, &Distance); + status += VL53L1X_ClearInterrupt(dev); /* clear interrupt has to be called to enable next interrupt*/ + if (status != 0) { + APP_LOG(TS_OFF, VLEVEL_L,"Error in operating the device\n"); + return (-1); + } + APP_LOG(TS_OFF, VLEVEL_M,"\r\nVL53L1X RANGE DISTANCE =%d \r\n", Distance); + *range_distance = Distance; + return status; +} diff --git a/STS/TOF/App/app_tof_peoplecount.h b/STS/TOF/App/app_tof_peoplecount.h index c5a6ed8..b84f7a6 100644 --- a/STS/TOF/App/app_tof_peoplecount.h +++ b/STS/TOF/App/app_tof_peoplecount.h @@ -172,6 +172,7 @@ int sts_tof_vl53lx_presence_detection_start(void); int sts_tof_vl53lx_peoplecount_init(void); int sts_tof_vl53lx_peoplecount_start(void); +int sts_tof_vl53lx_range_distance(uint16_t *range_distance); //int sts_tof_vl53lx_peoplecount(void);