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

센서 보정 루틴 구현하기

목표

-Raw, Pitch, Yaw에 대한 각도를 구하기 위한 값들을 구하기

왜 센서 보정을 해야하는가?

기본적으로 센서들(여기서는 가속도 센서, 자이로 센서)은 지면가 수평인 상태여도 무시할 수 있는 정도의 미세하게 흔들리는 값들을 보내게 된다. 그렇게 된다면 기본적인 초기 값을 지정해 주기 어려운데 이러한 이유로 인해서 가속도 센서가 현재 상태에서 가지는 기본 값의 평균값을 얻어내야 합니다.

그래서! 센서 보정 루틴을 구현을 해주여야 합니다.

이번 소스코드를 이해하기 위해서는 이전 포스팅에서 설명을 드렸던 "기본 루틴 구현하기"에서 이해를 먼저 하셔야 합니다.

본격적으로 센서 보정하는 소스코드에 대해 설명을 드리도록 하겠습니다.

소스코드

//센서 보정 루틴
#include 
 
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
 
void setup() {
  initMPU6050(); //가속도 자이로 센서 값을 읽음
  Serial.begin(115200);
  calibAccelGyro(); //센서 보정 루틴
}
 
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;
float baseAcX, baseAcY, baseAcZ;
float baseGyX, baseGyY, baseGyZ;

//센서 들의 기본값들의 평균을 내야하는 루틴(센서 보정 루틴)
void calibAccelGyro() 
{
  float sumAcX = 0, sumAcY = 0, sumAcZ = 0;
  float sumGyX = 0, sumGyY = 0, sumGyZ = 0;

  //가속도 자이로 센서를 읽어드림
  readAccelGyro();

  //읽어드렸으면 이제 읽어드린 값을 토대로 평균값을 구하면 됨
  for(int i=0; i<10; i++)
  {
    readAccelGyro();
    sumAcX += AcX; sumAcY += AcY; sumAcZ += AcZ;
    sumGyX += GyX; sumGyY += GyY; sumGyZ += GyZ;
    delay(100);//0.1초
  }
  //맨 처음 기본 센서 값들을 보여지고 그다음에 평균값을 구하는 함수
  baseAcX = sumAcX / 10;
  baseAcY = sumAcY / 10;
  baseAcZ = sumAcZ / 10;

  baseGyX = sumGyX / 10;
  baseGyY = sumGyY / 10;
  baseGyZ = sumGyZ / 10;
}

 //프로세싱 스케치로 각도 정보를 보내는 루틴
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);
}

파일 다운로드(2016.09.10)//수정본

sketch_sep10c.ino

이제 직접 추가된 부분의 소스코드에 대해 설명을 드리도록 하겠습니다. 기본 베이스는 기본 루틴에 센서 보정 루틴만 추가한 소스코드 입니다.

설명

추가를 해준 코드는 위에가 전부 다 입니다.

15번째 줄 : 센서 보정 루틴 함수 선언 

-> loop()구문에다가 코드를 전부 다 집어넣게되면 지져분하니까 따로 빼준 것 입니다.

48번째 줄 : 가속도 센서의 기본 값(평균 값)을 구하기 위한 변수 선언

49번째 줄 : 자이로 센서의 기본 값(평균 값)을 구하기 위한 변수 선언

54~55번째 줄 : 미세한 떨림이 있는 센서(자이로, 가속도)값들의 합계를 저장하기 위한 변수 선언

58번째 줄 : 기본적으로 가속도 자이로 센서 값들을 읽어드립니다.

61 ~ 67번째 줄 : 읽어드린 가속도 자이로 센서 값들의 합계를 저장하는 역할

70 ~ 77번째 줄 : 합계를 저장한 변수에 10을 나누어서 평균값(기본값)을 얻습니다.

이로서 드론이 기울어진 상태에 있던 무슨 상태에 있던 현재 상태에서 갖는 기본 값(평균값)을 얻어낼 수 있게 됩니다.

여기서 얻어지는 값들은 Roll, Yaw, pitch에 대한 각도를 구할때 사용이 되어지게 됩니다.