[Drone Firmware using STM32CubeIDE] 2-2-1. ICM-20602 DC Offset Remove

 자이로 3축 + 가속도 3축, 총 6축 센서인 ICM-20602는 드론의 각속도를 측정한다. 책상에 위에 가만히 놔둔 채로 ICM-20602의 센서값을 출력해보면 다음과 같이 출력된다.

 이론상으로 책상 위에 가만히 올려두면 각속도의 값은 0이 나와야한다. 하지만 그래프에 보이는바와 같이 0이 아닌 다른 값이 출력되고 있는 것이 확인된다. 이는 DC offset이 껴있기 때문이다. 이를 DC Bias라고도 한다. 이렇게 오프셋이 껴있는 상태로 비행 제어를 하면 드론이 한쪽으로 흐르는 현상이 발생할 수 있다. 따라서 이 오프셋을 제거해야한다.

 

 DC offset을 소프트웨어적으로 처리하여 제거할 수 있지만, ICM-20602은 자이로 오프셋을 제거해주는 기능을 제공한다. 

 

ICM-20602 Gyro Offset Adjustment Register

 DC offset을 제거하는 방법은 아주 간단하다. 오프셋 값만큼 빼주거나 더해주면 된다. ICM-20602는 오프셋을 제거해주는 레지스터가 있다. 이 레지스터에 어떤 값을 넣어주면, 넣어준 값만큼 더해줘서 센서값이 출력된다.

 Gyro Offset Adjustment Register에 넣을 값은 총 16bit로 상위 8비트[15:8]와 하위 8비트[7:0]를 각각 다른 레지스터에 저장한다. 상위 8비트를 낮은 번지의 레지스터에 저장하고 하위 8비트를 높은 번지의 레지스터에 저장하는 Big Endian 표현 방식이다.

 또한 2의 보수 형태로 숫자 표현되며 레지스터에 값을 넣을 땐, 2를 곱해서 넣어줘야한다. (이유는 데이터시트상에 명시되어있지 않다..) 위의 두 레지스터에 들어있는 값이 센서 값에 더해져서 출력된다. 예를 들어, Gyro DC offset이 -6이라고 가정하면 Offset Adjustment Register에 -6 * (-2) = 12 를 넣어주면 된다. 

 Y, Z Gyro Offset Adjustment Register에 대한 내용도 앞서 설명한 내용과 같고 레지스터의 주소만 다르다.

 

 ICM-20602의 레지스터의 값은 리셋하면 초기화되므로 센서 리셋시마다 매번 다시 값을 넣어줘야 한다. 즉, 부팅 할 때마다 레지스터에 값을 넣어줘야 한다. 따라서 장치 초기화 관련 부분에 레지스터에 값을 넣어주는 코드를 작성한다.

 

Source Code

 먼저 오프셋을 제기하기 전의 오프셋값을 확인하자.

ICM-20602 Sensor Data

 오프셋을 제거하기 전의 센서 값 출력 화면이다. 순서대로 gyro_x_raw, gyro_y_raw, gyro_z_raw 값이고, 그래프에선 각각 파란색, 빨간색, 초록색 값이다. 위와 같이 x축(Pitch 방향) 오프셋은 4, y축(Roll 방향) 오프셋은 10, z축(Yaw 방향) 오프셋은 -6 이 껴있는 것이 확인된다. 오프셋 값은 센서마다 껴있는 정도가 다르므로 오프셋 값 확인 후 확인된 값 만큼 제거해줘야 한다. 확인된 오프셋 값을 앞서 설명한 Offset Adjustment Register에 넣어주면 된다. 

 먼저 넣어줄 offset값을 정의하자. 레지스터에 넣어줄 값은 부호있는 16bit이므로 자료형은 short로 선언한다. 

short gyro_x_offset = 4, gyro_y_offset = 10, gyro_z_offset = -6;

 그리고 이 오프셋 값을 Offset Adjustment Register에 넣어주면 된다. 넣어주기 전에 -2를 곱하는 것도 잊지말자.

 먼저 ICM-20602의 레지스터에 값을 넣어주는 함수인 ICM20602_Writebyte() 함수에 대해 살펴보자. 이 함수는 ICM20602.c 파일에 정의되어있다.

void ICM20602_Writebyte(uint8_t reg_addr, uint8_t val)
{
	CHIP_SELECT(ICM20602);
	SPI1_SendByte(reg_addr & 0x7F); //Register. MSB 0 is write instruction.
	SPI1_SendByte(val); //Send Data to write
	CHIP_DESELECT(ICM20602);
}

 이 함수의 첫 번째 매개변수는 해당 레지스터의 주소이고, 두 번째 매개변수로는 해당 레지스터에 넣어줄 값으로 두 매개변수 모두 (uint8_t) 8비트이다. 먼저 Chip Select핀을 Low로 떨어뜨린 후, 레지스터에 주소를 보낸다. 이때, 쓰겠다는 의미로 주소의 최상위 비트를 0으로 초기화한다. 이는 0x7F(= 최상위 비트 0과 AND)와의 AND연산으로 구현되어있다. 그리고 레지스터에 넣어줄 값을 보내고 Chip Select를 High로 올려 SPI 통신을 끝낸다.

 

 ICM20602_Writebyte() 함수로 레지스터에 오프셋 값을 넣어준다. 

  ICM20602_Writebyte(0x13, (gyro_x_offset*-2)>>8);
  ICM20602_Writebyte(0x14, (gyro_x_offset*-2));
  ICM20602_Writebyte(0x15, (gyro_y_offset*-2)>>8);
  ICM20602_Writebyte(0x16, (gyro_y_offset*-2));
  ICM20602_Writebyte(0x17, (gyro_z_offset*-2)>>8);
  ICM20602_Writebyte(0x18, (gyro_z_offset*-2));

 앞서 살펴본 데이터시트대로 Offset Adjustment Register에 오프셋 값을 넣어준다. 먼저 상위 8비트를 넣어준다. 앞서 각각의 변수(gyro_x_offset, ...)들은 short형으로 선언하였다. 따라서 이 값을 그대로 매개변수로 전달하면 상위 8비트는 잘리고, 하위 8비트 1byte만이 레지스터에 저장된다. 상위 8비트를 저장하려면 해당 변수를 우로 8bit shift해준 후에 매개변수로 넣어줘야 한다. 하위 8비트는 해당 변수를 그대로 사용한다.

 이제 코드를 다운로드 한 후, 오프셋이 제거되었는지 확인해보자.

 

After Remove,

 다음은 Offset Adjustment Register에 값을 넣어준 후, 가만히 정지해 있을 때의 ICM-20602 센서 값 출력 화면이다.

ICM-20602 Sensor Data

 위와 같이 정지해 있을 때 자이로 센서값이 0을 유지하는 것이 확인된다. 값이 1씩 튀는 것도 여전히 보이는데, 이는 각속도 단위로 변환하면 미세한 수준이라 무시할 정도이다. 이렇게 오프셋을 제거하여 비행 성능을 조금 높일 수 있을 것으로 기대된다.