STM32 ] 초음파 센서로 거리재기 Timer Input Capture 사용 + 노이즈 (튀는 값) 제거

 

 

 

 

 

참고 사이트 : https://controllerstech.com/hcsr04-ultrasonic-sensor-and-stm32/

 

참고 유튜브 : https://www.youtube.com/watch?v=ti_1ZwRolU4 

 

 

 

배선

HC-SR04
VCC	5V
GND	GND
Trig	PA5 (GPIO Output)
Echo	PA6 (TIM3 CH1 Input Capture)

 

 

Cube MX 설정

 

 

TIM3은 APB1 클락소스(90MHz)를 공급받는다. 그리고 Prescaler 값을 90-1 로 해주면 1us(마이크로초) 마다 CNT가 오른다.

그리고 TIM3은 16비트 카운터라서 ARR은 최대값인 0xffff-1로 해준다. 

 

 

 

 

+ UART3 설정도 켠다

 

 

 

 

 

소스코드

/* USER CODE BEGIN PD */

#define TRIG_PIN GPIO_PIN_5
#define TRIG_PORT GPIOA

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
#ifdef __GNUC__
  /* With GCC, small printf (option LD Linker->Libraries->Small printf
     set to 'Yes') calls __io_putchar() */
  #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
  #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif /* __GNUC__ */
PUTCHAR_PROTOTYPE
{
  /* Place your implementation of fputc here */
  /* e.g. write a character to the EVAL_COM1 and Loop until the end of transmission */
  HAL_UART_Transmit(&huart3, (uint8_t *)&ch, 1, 0xFFFF);

  return ch;
}
/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

uint32_t IC_Val1 = 0;
uint32_t IC_Val2 = 0;
uint32_t Difference = 0;
uint8_t Is_First_Captured = 0;  // is the first value captured ?
uint8_t Distance  = 0;

/* USER CODE END PV */

 

 

 

 

/* USER CODE BEGIN PFP */

void delay (uint16_t time)
{
	__HAL_TIM_SET_COUNTER(&htim3, 0);
	while (__HAL_TIM_GET_COUNTER (&htim3) < time);
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
	if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)  // if the interrupt source is channel1
	{
		if (Is_First_Captured==0) // if the first value is not captured
		{
			IC_Val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1); // read the first value
			Is_First_Captured = 1;  // set the first captured as true
			// Now change the polarity to falling edge
			__HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
		}

		else if (Is_First_Captured==1)   // if the first is already captured
		{
			IC_Val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);  // read second value
			__HAL_TIM_SET_COUNTER(htim, 0);  // reset the counter

			if (IC_Val2 > IC_Val1)
			{
				Difference = IC_Val2-IC_Val1;
			}

			else if (IC_Val1 > IC_Val2)
			{
				Difference = (0xffff - IC_Val1) + IC_Val2;
			}

			Distance = Difference * .034/2;
			Is_First_Captured = 0; // set it back to false

			// set polarity to rising edge
			__HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
			__HAL_TIM_DISABLE_IT(&htim3, TIM_IT_CC1);
		}
	}
}

void HCSR04_Read (void)
{
	HAL_GPIO_WritePin(TRIG_PORT, TRIG_PIN, GPIO_PIN_SET);  // pull the TRIG pin HIGH
	delay(10);  // wait for 10 us
	HAL_GPIO_WritePin(TRIG_PORT, TRIG_PIN, GPIO_PIN_RESET);  // pull the TRIG pin low

	__HAL_TIM_ENABLE_IT(&htim3, TIM_IT_CC1);
}

/* USER CODE END PFP */

 

delay() 함수는 HAL_Delay는 ms 단위로 밖에 딜레이가 안되니까 µs 단위로 딜레이를 주기 위해 만든 함수다. HCSR04_Read 함수 안에서 10us 딜레이를 주기위해 쓰였다.

 

 

HAL_TIM_IC_CaptureCallback() 함수는 driver 폴더의 -tim.c 폴더에 정의된 함수를 가져와 재정의해서 쓴다. 코드설명을 하자면 만약 인터럽트 소스가 채널1 이라면 HAL_TIM_ReadCapturedValue() 로 캡쳐/비교기(CCR : Capture Compare Register)값을 읽고 폴링 엣지에 트리거 되게 설정을 바꾼다. 이 다음 트리거에서 다시 HAL_TIM_ReadCapturedValue() 로 CCR 값을 읽고 그 차이인 Difference 를 구하면 us초 단위로 에코 핀이 High 를 유지한 시간 간격을 측정해서 거리를 파악할 수 있다.  

 

