204 lines
5.3 KiB
C
204 lines
5.3 KiB
C
/**
|
|
******************************************************************************
|
|
* @file : app_tof.c
|
|
* @author Yunhorn (r) Technology Limited Application Team
|
|
* @brief configuration X-CUBE-TOF1.3.2.0 instances.
|
|
******************************************************************************
|
|
*
|
|
* @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.
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include "app_tof.h"
|
|
#include "main.h"
|
|
#include "sys_app.h"
|
|
#include <stdio.h>
|
|
#include "yunhorn_sts_prd_conf.h"
|
|
|
|
#ifdef STS_USE_TOF_VL53L1X
|
|
#include "VL53L1X_ULP_api.h"
|
|
//#include "53l1a2_ranging_sensor.h"
|
|
#endif
|
|
|
|
#ifdef STS_USE_TOF_VL53L0X
|
|
#include "VL53L0X_api.h"
|
|
|
|
#include "X-WL55_WLE5_53L0X.h"
|
|
#endif
|
|
|
|
#ifdef STS_TMG
|
|
#include "mlx90640_lcd_display.h"
|
|
#endif
|
|
|
|
#include "stm32wlxx_nucleo.h"
|
|
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
|
|
/* Private define ------------------------------------------------------------*/
|
|
#define TIMING_BUDGET (30U) /* 16 ms < TimingBudget < 500 ms */
|
|
#define POLLING_PERIOD (250U) /* refresh rate for polling mode (milliseconds, shall be consistent with TimingBudget value) */
|
|
|
|
/* Private variables ---------------------------------------------------------*/
|
|
//static RANGING_SENSOR_Capabilities_t Cap;
|
|
//static RANGING_SENSOR_ProfileConfig_t Profile;
|
|
//static int32_t status = 0;
|
|
|
|
volatile uint8_t ToF_EventDetected = 0;
|
|
extern volatile uint8_t sensor_data_ready;
|
|
#ifdef STS_USE_TOF_VL53L0X
|
|
extern volatile int sts_tof_distance_data[MAX_TOF_COUNT];
|
|
#endif
|
|
//volatile uint8_t sensor_data_ready=0;
|
|
/* Private function prototypes -----------------------------------------------*/
|
|
#ifdef STS_USE_TOF_VL53L1X
|
|
int sts_vl53l1x_ranging(uint8_t vl_model, 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 IsInterruptDetected(uint16_t dev);
|
|
#endif
|
|
|
|
void MX_TOF_Init(void)
|
|
{
|
|
/* USER CODE BEGIN SV */
|
|
|
|
/* USER CODE END SV */
|
|
|
|
/* USER CODE BEGIN TOF_Init_PreTreatment */
|
|
|
|
/* USER CODE END TOF_Init_PreTreatment */
|
|
|
|
/* Initialize the peripherals and the TOF components */
|
|
APP_LOG(TS_OFF,VLEVEL_L,"\r\n###################### MX TOF Init... \r\n");
|
|
#ifdef STS_USE_TOF_VL53L0X
|
|
XWL55_WLE5_53L0X_Init();
|
|
#endif
|
|
//MX_53L1A2_SimpleRanging_Init();
|
|
|
|
/* USER CODE BEGIN TOF_Init_PostTreatment */
|
|
|
|
/* USER CODE END TOF_Init_PostTreatment */
|
|
}
|
|
|
|
/*
|
|
* LM background task
|
|
*/
|
|
void MX_TOF_Process(void)
|
|
{
|
|
/* USER CODE BEGIN TOF_Process_PreTreatment */
|
|
|
|
/* USER CODE END TOF_Process_PreTreatment */
|
|
|
|
|
|
//APP_LOG(TS_OFF,VLEVEL_L,"\r\n###################### MX TOF Process... \r\n");
|
|
|
|
#ifdef STS_USE_TOF_VL53L1X
|
|
STS_TOF_VL53L1X_Range_Process();
|
|
#endif
|
|
|
|
#ifdef STS_USE_TOF_VL53L0X
|
|
STS_SENSOR_Power_ON(1);
|
|
HAL_Delay(100);
|
|
#if defined(TOF_1)||defined(TOF_2)
|
|
STS_TOF_VL53L0X_Range_Process();
|
|
#endif
|
|
#ifdef TOF_3
|
|
STS_TOF250_Range_Process();
|
|
#endif
|
|
|
|
STS_SENSOR_Power_OFF(1);
|
|
#endif
|
|
|
|
#if defined(STS_TOF_VL53L1X_PeopleCount) && defined(STS_USE_TOF_VL53L1X)
|
|
//STS_TOF_VL53LX_PeopleCounting_Process();
|
|
#endif
|
|
|
|
|
|
/* USER CODE BEGIN TOF_Process_PostTreatment */
|
|
|
|
/* USER CODE END TOF_Process_PostTreatment */
|
|
}
|
|
#if defined(YUNHORN_STS_R0_ENABLED)
|
|
void STS_R0_SENSOR_Read(STS_R0_SensorDataTypeDef *r0_data)
|
|
{
|
|
#ifdef STS_USE_TOF_VL53L0X
|
|
r0_data->distance_mm = sts_tof_distance_data[0];
|
|
r0_data->distance1_mm = sts_tof_distance_data[1];
|
|
r0_data->distance2_mm = sts_tof_distance_data[2];
|
|
#if defined(ROCTEC_R5)
|
|
r0_data->distance1_mm = sts_tof_distance_data[0]+sts_tof_distance_data[1]+sts_tof_distance_data[2];
|
|
#endif
|
|
#endif
|
|
sensor_data_ready = 1;
|
|
}
|
|
#endif
|
|
|
|
typedef enum {
|
|
STS_TOF_SHORT_RANGE=0,
|
|
STS_TOF_LONG_RANGE,
|
|
STS_TOF_LOW_POWER_RANGE,
|
|
STS_TOF_PP_RANGE
|
|
} range_mode_t;
|
|
typedef enum {
|
|
STS_TOF_VL53L0X=0,
|
|
STS_TOF_VL53L1X,
|
|
STS_TOF_VL53L2X,
|
|
STS_TOF_VL53L3X,
|
|
STS_TOF_VL53L4X,
|
|
STS_TOF_VL53L5X,
|
|
STS_TOF_VL53L6X,
|
|
STS_TOF_VL53L7X
|
|
} vl53lx_model;
|
|
|
|
uint8_t STS_SENSOR_MEMS_Get_ID(uint8_t *mems_Dev_ID)
|
|
{
|
|
uint16_t Id = 0xEEAA; //0xeeaa for VL53L0X
|
|
uint8_t cnt =0;
|
|
|
|
#ifdef STS_USE_TOF_VL53L0X
|
|
cnt = sts_tof_vl53l0x_DetectSensors();
|
|
#endif
|
|
|
|
if (cnt > 0)
|
|
{
|
|
mems_Dev_ID[0] = (Id >>8)&0xff;
|
|
mems_Dev_ID[1] = (Id & 0xff);
|
|
}
|
|
|
|
APP_LOG(TS_OFF, VLEVEL_H,"MEMS count=%d MEMS ID=%02x %02x \r\n", cnt, mems_Dev_ID[0],mems_Dev_ID[1]);
|
|
return cnt;
|
|
}
|
|
|
|
uint8_t IsInterruptDetected(uint16_t dev)
|
|
{
|
|
// To be filled with customer HW. This function should
|
|
// return 1 when an interrupt is raised by the ToF on GPIO1 pin (pin7)
|
|
#ifdef STS_USE_TOF_VL53L1X
|
|
if (ToF_EventDetected )
|
|
{
|
|
//APP_LOG(TS_OFF, VLEVEL_L,"############### TOF EVENT DETECTED \r\n");
|
|
ToF_EventDetected =0;
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|