[X-NUCLEO-53L4A3 TOF Evaluation Board] Gesture Recognition

lugl4313820 · 2024-11-16 10:08

[X-NUCLEO-53L4A3 TOF Evaluation Board] Gesture Recognition [Copy link]

[X-NUCLEO-53L4A3 TOF Evaluation Board] TouchGFX Distance Scale - Sensor - Electronic Engineering World - Forum

[X-NUCLEO-53L4A3 TOF Evaluation Board] Unboxing experience and conventional distance measurement - Sensors - Electronic Engineering World - Forum

【Foreword】

After obtaining the distance to the object through sensing, multiple coordinate points are obtained and combined with time to form a set of coordinate combinations on a two-dimensional plane. The least squares fitting method is used to determine whether these data points are approximately on a straight line, and then the slope is calculated to determine the direction of the object's movement. This time, hand recognition is realized through this principle.

【Data Collection】

Data collection and storage First, I defined two arrays to store the corresponding distance and system running time

double distanceData[DATA_POINTS]; //用于存放距离
double timeData[DATA_POINTS];     //用于存系统运行时间

Then use a loop to collect the values we pre-designed:

				status = VL53L4A3_RANGING_SENSOR_GetDistance(i, &Result);
				if (status == BSP_ERROR_NONE){
					distanceData[j] = Result.ZoneResult[0].Distance[0]; // 采集距离
					timeData[j] = HAL_GetTick();  // 采集时间
					HAL_Delay(20);
				}

// Calculate the parameters of the least squares fitting line and determine whether it is an approximate straight line

// 计算最小二乘法拟合直线的参数并判断是否近似直线
int isApproximatelyLinear(double t[], double d[], int n) {
    double sum_t = 0, sum_d = 0, sum_tt = 0, sum_td = 0;
    for (int i = 0; i < n; i++) {
        sum_t += t[i];
        sum_d += d[i];
        sum_tt += t[i] * t[i];
        sum_td += t[i] * d[i];
    }
    double denominator = n * sum_tt - sum_t * sum_t;
    if (denominator == 0) {
        return 0;
    }
    double a = (n * sum_td - sum_t * sum_d) / denominator;
    double b = (sum_d - a * sum_t) / n;
    double mse = 0;  // 均方误差
    for (int i = 0; i < n; i++) {
        double diff = d[i] - (a * t[i] + b);
        mse += diff * diff;
    }
    mse /= n;
    // 设定一个均方误差阈值，根据实际情况调整
    const double threshold = 3000;
		printf("方向:%.2f\r\n",mse);
    return mse;
}

By calculating the value that conforms to the straight line, I determined its domain value through an experiment to combine the judgment:

			ret = isApproximatelyLinear(timeData, distanceData, DATA_POINTS);
			if (ret <100) 
			{
				printf("物体静止或近似静止。\n");
			}
			else if( ret >=100 && ret <5000) //如果直线的概率在可接受范围内，则得出是向前还是向后的运动方向
			{
					double sum_t = 0, sum_d = 0, sum_tt = 0, sum_td = 0;
					for (int i = 0; i < DATA_POINTS; i++) {
							sum_t += timeData[i];
							sum_d += distanceData[i];
							sum_tt += timeData[i] * timeData[i];
							sum_td += timeData[i] * distanceData[i];
					}
					double denominator = DATA_POINTS * sum_tt - sum_t * sum_t;
					double a = (DATA_POINTS * sum_td - sum_t * sum_d) / denominator;
					if (a > 0) {
							printf("物体向前运动。\n");
					} else if (a < 0) {
							printf("物体向后运动。\n");
					} else {
							printf("物体静止或近似静止。\n");
					}
				} else {
						printf("数据点不近似在一条直线上，无法简单判断运动方向。\n");
				}

[Experimental results]

After downloading to the development board, we can accurately identify three states through the serial port, namely stop, forward, and backward.

【Summarize】

Since 53L4A3 is only for distance measurement, there is no API for regional distance measurement yet, so we can only judge these three states. Of course, we can also calculate the speed by calculating the slope, and further judge multiple postures. Of course, we can also obtain data and calculate the number of data that can be continuously added to the past BUFF to judge more algorithm results.

Attached is the complete algorithm file:

/**
  ******************************************************************************
  * @File : app_tof.c
  * @author : IMG SW Application Team
  * @brief : This file provides code for the configuration
  *                  of the STMicroelectronics.X-CUBE-TOF1.3.4.2 instances.
  ******************************************************************************
  *
  * @attention *
  * Copyright (c) 2023 STMicroelectronics.
  * 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 <stdio.h>

#include "53l4a3_ranging_sensor.h"
#include "app_tof_pin_conf.h"
#include "stm32f4xx_nucleo.h"

/* Private typedef -----------------------------------------------------------*/

/* Private define ------------------------------------------------------------*/
#define TIMING_BUDGET (30U) /* 10 ms < TimingBudget < 200 ms */
#define POLLING_PERIOD (250U) /* refresh rate for polling mode (ms, shall be consistent with TimingBudget value) */



#include <math.h>
#define DATA_POINTS 50


