[드론]시간 처리 루틴을 구현하기(회전각 구하기)

시간 간격 처리 루틴

(회전각)

드론이 회전을 할 경우에는 자이로 센서가 역할을 하게 됩니다. 이때 자이로 센서를 이용해서 회전각을 구하게 되는데 

이 과정에서 시간 간격에 대한 정보가 필요하다고 합니다.

그래서!! 시간 처리를 해주는 루틴을 구현을 해야 합니다.

이때 주의해야할 실제로 센서가 센서값을 읽는데 걸리는 시간은 2ms 전후가 됩니다. 또한 센서를 이용하여 드론을 날릴 경우 드론의 중심을 안정적으로 잡기 위해서는 3ms 전후가 되어야합니다.

(값이 큰 경우[10ms]인 경우가 되면 드론의 모터 회전 반응이 늦어져서 중심을 잡기 힘듭니다.)

즉, 시간 간격을 2ms 전후로 설정을 해주어서  드론이 비행할때 중심을 안정적으로 잡게 해주어야 합니다. 그래서 시간 간격 처리 루틴을 작성해야하는 이유입니다.

• 기본 루틴 구현하기

• 센서 보정 루틴 구현하기

소스코드

//단위 시간 처리 루틴
#include 
  
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
  
void setup() {
  initMPU6050(); //가속도 자이로 센서 값을 읽음
  Serial.begin(115200);
  calibAccelGyro(); //센서 보정 루틴
  initDT();// 시간 간격에 대한 초기화
}
  
void loop() {
  readAccelGyro();
  calcDT(); //시간 간격 계산

  static int cnt;
  cnt++;
  if(cnt%2==0)
    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);
}

unsigned long t_now;
unsigned long t_prev;

void initDT(){
  t_prev = millis();
}

void calcDT(){
  t_now = millis();
  dt = (t_now - t_prev) / 1000.0;
  t_prev = t_now;
}

다운로드 파일(2016.09.10)

sketch_sep10c.ino

코드 설명

20번째 줄을 보시게 되면 cnt의 값이 2의 배수일떄만 SendDataToProcessing 함수를 호출하고 있는걸 보실 수 있습니다. 이렇게 한 이유는 매 홀수번째에는 데이터를 안보내고 2의배수 즉 짝수번째에만 데이터를 보내기 때문에 정확한 시간값을 알 수 있게 되고 시간값(dt)가 2ms 전후로 출력되는걸 볼 수 있습니다.

unsigned 형인 long 형으로 t_now와 t_prev 변수를 선언해주었습니다.

소스코드에서 보시면 initDT()는 setup()함수에서 호출을 하고 calcDT() 함수는 loop() 함수에서 호출을 하는걸 확인할 수 있습니다.

initDT() 함수

t_prev변수에 현재 시간값을 저장하고 있습니다. setup()함수에서 호출이 되고있고요

calcDT() 함수

t_now 변수에 현재 시간값을 저장하고 있습니다. 이는 초기에는 t_now 변수값과 거의 비슷합니다.

그리고 dt변수에다가 매번 loop를 돌며 저장된 현재시간값에다가 이전 시간값을 빼줌니다. 그리고 1000을 나누고 있는데 그 이유는 millis()함수를 통해 얻은 값은 밀리초 단위의 현재 시간이므로 1000.0을 나누어 주어야 합니다.

결론은!! 시리얼 모니터창을 확인해 보면 dt값이 2ms 전후로 출력이 되는걸 확인하실 수 있습니다. 따라서 드론이 비행할때 중심을 안정적으로 잡을 수 있게 됩니다.

이제 우리는 상보필터를 적용한 가속도 및 자이로 센서를 프로세싱으로 보낼 준비를 다 했습니다. 이 다음부터는 직접 프로세싱으로 확인을 해보도록 하겠습니다.