iFBalance

iFBalance-코딩으로 뽀샤버리기

8강. 거리측정 ToF센서 제어하기

Posted by INDIFROG on 2021-03-08
Words 1.3k and Reading Time 8 Minutes
Viewed Times

8강. 거리측정 ToF센서 제어하기

iFBalance의 거리 센서인 ToF 센서의 종류는 2가지가 있습니다. 

1. VL53L3CX
2. VL53L1CXV0FY

ToF 센서의 단가가 비싸기도 하고 최근 반도체 품귀 사태 여파로 부품수급이 원할하지 않기때문에 iFBalance의 거리센서로 아래 그림에서 왼쪽의 VL53L3CX 라고 하는 ST의 ToF 센서모듈만 양산이 되어 있습니다.

VSCODE_LIB

사실 성능은 두번째인 VL53L1CXV0FY가 4M 까지 거의 선형으로 mm 단위의 뛰어난 정확성으로 거리를 판별할 수 있습니다만 가격이 VL53L3CX의 두배 이상입니다. 개발 시점에서는 VL53L1CXV0FY 칩셋으로 제작을 해서 공개된 C/C++ 라이브러리로 테스트를 했고 매우 우수한 성능을 얻을 수 있었습니다.

VSCODE_LIB

그러나 Field of View가 25도, 3M 정도까지 측정이 가능하며 다중 장애물 검출가능한 특징이 있는 VL53L3CX 칩셋과는 Pin2Pin이고 해서 크게 무리없이 사용이 가능하다고 해서 VL53L3CX으로 양산을 했었습니다. 특히, 다중 장애물 검출가능한 특징은 이륜 밸런스 로봇처럼 전진시에 10도 내외로 앞쪽으로 기울여야 이동이 가능한 로봇의 경우 반드시 필요한 기능입니다.

VL53L1CXV0FY에 비해 VL53L3CX 센서도 성능이 크게 떨어지는 것은 아닙니다만 ST에서 오픈소스로 제공하고 있는 VL53L3CX 전용 라이브러리 소스는 제가 테스트를 할 당시에는 ESP32에서는 동작하지 않았기 때문에 최종 릴리즈 시점에서 시간이 없는 관계로 VL53L1CXV0FY용 소스를 일부 수정해서 직접 튜닝을 완료했습니다.

VSCODE_LIB

iFBalance 제어 앱인 iFThings App에서 요구하는 1M 이내 충돌방지거리를 설정할 수 있는 기능은 VL53L3CX 센서를 사용해서도 충분히 제 역할을 할 수 있는 성능입니다.

VSCODE_LIB

그런데 다행히 오늘 강좌 자료를 정리하면서 ST에서 오픈소스로 제공하고 있는 VL53L3CX 전용 라이브러리 소스의 문제점(?)을 확인하고 몇 군데 수정을 해서 최종 동작하는 것을 확인했습니다.

VSCODE_LIB

하기 VL53L3CX 라이브러리를 프로젝트에 추가합니다.

VSCODE_LIB

그런데 이 라이브러리의 코드 수정없이 바로 동작하기 위해서는 VL53L3CX칩의 XSHUT 핀에 iFZero의 임의의 GPIO와 연결이 되어 있어야 하는데 iFBalance의 남는 GPIO가 없다 보니 연결을 하지 않고 단지 XSHUT 핀을 항상 풀업 상태로 둔 것이 문제의 원인이었습니다.

VSCODE_LIB

그렇다고 이 문제를 해결할 수 없는 것은 아닙니다.

VSCODE_LIB

VL53L3CX 칩의 첫번째 0번 레지스트리 이름이 이 문제를 해결해 줄 실마리입니다.

VL53LX_SOFT_RESET : VL53LX_software_reset()

VL53LX 클래스의 VL53LX_software_reset() 멤버함수를 사용하는 것이 해결책이 됩니다.

