sundp
/
STS_P2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
STS_P2/PeopleCount(VL53L1A2)/Core/Src/main.c

559 lines
16 KiB
C
Raw Blame History

/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
* opensource.org/licenses/BSD-3-Clause
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "rtc.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include "lora.h"
#include "user_tim.h"
#include "usart_user.h"
#include "user_tim.h"
#include "user_data_send.h"
#include "user_data_process.h"
#include "VL53L1X_API.h"
#include "VL53l1X_calibration.h"
#include "X-NUCLEO-53L1A1.h"
#include "VL53l1X_Config.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
uint16_t dev=0x52;
volatile uint8_t TIME_TO_SEND = DISABLE;
volatile uint8_t JUDGE_LORA_JOINED = DISABLE;
extern volatile uint8_t LORA_STATE;
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
void clean_peoplecount(PeopleCount *peoplecount);
int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus, PeopleCount *peopleinout);
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_I2C1_Init();
MX_USART2_UART_Init();
MX_TIM11_Init();
MX_USART1_UART_Init();
MX_RTC_Init();
MX_TIM10_Init();
/* USER CODE BEGIN 2 */
// uint32_t time;
// while(1)
// {
// LL_mDelay(1000);
// time = LL_RTC_TIME_Get(RTC);
// printf("time:%02X:%02X:%02X\r\n",(time>>16)&0xff,(time>>8)&0xff,time&0xff);
// }
// LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5);
LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_5);
LL_mDelay(100);
LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5);
LL_mDelay(100);
LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_5);
uint8_t data_to_send[4] = {0};
PeopleCount peopleinout;
clean_peoplecount(&peopleinout);
uint32_t people_idle_count = 0;
InitLoraModule(&lora);
UsartEnableIT(LORA_USART);
// UsartEnableIT(DEBUG_USART);
const uint8_t cmd_lora_status[13] = "AT+STATUS=?\r\n";
int status = 0;
uint8_t byteData, sensorState=0;
uint16_t wordData;
uint16_t Distance, Signal;
uint8_t RangeStatus;
uint8_t dataReady;
int PplCounter;
int center[2] = {FRONT_ZONE_CENTER, BACK_ZONE_CENTER}; /* these are the spad center of the 2 4*16 zones */
int Zone = 0;
status = XNUCLEO53L1A1_Init();
printf("XNUCLEO53L1A1_Init Status : %X\n", status);
status = VL53L1_RdByte(dev, 0x010F, &byteData);
printf("VL53L1X Model_ID: %X\n", byteData);
status = VL53L1_RdByte(dev, 0x0110, &byteData);
printf("VL53L1X Module_Type: %X\n", byteData);
status = VL53L1_RdWord(dev, 0x010F, &wordData);
printf("VL53L1X: %X\n", wordData);
while (sensorState == 0) {
status = VL53L1X_BootState(dev, &sensorState);
HAL_Delay(2);
}
printf("Chip booted\n");
/* Initialize and configure the device according to people counting need */
status = VL53L1X_SensorInit(dev);
status += VL53L1X_SetDistanceMode(dev, DISTANCE_MODE); /* 1=short, 2=long */
status += VL53L1X_SetTimingBudgetInMs(dev, TIMING_BUDGET); /* in ms possible values [15, 20, 50, 100, 200, 500] */
status += VL53L1X_SetInterMeasurementInMs(dev, TIMING_BUDGET);
status += VL53L1X_SetROI(dev, ROWS_OF_SPADS, 16); /* minimum ROI 4,4 */
if (status != 0) {
printf("Initialization or configuration of the device\n");
return (-1);
}
printf("Start counting people with profile : %s...\n", PROFILE_STRING);
status = VL53L1X_StartRanging(dev); /* This function has to be called to enable the ranging */
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
if(LORA_STATE != LORA_JOINED)
{
StartTIM(TIM10);
if(JUDGE_LORA_JOINED == ENABLE)
{
StopTIM(TIM10);
JUDGE_LORA_JOINED = DISABLE;
UsartxSendDataStr(LORA_USART,cmd_lora_status,sizeof(cmd_lora_status));
}
}
if(LORA_RECV_STATE == ENABLE)
{
LORA_RECV_STATE = DISABLE;
if(lora.recvRingBuff.Lenght != 0)
{
ReadLoraData(&lora.recvRingBuff);
data_process((char *)lora.loraData);
memset(lora.loraData, 0, sizeof(lora.loraData));
}
UsartEnableIT(LORA_USART);
}
if( TIME_TO_SEND != SEND_DISABLE )
{
if( TIME_TO_SEND == SEND_DATA)
{
if(( peopleinout.in != 0) || ( peopleinout.out != 0))
{
data_to_send[0] = (peopleinout.in>>8)&0xff;
data_to_send[1] = peopleinout.in&0xff;
data_to_send[2] = (peopleinout.out>>8)&0xff;
data_to_send[3] = peopleinout.out&0xff;
SendDate_Lora(SEND_DATA, data_to_send);
clean_peoplecount(&peopleinout);
memset(data_to_send, 0, sizeof(data_to_send));
people_idle_count = 0;
}
else
{
people_idle_count++;
if(people_idle_count >= 2)
{
TIME_TO_SEND = SEND_HEART_DATA;
people_idle_count = 0;
}
}
}
if( TIME_TO_SEND == SEND_HEART_DATA)
{
SendDate_Lora(SEND_HEART_DATA, data_to_send);
}
TIME_TO_SEND = SEND_DISABLE;
}
status = VL53L1X_CheckForDataReady(dev, &dataReady);
if(dataReady != 0)
{
dataReady = 0;
status += VL53L1X_GetRangeStatus(dev, &RangeStatus);
status += VL53L1X_GetDistance(dev, &Distance);
status += VL53L1X_GetSignalPerSpad(dev, &Signal);
status += VL53L1X_ClearInterrupt(dev);
if (status != 0) {
printf("Error in operating the device\n");
return (-1);
}
//HAL_Delay(WAIT_BEFORE_PROGRAMMING_OTHER_ZONE_CENTER); // 10, 8, 7, 6 tested OK
status = VL53L1X_SetROICenter(dev, center[Zone]);
if (status != 0) {
printf("Error in chaning the center of the ROI\n");
return (-1);
}
if ((RangeStatus == 0) || (RangeStatus == 4) || (RangeStatus == 7)) {
if (Distance <= MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
Distance = MAX_DISTANCE + MIN_DISTANCE;
}
else // severe error cases
{
Distance = MAX_DISTANCE;
}
// inject the new ranged distance in the people counting algorithm
PplCounter = ProcessPeopleCountingData(Distance, Zone, RangeStatus,&peopleinout);
// if(Zone == 1)
// printf("%d,%d,%d\n", Zone, Distance, Signal);
Zone++;
Zone = Zone%2;
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
LL_FLASH_SetLatency(LL_FLASH_LATENCY_2);
while(LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_2)
{
}
LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE2);
LL_RCC_HSE_Enable();
/* Wait till HSE is ready */
while(LL_RCC_HSE_IsReady() != 1)
{
}
LL_PWR_EnableBkUpAccess();
LL_RCC_LSE_Enable();
/* Wait till LSE is ready */
while(LL_RCC_LSE_IsReady() != 1)
{
}
if(LL_RCC_GetRTCClockSource() != LL_RCC_RTC_CLKSOURCE_LSE)
{
LL_RCC_ForceBackupDomainReset();
LL_RCC_ReleaseBackupDomainReset();
LL_RCC_SetRTCClockSource(LL_RCC_RTC_CLKSOURCE_LSE);
}
LL_RCC_EnableRTC();
LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSE, LL_RCC_PLLM_DIV_8, 84, LL_RCC_PLLP_DIV_2);
LL_RCC_PLL_Enable();
/* Wait till PLL is ready */
while(LL_RCC_PLL_IsReady() != 1)
{
}
LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1);
LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_2);
LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1);
LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL);
/* Wait till System clock is ready */
while(LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL)
{
}
LL_SetSystemCoreClock(84000000);
/* Update the time base */
if (HAL_InitTick (TICK_INT_PRIORITY) != HAL_OK)
{
Error_Handler();
}
LL_RCC_SetTIMPrescaler(LL_RCC_TIM_PRESCALER_TWICE);
}
/* USER CODE BEGIN 4 */
int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus, PeopleCount *peopleinout) {
static int PathTrack[] = {0,0,0,0};
static int PathTrackFillingSize = 1; // init this to 1 as we start from state where nobody is any of the zones
static int LeftPreviousStatus = NOBODY;
static int RightPreviousStatus = NOBODY;
static int PeopleCount = 0;
static uint16_t Distances[2][DISTANCES_ARRAY_SIZE];
static uint8_t DistancesTableSize[2] = {0,0};
uint16_t MinDistance;
uint8_t i;
#ifdef TRACE_PPC
#define TIMES_WITH_NO_EVENT 10// was 40
static uint32_t trace_count = TIMES_WITH_NO_EVENT; // replace by 0 if you want to trace the first TIMES_WITH_NO_EVENT values
#endif
int CurrentZoneStatus = NOBODY;
int AllZonesCurrentStatus = 0;
int AnEventHasOccured = 0;
// Add just picked distance to the table of the corresponding zone
if (DistancesTableSize[zone] < DISTANCES_ARRAY_SIZE) {
Distances[zone][DistancesTableSize[zone]] = Distance;
DistancesTableSize[zone] ++;
}
else {
for (i=1; i<DISTANCES_ARRAY_SIZE; i++)
Distances[zone][i-1] = Distances[zone][i];
Distances[zone][DISTANCES_ARRAY_SIZE-1] = Distance;
}
// pick up the min distance
MinDistance = Distances[zone][0];
if (DistancesTableSize[zone] >= 2) {
for (i=1; i<DistancesTableSize[zone]; i++) {
if (Distances[zone][i] < MinDistance)
MinDistance = Distances[zone][i];
}
}
if (MinDistance < DIST_THRESHOLD) {
// Someone is in !
CurrentZoneStatus = SOMEONE;
}
// left zone
if (zone == LEFT) {
if (CurrentZoneStatus != LeftPreviousStatus) {
// event in left zone has occured
AnEventHasOccured = 1;
if (CurrentZoneStatus == SOMEONE) {
AllZonesCurrentStatus += 1;
}
// need to check right zone as well ...
if (RightPreviousStatus == SOMEONE) {
// event in left zone has occured
AllZonesCurrentStatus += 2;
}
// remember for next time
LeftPreviousStatus = CurrentZoneStatus;
}
}
// right zone
else {
if (CurrentZoneStatus != RightPreviousStatus) {
// event in left zone has occured
AnEventHasOccured = 1;
if (CurrentZoneStatus == SOMEONE) {
AllZonesCurrentStatus += 2;
}
// need to left right zone as well ...
if (LeftPreviousStatus == SOMEONE) {
// event in left zone has occured
AllZonesCurrentStatus += 1;
}
// remember for next time
RightPreviousStatus = CurrentZoneStatus;
}
}
#ifdef TRACE_PPC
// print debug data only when someone is within the field of view
trace_count++;
if ((CurrentZoneStatus == SOMEONE) || (LeftPreviousStatus == SOMEONE) || (RightPreviousStatus == SOMEONE))
trace_count = 0;
if (trace_count < TIMES_WITH_NO_EVENT)
// printf ("%d,%d,%d,%d,%d\n", zone, Distance, MinDistance, RangeStatus, PeopleCount);
#endif
// if an event has occured
if (AnEventHasOccured) {
if (PathTrackFillingSize < 4) {
PathTrackFillingSize ++;
}
// if nobody anywhere lets check if an exit or entry has happened
if ((LeftPreviousStatus == NOBODY) && (RightPreviousStatus == NOBODY)) {
// check exit or entry only if PathTrackFillingSize is 4 (for example 0 1 3 2) and last event is 0 (nobobdy anywhere)
if (PathTrackFillingSize == 4) {
// check exit or entry. no need to check PathTrack[0] == 0 , it is always the case
if ((PathTrack[1] == 1) && (PathTrack[2] == 3) && (PathTrack[3] == 2)) {
// This an entry
PeopleCount ++;
peopleinout->in++;
// reset the table filling size in case an entry or exit just found
DistancesTableSize[0] = 0;
DistancesTableSize[1] = 0;
//printf("Walk In, People Count=%d\n", PeopleCount);
printf("People in %d\r\n", peopleinout->in);
} else if ((PathTrack[1] == 2) && (PathTrack[2] == 3) && (PathTrack[3] == 1)) {
// This an exit
PeopleCount --;
peopleinout->out++;
// reset the table filling size in case an entry or exit just found
DistancesTableSize[0] = 0;
DistancesTableSize[1] = 0;
//printf("Walk Out, People Count=%d\n", PeopleCount);
printf("People out %d\r\n", peopleinout->out);
} else {
// reset the table filling size also in case of unexpected path
DistancesTableSize[0] = 0;
DistancesTableSize[1] = 0;
printf("Wrong path\n");
}
}
PathTrackFillingSize = 1;
}
else {
// update PathTrack
// example of PathTrack update
// 0
// 0 1
// 0 1 3
// 0 1 3 1
// 0 1 3 3
// 0 1 3 2 ==> if next is 0 : check if exit
PathTrack[PathTrackFillingSize-1] = AllZonesCurrentStatus;
}
#ifdef TRACE_PPC
if (AnEventHasOccured) {
for (int j=0; j<PathTrackFillingSize; j++)
printf ("%d ", PathTrack[j]);
}
printf("\n");
#endif
}
// output debug data to main host machine
return(PeopleCount);
}
void clean_peoplecount(PeopleCount *peoplecount)
{
peoplecount->in = 0;
peoplecount->out = 0;
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Reference in New Issue View Git Blame Copy Permalink