double distanceData[DATA_POINTS];
double timeData[DATA_POINTS];
int isApproximatelyLinear(double t[], double d[], int n) ;

/* Private variables ---------------------------------------------------------*/
static int32_t status = 0;
static uint8_t ToF_Present[RANGING_SENSOR_INSTANCES_NBR] = {0};
volatile uint8_t ToF_EventDetected = 0;

static const char *TofDevStr[] =
{

  [VL53L4A3_DEV_CENTER] = "CENTER", 
};

/* Private function prototypes -----------------------------------------------*/
static void MX_53L4A3_MultiSensorRanging_Init(void);
static void MX_53L4A3_MultiSensorRanging_Process(void);
static void print_result(RANGING_SENSOR_Result_t *Result);
static void write_lowpower_pin(uint8_t device, GPIO_PinState pin_state);
static void reset_all_sensors(void);

static void reset_all_sensors(void);

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 */

  MX_53L4A3_MultiSensorRanging_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 */

  MX_53L4A3_MultiSensorRanging_Process();

  /* USER CODE BEGIN TOF_Process_PostTreatment */

  /* USER CODE END TOF_Process_PostTreatment */
}

static void MX_53L4A3_MultiSensorRanging_Init(void)
{
  uint8_t device;
  uint16_t i2c_addr;
  uint32_t id;

  /* Initialize Virtual COM Port */
  BSP_COM_Init(COM1);

  printf("53L4A3 Multi Sensor Ranging demo application\n");

  reset_all_sensors();

  /* Turn off all the sensors */
  for (device = 0; device < RANGING_SENSOR_INSTANCES_NBR; device++)
  {
    write_lowpower_pin(device, GPIO_PIN_RESET);
  }

  /* initializes each device and put it in low power mode */
  for (device = 0; device < RANGING_SENSOR_INSTANCES_NBR; device++)
  {
    /* enable only one sensor */
    write_lowpower_pin(device, GPIO_PIN_SET);
    HAL_Delay(2);

	printf("Initialize sensor %s\n", TofDevStr[device]);
    status = VL53L4A3_RANGING_SENSOR_Init(device);

    if (status != BSP_ERROR_NONE)
    {
      printf("VL53L4A3_RANGING_SENSOR_Init %d failed\n", device);
      ToF_Present[device] = 0; /* device not detected */
    }
    else
    {
      ToF_Present[device] = 1; /* device detected */
    }

    write_lowpower_pin(device, GPIO_PIN_RESET); /* turn off the device */
  }

  /* power on the devices one at a time, initialize them and change their address.
   * once the address is updated, the communication with the devices is checked
   * reading its ID.
   */
  for (device = 0; device < RANGING_SENSOR_INSTANCES_NBR; device++)
  {
    /* skip the sensor if init not successful */
    if (ToF_Present[device] == 0) { continue; }

    /* turn on the device */
    write_lowpower_pin(device, GPIO_PIN_SET);
    HAL_Delay(2);

    /* left: 0x54, center: 0x56, right: 0x58 */
    i2c_addr = (RANGING_SENSOR_VL53L4ED_ADDRESS + (device + 1) * 2);
	printf("Set sensor %s I2C address to 0X%x\n", TofDevStr[device], i2c_addr);
    VL53L4A3_RANGING_SENSOR_SetAddress(device, i2c_addr);

    /* check the communication with the device reading the ID */
    VL53L4A3_RANGING_SENSOR_ReadID(device, &id);
    printf("ToF sensor %d - ID: %04lX\n", device, (unsigned long)id);
  }
}

static void MX_53L4A3_MultiSensorRanging_Process(void)
{
  uint8_t i,j;
	int ret;
  RANGING_SENSOR_Result_t Result;
  RANGING_SENSOR_ProfileConfig_t Profile;

  Profile.RangingProfile = VL53L4ED_PROFILE_CONTINUOUS;
  Profile.TimingBudget = TIMING_BUDGET;
  Profile.Frequency = 0; /* Induces intermeasurement period, NOT USED for normal ranging */
  Profile.EnableAmbient = 0; /* Enable: 1, Disable: 0 */
  Profile.EnableSignal = 0; /* Enable: 1, Disable: 0 */

  for (i = 0; i < RANGING_SENSOR_INSTANCES_NBR; i++)
  {
    /* skip this device if not detected */
    if (ToF_Present != 1) { continue; }

    VL53L4A3_RANGING_SENSOR_ConfigProfile(i, &Profile);
    status = VL53L4A3_RANGING_SENSOR_Start(i, RS_MODE_BLOCKING_CONTINUOUS);

    if (status != BSP_ERROR_NONE)
    {
      printf("VL53L4A3_RANGING_SENSOR_Start failed\n");
      while (1);
    }
  }

  while (1)
  {
    /* polling mode */
    for (i = 0; i < RANGING_SENSOR_INSTANCES_NBR; i++)
    {
      if (!ToF_Present) { continue; }
			for(j = 0;j<DATA_POINTS; j++)
			{
				status = VL53L4A3_RANGING_SENSOR_GetDistance(i, &Result);
				if (status == BSP_ERROR_NONE){
					distanceData[j] = Result.ZoneResult[0].Distance[0]; // 这里只是示例赋值，实际需要从传感器获取
					timeData[j] = HAL_GetTick();  // 同样是示例赋值
					HAL_Delay(20);
				}
			}
			ret = isApproximatelyLinear(timeData, distanceData, DATA_POINTS);
			if (ret <100) 
			{
				printf("物体静止或近似静止。\n");
			}
			else if( ret >=100 && ret <5000)
			{
					double sum_t = 0, sum_d = 0, sum_tt = 0, sum_td = 0;
					for (int i = 0; i < DATA_POINTS; i++) {
							sum_t += timeData;
							sum_d += distanceData;
							sum_tt += timeData * timeData;
							sum_td += timeData * distanceData;
					}
					double denominator = DATA_POINTS * sum_tt - sum_t * sum_t;
					double a = (DATA_POINTS * sum_td - sum_t * sum_d) / denominator;
					if (a > 0) {
							printf("物体向前运动。\n");
					} else if (a < 0) {
							printf("物体向后运动。\n");
					} else {
							printf("物体静止或近似静止。\n");
					}
				} else {
						printf("数据点不近似在一条直线上，无法简单判断运动方向。\n");
				}
    }
    printf("\n");
  }
}