VL53LX 라이브러리 소스코드를 직접 수정할 필요가 있습니다.
해당 소스 코드는 .pio/build/libdeps/kits-edu/STM32arduino_VL53L3CX 폴더내에
vl53lx_class.h 헤더파일입니다.

  1. 먼저, VL53LX 클래스의 디폴트 생성자 하나 추가해 줍니다. 기존 생성자는 VL53LX(TwoWire *i2c, int pin) 로 반드시 파라미터를 사용해서 생성을 해야 하므로 추가 사용자 클래스에서 멤버 변수로 사용하기가 어렵습니다.

간혹, 디폴트 생성자가 없는 클래스는 다른 클래스의 멤버 변수로 사용할 수가 없는 문제가 있습니다. 가능하다면 클래스를 만들때, 반드시 디폴트 생성자를 추가해 주고, 추가적으로 파라미터를 대입하고 초기화할 수 있는 init() 함수를 제공하는 방식에 익숙해 질 필요가 있습니다. 

1
2
3
4
5
6
7
8
9
10
11
12
13
/** Constructor 
    *  by added Serimo, 2021.06.21
    *  for iFBalance, ESP32_PICO_V3
    */
   VL53LX() : RangeSensor()   
   {
     dev_i2c = &Wire;
     gpio0 = -1;
     Dev = &MyDevice;
     memset((void *)Dev, 0x0, sizeof(VL53LX_Dev_t));
     MyDevice.I2cHandle = dev_i2c;
     MyDevice.I2cDevAddr = VL53LX_DEFAULT_DEVICE_ADDRESS ;
   }
  1. 두번째는 VL53LX 클래스의 InitSensor 함수에서 VL53LX_software_reset() 함수를 호출해 주면 됩니다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
VL53LX_Error InitSensor(uint8_t address)
    {
      VL53LX_Error status = VL53LX_ERROR_NONE;
      
      // VL53LX_Off();
      // VL53LX_On();

      /** 
        *  by added Serimo, 2021.06.21
        *  for iFBalance, ESP32_PICO_V3
        */
      VL53LX_software_reset();

      status = VL53LX_SetDeviceAddress(address);
      if (status == VL53LX_ERROR_NONE) {
        status = VL53LX_WaitDeviceBooted();
      }

      if (status == VL53LX_ERROR_NONE) {
        status = VL53LX_DataInit();
      }

      return status;
    }
  1. 셈플 예제에서 잘못 사용된 디폴트 I2C주소( 0x12 => 0x52)의 오류와 인터럽트를 수신하기 위해 초기에 VL53LX_ClearInterruptAndStartMeasurement 함수를 호출해 주어야 하는데… 역시 누락된 문제…

실제로 이 문제들이… 가장 큰 원흉입니다.

다음은 이들을 호출하는 iFVL53LX의 init()함수 입니다.
먼저 VL53LX 객체를 생성하고 나서 init()함수에서 이 객체에 대한 초기화 코드를 호출합니다.
따라서 코드를 간결하게 사용하기 위해 iFVL53LX와 같은 래퍼 클래스를 만드는 이유입니다.

1
2
3
4
5
6
7
8
9
10
11
12

VL53LX      iFVL53LX::vl53lx;

void iFVL53LX::init(){
  // Configure VL53LX satellite component.
  vl53lx.begin();
  vl53lx.InitSensor(0x52);

  // Start Measurements
  vl53lx.VL53LX_StartMeasurement();
  vl53lx.VL53LX_ClearInterruptAndStartMeasurement(); 
}

참… 위 문제들을 수정해 줌으로써 ST에서 릴리즈한 VL53L3CX용 오픈 소스는 잘 동작할 것입니다.

공짜로… 제가 처음으로 공개하는 것입니다.

자… 이제 실제로 동작하는 클래스를 만들어봐야죠… 음. 자존심 때문에 위 클래스를 그냥 쓸 수는 없지요…
사실, 자존심이라기 보다는 코드의 가독성과 FreeRTOS에서 동작시키기 위해 iFVL53LX 클래스를 만들었습니다.

iFVL53LX.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
/******************************************************************************
iFVL53L1X.h - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFZero, iFLine, iFBalance
******************************************************************************/

#pragma once

#include <Arduino.h>
#include <vl53lx_class.h>

class iFVL53LX {

    private:
        static uint32_t     m_nDistance;
        static TaskHandle_t taskHandle;
        static portTickType delayTask; 
        static VL53LX       vl53lx;