if (IC_Val1 > IC_Val2) 부분은 Overflow를 고려한 코드이다. 예를 들어 IC_Val1이 60000 이고 IC_Val2가 10000 일때 Difference는 65535-60000+10000us 이 된다. 그리고 해당 초음파 센서의 최대 측정거리는 4m정도(안정적인 측정범위는 3cm-2m)라서 측정범위가 넘어가면 Echo핀이 다시 Low로 떨어질 테니까 이 외의 케이스는 고려해줄 필요가 없다.

 

Difference 를 다시 음속인 340m/s를 곱하고 단위환산해서 cm 단위로 바꿔 준 값(10^-6 * 10^2)에 왕복거리를 편도거리로 바꾼 것을 Distance에 대입한다.

 

그리고 다시 라이징 엣지에 트리거 되게 설정을 바꾼다.

 

  /* USER CODE BEGIN 2 */

  HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_1);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	  HCSR04_Read();
	  printf("%d cm\r\n",Distance);
	  HAL_Delay(200);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }

 

main 함수 내에선 HAL_TIM_IC_Start_IT 해주고 HCSR04_Read() 함수를 호출한 뒤 전역변수인 Distance를 UART를 통해 출력한다.

 

트리거핀은 최소 10us 동안 HIGH 상태를 유지해야 측정이 가능하고, 측정주기는 Datasheet를 찾아보면 60ms 이상을 권장하고 있다. 위 코드에선 천천히 출력해서 보려고 200ms 로 해줬다.

 

 

초음파 센서에 대한 설명 : https://eteo.tistory.com/23

[ 아두이노 ] 초음파 거리 측정 센서 HC-SR04

 

 

 

결과물

 

 

 

 

 

 

 

하지만 여기에는 문제가 있다. 아래 움짤처럼 같은 거리가 나와야 하는 상황인데도 불구하고 튀는 값이 나올 때가 종종 있다는 것이다.

 

 

 

 

 

 

 

 

그래서 이동평균필터(Moving Average Filter)를 사용하여 출력의 노이즈 제거해 보았다.

 

수식과 원리는 다음과 같다.

 

출처 : https://jeonhj.tistory.com/23

 

 

 

 

소스코드

#define ARRAYNUM 10
int readings[ARRAYNUM];      // the readings from the analog input
int idx = 0;              // the index of the current reading
int total = 0;                  // the running total
int average = 0;                // the average
/* USER CODE END PV */

...

/* USER CODE BEGIN WHILE */
  while (1)
  {

	  HCSR04_Read();

	  // subtract the last reading:
	  total = total - readings[idx];
	  // read from the sensor:
	  readings[idx] = Distance;
	  // add the reading to the total:
	  total = total + readings[idx];
	  // advance to the next position in the array:
	  idx = idx + 1;

	  // if we're at the end of the array...
	  if (idx >= ARRAYNUM) {
		  // ...wrap around to the beginning:
		  idx = 0;
	  }
	  // calculate the average:
	  average = total / ARRAYNUM;
	  // send it to the computer as ASCII digits
	  printf("%d cm, ",Distance);
	  printf("%d cm\r\n",average);
	  HAL_Delay(100);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }

참고 : https://www.arduino.cc/en/Tutorial/BuiltInExamples/Smoothing

 

 

 

 

 

 

 

 

 

 

파란색 : 필터링 전

녹색 : 필터링 후

 

 

노이즈는 어느정도 제거가 됐지만 반응속도가 느려진 것은 어쩔 수 없다.

코드에 있는 배열의 길이가 커질수록 이평선이 부드럽게 그려지겠지만 반응속도는 더 느려질 것이다.

 

 

 

 

 

 

그리고 지금은 결과치인 Distance의 이동평균을 내줬는데 원래 소스인 Echo핀의 HIGH 상태 유지 시간 Difference의 이동평균을 낸 후 UART 출력 전에 변환을 하는 방법이 더 좋은 것 같다. 어차피 둘다 인터럽트에서 쓰이는 전역변수니까 while문에서 가져다 쓰면 된다.