static void print_result(RANGING_SENSOR_Result_t *Result)
{
  uint8_t i;

  for (i = 0; i < RANGING_SENSOR_MAX_NB_ZONES; i++)
  {
    printf("Status = %2ld, Distance = %5ld mm\r\n",
           (long)Result->ZoneResult.Status[0],
           (long)Result->ZoneResult.Distance[0]);
  }
  printf("\n");
}

static void write_lowpower_pin(uint8_t device, GPIO_PinState pin_state)
{
  switch (device)
  {
    case VL53L4A3_DEV_CENTER:
      HAL_GPIO_WritePin(VL53L4A3_XSHUT_C_PORT, VL53L4A3_XSHUT_C_PIN, pin_state);
      break;

    case VL53L4A3_DEV_LEFT:
      HAL_GPIO_WritePin(VL53L4A3_XSHUT_L_PORT, VL53L4A3_XSHUT_L_PIN, pin_state);
      break;

    case VL53L4A3_DEV_RIGHT:
      HAL_GPIO_WritePin(VL53L4A3_XSHUT_R_PORT, VL53L4A3_XSHUT_R_PIN, pin_state);
      break;

    default:
      break;
  }
}

static void reset_all_sensors(void)
{
  HAL_GPIO_WritePin(VL53L4A3_XSHUT_C_PORT, VL53L4A3_XSHUT_C_PIN, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(VL53L4A3_XSHUT_L_PORT, VL53L4A3_XSHUT_L_PIN, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(VL53L4A3_XSHUT_R_PORT, VL53L4A3_XSHUT_R_PIN, GPIO_PIN_RESET);
  HAL_Delay(2);

  HAL_GPIO_WritePin(VL53L4A3_XSHUT_C_PORT, VL53L4A3_XSHUT_C_PIN, GPIO_PIN_SET);
  HAL_GPIO_WritePin(VL53L4A3_XSHUT_L_PORT, VL53L4A3_XSHUT_L_PIN, GPIO_PIN_SET);
  HAL_GPIO_WritePin(VL53L4A3_XSHUT_R_PORT, VL53L4A3_XSHUT_R_PIN, GPIO_PIN_SET);
  HAL_Delay(2);

}


// 计算最小二乘法拟合直线的参数并判断是否近似直线
int isApproximatelyLinear(double t[], double d[], int n) {
    double sum_t = 0, sum_d = 0, sum_tt = 0, sum_td = 0;
    for (int i = 0; i < n; i++) {
        sum_t += t[i];
        sum_d += d[i];
        sum_tt += t[i] * t[i];
        sum_td += t[i] * d[i];
    }
    double denominator = n * sum_tt - sum_t * sum_t;
    if (denominator == 0) {
        return 0;
    }
    double a = (n * sum_td - sum_t * sum_d) / denominator;
    double b = (sum_d - a * sum_t) / n;
    double mse = 0;  // 均方误差
    for (int i = 0; i < n; i++) {
        double diff = d[i] - (a * t[i] + b);
        mse += diff * diff;
    }
    mse /= n;
    // 设定一个均方误差阈值，根据实际情况调整
    const double threshold = 3000;
		printf("方向:%.2f\r\n",mse);
    return mse;
}


#ifdef __cplusplus
}
#endif

Jacktang · Published on 2024-11-16 09:24

Only these three states can be judged, which is also a method. Thank you for sharing

不爱胡萝卜的仓鼠 · Published on 2024-11-16 10:08

Awesome, it's great that a single point can achieve such an effect. It would be even better if there is a demonstration video. I will have a chance to play with VL53L5 in the future. The 8X8 dot matrix should be able to recognize more gestures.

[X-NUCLEO-53L4A3 TOF Evaluation Board] Gesture Recognition [Copy link]

Latest reply