서론

---

Gyroscope
Accelometer
이 둘은 드론의 비행에서 빠질 수 없다.
그래서 그 두가지 센서를 가지고 있는 MPU6050을 살펴보자.
MPU6050 칩을 가지고있는 GY-521 모듈 활용

Gyroscope

---

1. Datasheet

STM32 에서 자이로스코프 센서 값을 가져와도 이게 무슨 의미인지 잘 몰랐다.

위 사진은 MPU6050 의 Gyroscope 데이터 시트이다.

Full-Scale Range

단위를 보면 º/s 로 되어있다.
이 뜻은
FS_SEL=0 -> 1초에 ±250도 회전을 측정 가능하다는 것이다. (각속도 측정)

ADC Word Length

16 bits 로 아날로그 값을 디지털로 표현하는 양자화의 범위를 나타낸다.

16 bits -> 2^16 = 65536 (0~65535)
± 값이 있다면 -32768 ~ 32767

Sensivity Scale Factor

단위를 보면 LBS / (º/s) 로 되어있다.
LSB는 Least Significant Bit, 최하위 비트이다.

해석을 해보자면
측정한 각속도 당 센서값(LSB의 개수)이 얼마나 변화한다는 것인가 이다.
FS_SEL = 0 -> 1 º/s 당 131 의 LSB 가 변화함

[ 출력 값 이해 ]
Z-Axis 를 기준으로 5º/s 의 각속도를 가지고 있다면

Z-Axis Output = 5º/s x 131/(º/s) = 655

FS_SEL = 0 일 때 최대 250º/s 각속도를 가진다
-> 250 x 131 = 32750
위 ADC 측정 가능 값의 끝부분에 해당한다.

[ 출력 값 이해 2 ]
Z-Axis Output = 5º/s x 131/(º/s) = 655
10 초 동안 지속한다고 하면
655 x 10 = 6550

-> 6550 / 131 = 50º
(5º/s * 10 = 50º)

즉, 일정 시간동안 누적된 값을 이용하여
얼마나 회전했는지를 알수 있다. (각도)

[ 출력 값 이해 3 ]
데이터 시트에는 체크가 안되었지만 Output Data Rate 라는 것이 있다.
출력 값을 얼마나 자주 만드느냐에 대한 것인데
예를 들어
25HZ -> 1초에 25 번 진동
1번 진동 -> 1/25 초 = 0.04초
즉, 측정 수가 1초에 25번이니까 값의 누적이 25 배가 되는 것이다.

바로 위 식에 대입하면
누적되는 값을 25으로 나누면 된다.
각도 = 6550/25/131 (한번에 계산) = 2º -> 0.4 초 동안 회전 각도
또는
각도 += 655/25/131 (연속 계산 0.2º 변화) -> 1/25 초 동안 회전 각도 변화
(1/25/131 = 약 0.0003053)
이 각도는 Pitch, Roll, Yaw 각도에 해당하는 값이다.

참고)
이해가 안될 경우 역으로 생각해보면 (25Hz, 5º/s 각속도)
25 readings pulse/s x 10s = 250 readings (10초 동안 250 번 읽어옴)
각도 = 250 x 655 / 25 / 131 = 50º

2. 노이즈 제거

Calibration

자이로 스코프는 민감한 센서라서
가만히 있더라도 센서값이 움직인다.
이 튀는 값(노이즈)들을 최소화하기 위해서

평균을 사용해야한다.
하드웨어적 사양이 높다면
상위, 하위 5% 값을 제거하여 평균을 내어도 좋다.

가지고 있는 라이브러리를 보면
X, Y, Z의 자이로 값을 읽어와서
2000 개의 값의 평균을 내고 있다.

void MPU_Gyrocali(MPU6050* mpu6050, ANGLE* angle){
	int32_t temp32[3]={0,0,0};
	int16_t temp16[3];
	uint8_t databuf[6];
	databuf[0]=MPU6050_RA_GYRO_XOUT_H;
	LL_mDelay(100);
	for(uint32_t i=0;i<2000;i++){

		if(i%100==0)
			printf("%ld\r\n",i);

		I2C_Transmit(&mpu6050->I2C,mpu6050->gyro_address,databuf,1);
		I2C_Receive(&mpu6050->I2C,mpu6050->gyro_address,databuf,6);

		temp16[0]=(int16_t)(databuf[0]<<8 | databuf[1]);
		temp16[1]=(int16_t)(databuf[2]<<8 | databuf[3]);
		temp16[2]=(int16_t)(databuf[4]<<8 | databuf[5]);

		temp32[0]+=temp16[0];
		temp32[1]+=temp16[1];
		temp32[2]+=temp16[2];
	}
	angle->gyro_cal[0]=temp32[0]/2000;
	angle->gyro_cal[1]=temp32[1]/2000;
	angle->gyro_cal[2]=temp32[2]/2000;
	printf("gy %d %d %d\r\n",angle->gyro_cal[0],angle->gyro_cal[1],angle->gyro_cal[2]);
}

Vibration

자이로 스코프가 모터의 동작으로 인해 진동이 가해지면 가만히 있더라도 그 값이 튄다.
선형 가속도에 대한 감도
진동 정류에 대한 감도
를 보정 해야한다.

상당히 어려운 내용이라
가속도계에 의한 보정을 하고
학습을 추가적으로 해봐야 겠다.

Temperature

자이로 스코프는 온도의 영향에 따라 값을 보정해야 한다.

Pitch, Roll, Yaw

X축 회전 -> Pitch
Y축 회전 -> Roll
Z축 회전 -> Yaw

Z축을 기준으로 회전을 하면
위에서 봤을 때 X, Y 평면에서 자이로 값이 변한다.
그럼 이 둘 사이에 관계가 백터로 표현이 되는데
삼각함수를 통해서 그 값을 구할 수 있다.

선행 학습 )
각 -> 라디안 : π / 180
라디안 -> 각 : 180 / π

참고)
tan(라디안) -> 각도
atan(각도) -> 라디안

단위 원에서 각에 따라 값이 변하는 것은 sin 그래프로 그릴 수 있다.
0부터 시작 -> sine
1부터 시작 -> cosine
(원이기 때문에 크기의 절대값은 같음)

Z축을 기준으로 회전을 하면
Roll -> Pitch
Pitch -> Roll
로 값이 변환되는데

그 크기는
Z축의 회전 각도에 따른 Sine 그래프에 비례한다.

Angle_Roll -= Angle_Pitch * sin(gyro_z * (π / 180))
Angle_Pitch += Angle_Roll * sine(gyro_z * (π / 180))

여기서 gyro_z 는
Output Data Rate 와 FS_SEL 을 적용한
한 번 읽힐 때의 값이다.

<작성중>