Gyro Accel MPU 6050 - FabLabSeoul/WingProject GitHub Wiki

• MPU-6050
  • Gyro 3 Axis + Accel 3 Axis 인식 센서
  • 센서 소개: http://www.invensense.com/mems/gyro/mpu6050.html
  • Spec: http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf

(GY-521 breakout보드에 MPU-6050이 장착되어 있다.)

STM32 Value line 보드에서 GY-521 breakout 보드를 이용하여 MPU-6050을 테스트 한다. 소스는 STM32\stm32vldiscovery_package\Project\Examples\MPU-6050 에 있다.

이 프로젝트 소스는 MPU-6050과 I2C 통신으로 가속도 x,y,z (단위 g), 자이로 x,y,z (단위 각속도, degree/seconds) 값을 가져와서 roll, pitch, yaw 각을 계산한다. 계산은 Stm32에서 이뤄지고, 결과를 시리얼 통신으로 보낸다. 시리얼 정보를 받은 컴퓨터는 Processing 프로그램을 통해 3차원 모델로 출력한다.

아직 Stm32 Value line 보드로 컴퓨터와 시리얼통신을 하는 법을 몰라서, 아두이노를 거쳐 컴퓨터에 시리얼 정보를 보낸다. 센서정보는 다음과 같이 흘러간다. GY-521 -> Stm32 -> Arduino -> Computer -> Processing (이 문제 해결은 Stm32 printf Debugging, Stm32 Serial Debugging 문서에서 확인할 수있다.)

이 예제를 만들기위해 두개의 오픈 소스를 사용하였다. MPU-6050에서 I2C 통신으로 자이로, 가속도 값을 받아오는 부분은 https://harinadha.wordpress.com/2012/05/23/mpu6050lib/ 라이브러리를 이용했다. (이 소스는 ACK를 무한정 기다리는 버그가 있기 때문에 실제 큐브에서는 쓰지 않을 계획이다.)

MPU-6050으로부터 받은 정보를 실제로 쓰일 Roll, Pitch, Yaw 각으로 바꾸는 계산은 http://m.kocoafab.cc/tutorial/view/239 을 이용했다.

회로구성은 https://harinadha.wordpress.com/tag/stm32/ 을 참고하자. Stm32 I2C포트로 쓰일 PB10,11을 GY-521의 SCL, SDA 핀에 연결하면 된다. 정확히는 다음과 같다.

• GY-521 Vcc -> 3.3V or 5V
• GY-521 Gnd -> Ground
• Stm32 PB10(I2C2 SCL) -> GY-521 SCL
• Stm32 PB11(I2C2 SDA) -> GY-521 SDA

I2C 설정

MPU-6050과 통신을 하기 위한 I2C 초기화 코드다. 이는 STM32 I2C문서와 크게 다르지 않다. PB10,11 포트는 MPU-6050과의 통신을 위해 쓰이고, I2C주소를 0x68로 설정한다. 마스터인 Stm32 MCU는 슬래이브인 MPU-6050에 접근하기 위해 0x68주소로 I2C통신을 시작하게 된다.