        static volatile int interruptCount;

        int interruptPin  = 9;
    public:
        static SemaphoreHandle_t xMutex;

    public:
        iFVL53LX();

        void init();
        void begin();

        static void onHandler();
        static void taskProcess(void *pvParameter);
        static void calcDistance();
        static uint32_t getDistance();
};

iFVL53LX.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
#include <Arduino.h>
#include "iFVL53LX.h"

uint32_t iFVL53LX::m_nDistance = 0;
TaskHandle_t iFVL53LX::taskHandle = NULL;
portTickType iFVL53LX::delayTask = 100; 
SemaphoreHandle_t iFVL53LX::xMutex = NULL;
volatile int iFVL53LX::interruptCount = 0;
VL53LX      iFVL53LX::vl53lx;

void iFVL53LX::onHandler()
{
    if (xSemaphoreTake( xMutex, portMAX_DELAY ) == pdTRUE){       
      interruptCount++;
    }
    xSemaphoreGive( xMutex );
}

iFVL53LX::iFVL53LX(){
}

void iFVL53LX::taskProcess(void *pvParameter) {
  portTickType delay = *(portTickType*)pvParameter;

  while(1) {    
    if (xSemaphoreTake( xMutex, portMAX_DELAY ) == pdTRUE){      
      calcDistance();
    }
    xSemaphoreGive( xMutex );
    vTaskDelay( delay / portTICK_RATE_MS );
  }
}

void iFVL53LX::calcDistance(){
  VL53LX_MultiRangingData_t MultiRangingData;
  VL53LX_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
  uint8_t NewDataReady = 0;
  int no_of_object_found = 0, j;

  //if( interruptCount > 0 )
  {
    int status = vl53lx.VL53LX_GetMeasurementDataReady(&NewDataReady);

    if((!status)&&(NewDataReady!=0))
    {
      status = vl53lx.VL53LX_GetMultiRangingData(pMultiRangingData);
      no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
      for(j=0;j<no_of_object_found;j++)
      {
        m_nDistance = pMultiRangingData->RangeData[j].RangeMilliMeter;
      }

      if (status==0)
      {
        status = vl53lx.VL53LX_ClearInterruptAndStartMeasurement();
      }  
    }
  }
}

uint32_t iFVL53LX::getDistance(){
    return m_nDistance;
}

void iFVL53LX::init(){
  //pinMode(interruptPin, INPUT_PULLUP);
  //attachInterrupt(interruptPin, onHandler, FALLING);

  // Configure VL53LX satellite component.
  vl53lx.begin();
  vl53lx.InitSensor(0x52);

  // Start Measurements
  vl53lx.VL53LX_StartMeasurement();
  vl53lx.VL53LX_ClearInterruptAndStartMeasurement();    
}

void iFVL53LX::begin(){  
    xMutex = xSemaphoreCreateMutex();
    xTaskCreatePinnedToCore(&taskProcess, 
                          "taskProcessVL53L1X", 
                          2048, 
                          (void*)&delayTask, 
                          uxTaskPriorityGet(NULL), 
                          &taskHandle, 
                          0);     
}

main.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
/******************************************************************************
main.cpp - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFBalance
******************************************************************************/

#include <Arduino.h>
#include "iFLED.h"
#include "iFMixer.h"
#include "iFEncoder.h"
#include "MPU9250.h"
#include "iFPID.h"
#include "iFVL53LX.h"

#define SerialDebug   false  // Set to true to get Serial output for debugging
#define I2Cclock      400000
#define MPU_INT_PIN   5 // Interrupt Pin definitions

iFLED         ifLed;
iFMixer       ifMixer;
iFEncoder     ifEncoder;
iFPID         ifPid;

MPU9250       ifMpu;
iFVL53LX      ifVL53LX;

#define SerialPort Serial

// Encoder Count
int32_t      _nCurLeft = 0, _nCurRight = 0;

void getEncoderSpeed(){     
    portENTER_CRITICAL(&ifEncoder.muxr);  
    _nCurLeft = ifEncoder.getCountLeftReset();          
    _nCurRight = ifEncoder.getCountRightReset();          
    portEXIT_CRITICAL(&ifEncoder.muxr);
}

