Page 163 - 만들면서 배우는 아두이노 드론
P. 163
filtered_angle_z = tmp_angle_z;
}
void setup()
{
initMPU6050(); // mpu-6050 과의 통신을 초기화합니다.
Serial.begin(115200);
calibAccelGyro(); // 안정된 상태에서의 가속도, 자이로 값을 계산합니다.
initDT();
}
void loop()
{
readAccelGyro(); // 가속도 자이로 값을 센서로부터 읽어옵니다.
calcDT();
calcAccelYPR(); // 가속도 센서를 이용하여 Roll, Pitch, Yaw 각도를 계산합
니다
calcGyroYPR(); // 자이로 센서를 이용하여 Roll, Pitch, Yaw 각도를 계산합
니다.
calcFilteredYPR(); // 두 센서 값에 상보 필터를 적용하여 Roll, Pitch, Yaw
각도를 구합니다.
Serial.print("FX : ");
Serial.print(filtered_angle_x);
Serial.print(" ");
Serial.print("FY : ");
Serial.print(filtered_angle_y);
Serial.print(" ");
Serial.print("FZ : ");
Serial.println(filtered_angle_z);
}
[소스 코드 설명]
? initMPU6050() : MPU-6050 센서를 활성화합니다.
? calibAccelGyro() : 가속도 자이로 센서의 초기 평균값을 구합니다.
? initDT() : 시간 간격 초기화를 합니다.
? calcDT() : 시간 간격을 계산합니다.
? readAccelGyro() : 가속도 자이로 센서 값을 읽습니다.
162
