[드론] 상보필터를 적용한 기본 루틴 구현하기

기본 루틴 구현하기

[목표]

드론을 움직여도 프로세싱상에서 보여지는 도형들이 움직이지 않도록 Roll, Pitch, Yaw 각도에 대해 0으로 초기화 시킨 값만 전달하기

바로 코드로 넘어가겠습니다.

소스 코드

//기본 루틴 구현
#include 
 
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
 
void setup() {
  initMPU6050(); //가속도 자이로 센서 값을 읽음
  Serial.begin(115200);
}
 
void loop() {
  readAccelGyro();
  SendDataToProcessing(); //Roll, Pitch, Yaw에 대한 각도 정보를 보냄
}

//MPU-6050초기화 루틴
void initMPU6050()
{
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
}

 //가속도 자이로 센서를 읽는 루틴
void readAccelGyro()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();
  Tmp = Wire.read() << 8 | Wire.read();
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();
}

float dt;
float accel_angel_x, accel_angel_y, accel_angel_z;
float gyro_angel_x, gyro_angel_y, gyro_angel_z;
float filtered_angel_x, filtered_angel_y, filtered_angel_z;

 //프로세싱 스케치로 각도 정보를 보내는 루틴
void SendDataToProcessing()
{
  Serial.print(F("DEL:"));
  Serial.print(dt,DEC);
  Serial.print(F("#ACC:"));
  Serial.print(accel_angel_x, 2);
  Serial.print(F(","));
  Serial.print(accel_angel_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angel_z, 2);
  Serial.print(F("#GYR:"));
  Serial.print(gyro_angel_x, 2);
  Serial.print(F(","));
  Serial.print(gyro_angel_y, 2);
  Serial.print(F(","));
  Serial.print(gyro_angel_z, 2);
  Serial.print(F("#FIL:"));
  Serial.print(filtered_angel_x, 2);
  Serial.print(F(","));
  Serial.print(filtered_angel_y, 2);
  Serial.print(F(","));
  Serial.print(filtered_angel_z, 2);
  Serial.println(F(""));
  delay(5);
}

다운로드 파일

sketch_sep10c.ino

코드는 위와 같이 되면 결과값은 드론을 아무리 움직여도 도형은 변하지가 않습니다.

요롷게요 ㅎ

이제 코드 설명을 들어가겠습니다.(내 머리속에서 나온거라 아주 자세하게!)

소스코드 설명

일단 들어가기에 앞서 MPU-6050에 대한 기본적인 소스코드 설명은 MPU-6050 가속도 자이로 센서 여기에 들어가시면 자세히 설명되어 있습니다. 그렇지만!!! 저는 공부를 하는 학생이기 때문에 여기다가 다시 정리를 하나부터 백까지 자~세히 정리를 해볼게요 ㅎㅎ

Wire 라이브러리는 I2C를 사용하기 위한 라이브러리 입니다. I2C가 무엇인지 잘 모르시는 분들은 아래 링크를 참고하시면 좋을거 같습니다.

I2C 통신

4번째 줄 : 이 내용은 전에 포스팅에서도 다룬것입니다. I2C 통신을 하는 모듈(Arduino)은 주소를 가지게 되어있습니다.

여기서는 0x68로 설정을 해주었습니다.

5번째 줄 : 그리고 16비트형 정수형 타입으로 7개의 변수를 선언해주었습니다.

※ 16bit로 설정을 해준 이유

->MPU-6050의 특징 중 각 채널에 대한 16bit 크기의 값을 출력해 주는 ADC 모듈을 가지고 있기 때문

8번째 줄 : initMPU6050함수는 가속도 자이로 센서 값을 읽는 함수이며 함수에 대한 정의는 아래쪽에서 설명을 드리겠습니다. 

9번째 줄 : 그리고 시리얼 통신을 115200바운드레이트 속도로 통신을 시작하겠다 라는 의미 입니다.

13번째 줄 : readAccelGyro() 함수는 가속도 자이로 센서 값을 읽는 함수입니다.

14번째 줄 : 프로세싱 스케치로 Roll, Pith, Yaw에 대한 각도 정보를 보내는 함수입니다.

여기서는 무한 루프 구문이니까 매번 센서 값을 읽고 그에 따른 각도 정보를 프로세싱으로 보내지게 되겠죠?

이제 함수 정의에 대한 코드가 시작됩니다.

두둥

19번째 줄 : I2C 통신을 시작하겠다.