void setup()
{ 
  Serial.begin( 921600 );

  Wire.begin(21, 22, I2Cclock);

  ///////////////////////////////////////////////////////
  // VL53LX
  ifVL53LX.init();
  ifVL53LX.begin();

  ///////////////////////////////////////////////////////
  // MPU9250
  ifMpu.selectFilter( QuatFilterSel::MADGWICK);
  ifMpu.setup(0x68);  // change to your own address
  delay(2000);      // to get stable data of MPU9250

  ///////////////////////////////////////////////////////
  // Color LED
  ifLed.init();  
  ifLed.setMode( BLINK );

  ifMixer.init();
  ifPid.init();
  ifPid.SetOutputLimits( -ifMixer.getMaxOutput(), ifMixer.getMaxOutput());

  ///////////////////////////////////////////////////////
  // Encoder
  ifEncoder.init(25, 14, 13, 15);  
}

void loop() {
  bool isIMUUpdate = false;
  uint32_t distance = 0;
  if (xSemaphoreTake( iFVL53LX::xMutex, portMAX_DELAY ) == pdTRUE){ 
    isIMUUpdate = ifMpu.update();
    distance = ifVL53LX.getDistance();
  }
  xSemaphoreGive( iFVL53LX::xMutex );

  if (isIMUUpdate) {    
    getEncoderSpeed();
    uint32_t speed = abs(_nCurLeft) + abs(_nCurRight) / 2;
    Serial.println("speed: " + String(speed) + "   distance: " + String(distance));

    double motorVals[2];
    ifPid.calc(ifMpu.getPitch(), &motorVals[0]);
    ifMixer.run( &motorVals[0]);
  }
}

iFBalance를 개발하면서 가장 늦게 안정화가 이루어진 코드 부분을 보고 있습니다. 가장 먼저 만들었음에도 불구하고 모듈별 테스트할 때는 전혀 문제가 없다가 FreeRTOS, 인터럽트가 동작하고 2개 이상의 칩에 의해 I2C 포트가 공동으로 사용이 될 때는 어느 순간 끝없이 리부팅을 하게 됩니다.

즉, 단독으로 iFVL53LX 클래스 내부적으로 FreeRTOS, 인터럽트를 사용할 때는 문제없이 동작을 하지만 MPU9250 센서와 VL53LX 센서 둘다 동시에 I2C 버스에 접근하면 문제가 발생하게 되는 것이죠.

이와 같은 문제가 발생하지 않도록 loop 함수 내에서 xSemaphoreTake 함수로 iFVL53LX의 태스크를 잠시 대기시키고 MPU9250에게 I2C 버스의 사용 권한을 넘긴 후 MPU9250의 데이터를 읽어오게 합니다.

그리고 나서 xSemaphoreGive 함수로 iFVL53LX의 태스크의 대기 상태를 해제하고 I2C 버스를 사용 권한을 VL53LX 센서에게 넘깁니다.

IMU센서가 업데이트가 되면 iFBalance의 움직이는 스피드와 거리등의 값을 시리얼디버그에 출력을 하면서 동시에 자세 균형을 유지할려고 하는 코드입니다. 여기서 중요한 점은 FreeRTOS를 이용하여 delay없이 각각의 태스크에서 처리한 값을 가져와서 동시에 처리한다는 것이죠.

소스는 간단한 것처럼 보이지만, 리부팅없이 장애물과의 거리까지 계산하는 꽤 잘 동작하는(?) 밸런싱 로봇을 위한 복잡하고 난해한 코드입니다.

그러나 아직은 왔다리 갔다리 하는 걸음마단계의 수준이며, 밸런싱 로봇의 흔들리는 움직임등으로 인해 장애물과의 거리도 역시 흔들리는 문제, 기타 원격으로 제어하는 문제 등 무수히 많은 해결해야 할 숙제가 남아 있습니다.

이제 남은 문제는 여러분들이 할 내용입니다. ㅋㅋㅋ

iFBalance 8강 소스 파일 : iFBalance_08.zip