뉴티씨



  • HOME
  • 고객지원
  • 질문답변


 
자이로 센서 데이터 받는 법에 대해서..
 글쓴이 : 현동규
작성일 : 17-04-20 21:45
조회 : 208  
안녕하세요.. 모 대학교 전자공학과 학생입니다..

고객지원-자주하는 질문에 올라온 자이로센서 데이터 받는 소스는 제가 이해하기가 좀 어려워서
임의로 간단하게 데이터를 받아서 사용중인데요. 데이터 신뢰도가 너무 낮아서 좀 쉽게 설명해주실 수 있나
해서 질문 올려요.. 가속도는 그럭저럭 데이터가 괜찮아요.. 상보필터를 이용해서
보정을 하려고하는데 일반적으로 자이로데이터0.9 가속도데이터0.1 정도를 사용한다는데
저는 가속도를 0.8을 사용하고 자이로를 0.2만 썼습니다.. 없는거나 마찬가지인데요..
자이로 센서 데이터받을 때 신뢰도를 높이려면 어떻게 해야되는지 설명해주실 수 있나요?..

아래 소스에서 자이로 가속도 둘다 평균내서 쓰고있는데
제가 아는 한에서는 상보필터에서 자이로는 평균내지않고 순간변화량을 신뢰하고 가속도는 평균값을 신뢰한다고하는데 자이로를 평균내주지 않으면 그나마도 데이터를 볼 수 없을 정도로 드리프트가 심해요..


#include <io.h>
#include <delay.h>
#include <alcd.h>
#include <stdio.h>
#include <math.h>

#define FIRST_ADC_INPUT 0
#define LAST_ADC_INPUT 1
unsigned int adc_data[LAST_ADC_INPUT-FIRST_ADC_INPUT+1];
#define ADC_VREF_TYPE ((0<<REFS1) | (0<<REFS0) | (0<<ADLAR))

void cw_step(int a);
void ccw_step(int a);

interrupt [ADC_INT] void adc_isr(void)
{
static unsigned char input_index=0;
adc_data[input_index]=ADCW;
if (++input_index > (LAST_ADC_INPUT-FIRST_ADC_INPUT))
  input_index=0;
ADMUX=(FIRST_ADC_INPUT | ADC_VREF_TYPE)+input_index;
delay_us(10);
ADCSRA|=(1<<ADSC);
}

void main(void)
{
char text[16];
char text2[16];

int gyro_volt = 0;
int gyro_volt2_sum = 0;
int gyro_volt2_average = 0;
int gyro_degree = 0;

int acc_volt = 0;
int acc_volt_sum = 0;
int acc_volt_average = 0;
int acc_degree = 0;

int before_angle = 0;
int after_angle = 0;

int i = 0;

PORTA = 0x00;
DDRA = 0xFF;

ASSR = 0x00;
TCCR0 = 0x06; //256분주
TCNT0 = 0x83;
OCR0 = 0x00;

ADMUX=FIRST_ADC_INPUT | ADC_VREF_TYPE;
ADCSRA=(1<<ADEN) | (1<<ADSC) | (0<<ADFR) | (0<<ADIF) | (1<<ADIE) | (1<<ADPS2) | (0<<ADPS1) | (0<<ADPS0);
SFIOR=(0<<ACME);

lcd_init(16);

#asm("sei")

while (1)
      {
      lcd_clear();
      lcd_gotoxy(0,0);
     
      if(gyro_volt == 0)
      gyro_volt = adc_data[0];
      if(acc_volt == 0) 
      acc_volt = adc_data[1];
     
      for(i=0;i<10;i++){     
      gyro_volt2_sum += adc_data[0];
      delay_us(1);
      }
     
      for(i=0;i<10;i++){     
      acc_volt_sum += adc_data[1];
      delay_us(1);
      }
     
      gyro_volt2_average = gyro_volt2_sum/10;
      acc_volt_average = acc_volt_sum/10;

      gyro_degree += (int)((gyro_volt - gyro_volt2_average)*9.7533136*0.1);
     
      acc_degree = (int)(asin(((acc_volt_average-acc_volt)*0.0048828125/0.8)) * 180/PI * 1.6);
     
      before_angle = (int)(0.20*(before_angle + (gyro_volt - gyro_volt2_average)*9.7533136*0.1)+0.80*(asin((acc_volt_average-acc_volt)*0.0048828125/0.8)) * 180/PI);
     
      sprintf(text,"%i %i %i",gyro_volt,adc_data[0],gyro_degree); 
     
      sprintf(text2,"%i %i %i",acc_volt, acc_degree, before_angle);
     
      gyro_volt2_sum = 0;
      acc_volt_sum = 0;
     
      lcd_puts(text);
      lcd_gotoxy(0,1);
      lcd_puts(text2);
     
      if(after_angle-before_angle>0){//int형이므로 1도 차이마다 모터제어 
      cw_step(after_angle-before_angle);
      }
      else if(after_angle-before_angle<0){
      ccw_step(before_angle-after_angle);
      }   
     
      after_angle = before_angle;
         
      delay_ms(10);     
      }
}

void cw_step(int a)
{
    int b;
    for(b=0;b<a;b++){
      PORTA.0 = 0x01;
      PORTA.1 = 0x00;
      PORTA.2 = 0x01;
      PORTA.3 = 0x00;
      delay_ms(1);

      PORTA.0 = 0x01;
      PORTA.1 = 0x00;
      PORTA.2 = 0x00;
      PORTA.3 = 0x01;
      delay_ms(1);

      PORTA.0 = 0x00;
      PORTA.1 = 0x01;
      PORTA.2 = 0x00;
      PORTA.3 = 0x01;
      delay_ms(1);

      PORTA.0 = 0x00;
      PORTA.1 = 0x01;
      PORTA.2 = 0x01;
      PORTA.3 = 0x00;
      delay_ms(1); 
      }
}

void ccw_step(int a)
{
    int b;
    for(b=0;b<a;b++){
      PORTA.0 = 0x00;
      PORTA.1 = 0x01;
      PORTA.2 = 0x01;
      PORTA.3 = 0x00;
      delay_ms(1);

      PORTA.0 = 0x00;
      PORTA.1 = 0x01;
      PORTA.2 = 0x00;
      PORTA.3 = 0x01;
      delay_ms(1);

      PORTA.0 = 0x01;
      PORTA.1 = 0x00;
      PORTA.2 = 0x00;
      PORTA.3 = 0x01;
      delay_ms(1);

      PORTA.0 = 0x01;
      PORTA.1 = 0x00;
      PORTA.2 = 0x01;
      PORTA.3 = 0x00;
      delay_ms(1);
      }
}