이 소스코드는 Harinadha Reddy가 짠 코드의 일부다. (https://harinadha.wordpress.com/about/)

#define MPU6050_I2C                  I2C2
#define MPU6050_I2C_RCC_Periph       RCC_APB1Periph_I2C2
#define MPU6050_I2C_Port             GPIOB
#define MPU6050_I2C_SCL_Pin          GPIO_Pin_10
#define MPU6050_I2C_SDA_Pin          GPIO_Pin_11
#define MPU6050_I2C_RCC_Port         RCC_APB2Periph_GPIOB
#define MPU6050_I2C_Speed            100000 // 100kHz standard mode

MPU6050_I2C_Init();

/**
 * @brief  Initializes the I2C peripheral used to drive the MPU6050
 * @param  None
 * @return None
 */
void MPU6050_I2C_Init()
{
    I2C_InitTypeDef I2C_InitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;

    /* Enable I2C and GPIO clocks */
    RCC_APB1PeriphClockCmd(MPU6050_I2C_RCC_Periph, ENABLE);
    RCC_APB2PeriphClockCmd(MPU6050_I2C_RCC_Port, ENABLE);

    /* Configure I2C pins: SCL and SDA */
    GPIO_InitStructure.GPIO_Pin = MPU6050_I2C_SCL_Pin | MPU6050_I2C_SDA_Pin;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
    GPIO_Init(MPU6050_I2C_Port, &GPIO_InitStructure);

    /* I2C configuration */
    I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
    I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
    I2C_InitStructure.I2C_OwnAddress1 = MPU6050_DEFAULT_ADDRESS; // MPU6050 7-bit adress = 0x68, 8-bit adress = 0xD0;
    I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
    I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
    I2C_InitStructure.I2C_ClockSpeed = MPU6050_I2C_Speed;

    /* Apply I2C configuration after enabling it */
    I2C_Init(MPU6050_I2C, &I2C_InitStructure);
    /* I2C Peripheral Enable */
    I2C_Cmd(MPU6050_I2C, ENABLE);
}

I2C를 통한 MPU-6050 초기화

MPU-6050과 I2C통신을해서 옵션을 설정한다. 옵션은 MPU-6050의 고유 레지스터 주소값에 정보를 쓰는 것을 바탕으로 한다. I2C 통신을 통해 MPU-6050 레지스터에 데이타를 쓰는 방법은 다음과 같다.

---

MPU-6050에 정보를 쓸 때.

• 마스터가 MPU-6050에 Start신호를 보낸다. (쓰기 모드 W)
• 기본 과정을 거친다. ~ (ACK)
• 값을 바꿀 MPU-6050의 레지스터 주소값을 보낸다. (RA)
• 기본 과정을 거친다. ~ (ACK)
• 해당 레지스터 주소에 저장할 정보를 한 바이트씩 송신한다. (DATA)
  • 접속을 끊을 때까지 정보가 송신되고, 받는 측에서는 자동적으로 다음 주소를 가르키게 된다.
• 연결을 끊는다.

---

MPU-6050 정보를 받아 올 때.

• 마스터가 MPU-6050에 Start신호를 보낸다. (쓰기 모드 W)
• 기본 과정을 거친다. ~ (ACK)
• 값을 얻어올 MPU-6050의 레지스터 주소값을 보낸다. (RA)
• 다시 Start신호를 보낸다. (읽기 모드 R)
• 기본 과정을 거친다. ~ (ACK)
• 데이타를 한 바이트씩 원하는 횟수만큼 받는다. (DATA)
  • ACK를 보낸 횟수만큼 정보를 받고, 보내는 쪽에서 자동적으로 다음 메모리 주소를 가르키게 된다.
• 연결을 끊는다.

MPU-6050의 레지스터 주소는 매뉴얼을 참조하자. 일단 우리가 관심을 가지는 gyro_x,y,z, accel_x,y,z 레지스터 주소는 다음과 같다. 메모리주소가 순서대로 ACCEL_XOUT_H ~ GYRO_ZOUT_L 까지 나열되어 있기 때문에, 한 번의 접속으로 순서대로 읽으면 모두 읽어 올 수 있다. 가운데 온도센서 값을 가져오지만(TEMP_OUT_H,L), 이 값은 무시한다.

MPU-6050초기화 코드는 다음과 같다. MPU-6050 옵션은 매뉴얼을 통해 확인하자.

자이로 센싱 범위, 가속도 센싱 범위를 각각 +-250 degree/s, +-2g 로 설정했다.

/** Power on and prepare for general usage.
 * This will activate the device and take it out of sleep mode (which must be done
 * after start-up). This function also sets both the accelerometer and the gyroscope
 * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
 * the clock source to use the X Gyro for reference, which is slightly better than
 * the default internal clock source.
 */
void MPU6050_Initialize()
{
    MPU6050_SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
    MPU6050_SetFullScaleGyroRange(MPU6050_GYRO_FS_250);
    MPU6050_SetFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    MPU6050_SetSleepModeStatus(DISABLE);
}

---

MPU-6050 매뉴얼은 다음과 같다.

자이로센서 Scale Factor, 가속센서 Scale Factor가 각각 +-250, 2g일 때, 131, 16384인 것을 확인하자. 나중에 이 값으로 roll, pitch, yaw 값을 계산한다.

---

MPU_6050과 I2C 통신을 하는 간단한 예로, 자이로 센서 범위 설정하는 코드를 보자. MPU6050_RA_GYRO_CONFIG 레지스터 주소에 정보를 쓰기전에, 옵션이 덮어 씌어질 수 있기 때문에, 기존 옵션 정보를 가져온 후, 바꿀 비트값만 업데이트 한다. 이 과정은 MPU6050_WriteBits() 함수에 처리 한다. 우리가 주목할 것은, 정보를 쓸 때, 어떤 흐름으로 I2C통신을 하느냐이니 MPU6050_I2C_ByteWrite()함수를 보자. MPU-6050과 접속을 시작한 후, 레지스터 주소를 송신하고, 이 후에 쓸 데이타를 보내고 있다.

이 코드는 Harinadha Reddy가 작성했다.

MPU-6050에서 I2C통신으로 정보를 가져오는 부분은 MPU6050_I2C_BufferRead() 함수를 확인하자. 위에 나온 시퀀스 다이어그램과 일치한다.

MPU6050_I2C_ByteWrite(MPU6050_DEFAULT_ADDRESS, data, MPU6050_RA_GYRO_CONFIG);

void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8* pBuffer, u8 writeAddr)
{
    // ENTR_CRT_SECTION();

    /* Send START condition */
    I2C_GenerateSTART(MPU6050_I2C, ENABLE);

    /* Test on EV5 and clear it */
    while (!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));

    /* Send MPU6050 address for write */
    I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter);

    /* Test on EV6 and clear it */
    while (!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

    /* Send the MPU6050's internal address to write to */
    I2C_SendData(MPU6050_I2C, writeAddr);

    /* Test on EV8 and clear it */
    while (!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

    /* Send the byte to be written */
    I2C_SendData(MPU6050_I2C, *pBuffer);

    /* Test on EV8 and clear it */
    while (!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

    /* Send STOP condition */
    I2C_GenerateSTOP(MPU6050_I2C, ENABLE);

    // EXT_CRT_SECTION();
}

MPU-6050과 정상적으로 연결되었는지를 확인하고, 센서 정보를 가져온다.

MPU-6050과 정상적으로 연결이 되었다면, MPU6050_GetRawAccelGyro() 함수로 센서 정보를 가져온다. 6개의 정수값은 순서대로 가속도 x,y,z 자이로센서 x,y,z 값을 가져온다.

int16_t  AccelGyro[6]={0};

if (MPU6050_TestConnection() != 0)
{
 // connection success
}
else
{
	 // connection failed
	return 0;
}
 	
MPU6050_GetRawAccelGyro(AccelGyro);

MPU-6050 센서정보를 Roll,Pitch,Yaw 각으로 변환

이 코드는 http://m.kocoafab.cc/tutorial/view/239 에서 가져 왔고, 원본은 Krodal이 작성했다.

가속도 센서와 자이로 센서를 간단한 상보필터를 이용해서 구현하고 있다. 가속도를 roll, pitch, yaw로 변환하는 공식은 여러 사이트에서 확인할 수 있다. 여기서는 yaw값은 구하지 않고 있다.

• http://pinkwink.kr/73
• http://hs36.tistory.com/19

//
// 소스 참조. 아두이노 소스를 stm32 소스로 바꿨다.
// http://m.kocoafab.cc/tutorial/view/239
//
void SendSerialAccelGryro( int16_t accelgyro[6] )
{
	//  250 degree/s 범위를 가지고, 이 값을 16 bit 분해능으로 표현하고 있으므로, 
	// mpu-6050에서 보내는 값은 -32766 ~ +32766 값이다.
	// 그러므로 이 값을 실제 250 degree/s 로 변환하려면 131로 나눠줘야 한다. 범위가
	// 다르면 이 값도 같이 바껴야한다.
	float FS_SEL = 131;
	
	  //회전을 했을 떄 시간 알기
  unsigned long t_now = GetTickCount();

	
  float gyro_x = (accelgyro[3] - base_x_gyro)/FS_SEL;
  float gyro_y = (accelgyro[4] - base_y_gyro)/FS_SEL;
  float gyro_z = (accelgyro[5] - base_z_gyro)/FS_SEL;


// 가속도 값 범위는?
// 16bit 니 -32766 ~ +32766 범위이고,
//	 +-2g 범위라면 mpu-6050으로부터 넘어온 값을 실제 g단위로 환산하려면
//	  scale factor(16384)로 나눠줘야 한다. +-2g 범위는 +-32766값이다. 즉 32766값이면 2가 된다.	

  //acceleration 원시 데이터 저장
  float accel_x = accelgyro[0];
  float accel_y = accelgyro[1];
  float accel_z = accelgyro[2];
	
	 
  //accelerometer로 부터 각도 얻기
  float RADIANS_TO_DEGREES = 180/3.14159;

  // float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
  float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
  float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
  float accel_angle_z = 0;
	

  //gyro angles 계산1
  float dt =(t_now - get_last_time())/1000.0;
  float gyro_angle_x = gyro_x*dt + get_last_x_angle();
  float gyro_angle_y = gyro_y*dt + get_last_y_angle();
  float gyro_angle_z = gyro_z*dt + get_last_z_angle();
  
  //gyro angles 계산2
  float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle();
  float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle();
  float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle();
  
  //알파를 이용해서 최종 각도 계산3
  float alpha = 0.96;
  float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
  float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
  float angle_z = gyro_angle_z;  //Accelerometer는 z-angle 없음	
	
	
 //최종 각도 저장
  set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
 	
	// 시작 문자 전송
	print_byte('S');	
	
	print_byte( (int16_t)dt );
	
	// 2 바이트 정수로 보내기 위해 100을 곱하고, 받을 때, 100을 나눠준다. 
	print_short( (int16_t)(accel_angle_x*100) );
	print_short( (int16_t)(accel_angle_y*100) );
	print_short( (int16_t)(accel_angle_z*100) );

	print_short( (int16_t)(unfiltered_gyro_angle_x*100) );
	print_short( (int16_t)(unfiltered_gyro_angle_y*100) );
	print_short( (int16_t)(unfiltered_gyro_angle_z*100) );
	
	print_short( (int16_t)(angle_x*100) );
	print_short( (int16_t)(angle_y*100) );
	print_short( (int16_t)(angle_z*100) );
}

---

레퍼런스

• https://harinadha.wordpress.com/2012/05/23/mpu6050lib/
• 아두이노에서 MPU-6050을 사용하는 법
  • http://m.kocoafab.cc/tutorial/view/239
• mpu-6050 매뉴얼 번역
  • http://blog.naver.com/heennavi1004/10183541403
• http://www.nano-i.com/salse/1272665