20번째 줄 : 0x6B번지 값을 가지는 MPU_6050과 I2C 통신을 시작하겠다.

beginTransmission() 함수는 인자로 주어진 번지와 I2C 슬레이브 모듈과 통신을 시작할 때 사용

21번째 줄 : MPU-6050의 내부에 0x6B번지에는 PWR-MGMT_1 레지스터가 존재합니다. 따라서 전송하고자 하는 1Byte 데이터를 메모리 큐에 저장을 합니다.

22번째 줄 : 메모리 큐에 저장을 하는데 저장하는 것이 0 입니다. 이것은 잠자는 사자의 콧털을 건들여서 깨우는...ㅋㅋ그러니까 잠자는 MPU-6050을 깨운다는 의미입니다.

23번째 줄 : 21번째줄에서 Wire.write 함수를 사용함으로서 큐에 저장된 하나 이상의 Byte 데이터를 슬레이브 모듈로 보내지면서 전송을 끝내는 역할을 지금 23번째 줄이 하고 있는 것 입니다. 가만 보니 인자값이 true인데요 인자값이 true로 넘어오게 되면 데이터 전송 후 정지 메세지를 보내고 I2C 버스의 제어권에 손을 땐다는 의미입니다.

여기에서도 Wire에 대한 함수들이 나왔네요

28번째 줄 : 인자로 주어진 I2C 슬레이브 모듈과 통신을 시작한다.

29번째 줄 : 전송하고자 하는 1Byte 데이터를 메모리 큐에 저장(여기서는 0x3B 번지에 저장)

30번째 줄 : 인자값이 위와는 다르게 false입니다. 이것은 데이터 전송 후 재시작 메세지를 보내게 되며 I2C 버스에 대한 제어권은 여전히 놓지 않고 있는 상태입니다.  

31번째 줄 : 추가적인 데이터를 요청하고 있는데 0x3B 번지 ~ 0x48 번지에 있는 14바이트 데이터를 요청하고 있습니다.

아래 표를 보면 이해하기가 쉬울거에요

(MPU-6050 레지스터 표)

Addr(Hex)	Addr(Dec.)	Register Name
3B	59	ACCEL_XOUT_H
3C	60	ACCEL_XOUT_L
3D	61	ACCEL_YOUT_H
3E	62	ACCEL_YOUT_L
3F	63	ACCEL_ZOUT_H
40	64	ACCEL_ZOUT_L
41	65	TEMP_OUT_H
42	66	TEMP_OUT_L
43	67	GYRO_XOUT_H
44	68	GYRO_XOUT_L
45	69	GYRO_YOUT_H
46	70	GYRO_YOUT_L
47	71	GYRO_ZOUT_H
48	72	GYRO_ZOUT_L

즉, 0X3B 번지부터 총 14바이트 크기의 레지스터에 가속도 자이로 센서 값과 온도 센서 값이 저장이 되게 됩니다.

32번째 ~ 38번째 줄 : wire.read 함수를 이용해서 센서 값들이 저장된 레지스터에 값을 읽어내 I2C 통신을 통해서 1Byte씩 데이터를 받게 되며 총 14바이트의 데이터를 받고 있습니다.

41번째 줄 : 센서 값을 반복적으로 읽는 과정에서 시간 간격을 저장하기 위한 변수입니다.

42번재 줄 : 가속도 센서 값을 해석해서 얻은 Roll, Pitch, Yaw에 대한 각도를 저장하는 변수

43번째 줄 : 자이로 센서 값을 해석해서 얻은 Roll, Pitch, Yaw에 대한 각도를 저장하는 변수입니다.

44번째 줄 : 가속도 + 자이로 센서 값인 상보필터를 적용해서 얻은 Roll, Pitch, Yaw에 대한 각도를 저장하는 병수 입니다.

50번째 줄 : F라는게 보이실 것 입니다. 이것은 Flash-memory based 출력 즉, "DEL:" 문자열을 Flash-memory에 저장되고 Flash-memory에서 문자열을 불러서 출력을 하게 되는 것 입니다.

DEL 문자열 -> Flash-memory -> Flash-memory를 통해 문자열 출력!!

Serial.print(value, ??) 이거에 대한 문장은 저도 처음 보는거라 찾아봤더니 아주 잘 설명되어 있는 곳을 발견했습니다.

출처 : 소프트웨어 놀이터

다음 포스팅에서는 센서 보정 루틴을 구현해서 평균값을 구해보는 시간을 가져보도록 하겠습니다.