AnsweredAssumed Answered

L3G4200D unacceptable big values WHY?

Question asked by Emrah Duatepe on Oct 4, 2016
Latest reply on Nov 18, 2016 by Marian L@ST
#include <L3G4200D.h>
#include <lcd.c>
#include <math.h>






#define WHO_AM_I 0x0F
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
#define CTRL_REG6 0x25
#define FIFO_CTRL_REG 0x2E
#define FIFO_SRC_REG 0x2F
#define OUT_X_L 0x28
#define OUT_X_H 0x29
#define OUT_Y_L 0x2A
#define OUT_Y_H 0x2B
#define OUT_Z_L 0x2C
#define OUT_Z_H 0x2D
#define STATUS_REG 0x27
#define REFERENCE 0x25
#define INT1_CFG  0x30
#define INT1_SRC  0x31
#define INT1_THS_XH 0x32
#define INT1_THS_XL 0x33
#define INT1_THS_YH 0x34
#define INT1_THS_YL 0x35
#define INT1_THS_ZH 0x36
#define INT1_THS_ZL 0x37
#define INT1_DURATION 0x38
/*#define x_eksen 0
#define y_eksen 1
#define z_eksen 2*/


enum eksenler {
    x_eksen = 0,
    y_eksen = 1,
    z_eksen = 2,
};


signed int8 xMSB=0;
signed int8 xLSB=0;
signed int8 yMSB=0;
signed int8 yLSB=0;
signed int8 zMSB=0;
signed int8 zLSB=0;


signed int16 Rm[3]={0};
float32 Ro[3]={0};
float32 Rth[3]={0};
float32 Rt[3]={0};


void           init();  //Fonksiyon prototipleri
void           Setup_Gyro();
void           Gyroscope_Write(unsigned int8 yazilacakAdres,unsigned int8 deger);
signed int8    Gyroscope_Read(unsigned int8 okunacakAdres);
void           Gyroscope_Values_Raw();
void           Gyroscope_Values();
void           Calibrate_Gyro();


void main() //Ana Fonksiyon
{
   init();
   Setup_Gyro();
   Calibrate_Gyro();
   while(true)
   {
      Gyroscope_Values();
      delay_ms(100);
      printf(lcd_putc,"\f");
      lcd_gotoxy(1,1);
      printf(lcd_putc,"X:%Ld",Rt[x_eksen]);
      lcd_gotoxy(1,2);
      printf(lcd_putc,"Y:%Ld",Rt[y_eksen]);
      lcd_gotoxy(9,1);
      printf(lcd_putc,"Z:%Ld",Rt[z_Eksen]);
        
   }
}
//#############################################################################
void init() //Denetleyici ayarları
{
   setup_psp(PSP_DISABLED);
   setup_timer_0(RTCC_OFF);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DISABLED,0,1);
   setup_timer_3(T3_DISABLED);
   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_OFF);
   setup_CCP1(CCP_OFF);
   setup_CCP2(CCP_OFF);
   setup_comparator(NC_NC_NC_NC);
   lcd_init();
   lcd_gotoxy(1,1);
   printf(lcd_putc,"LCD confirmed");
   delay_ms(500);
   printf(lcd_putc,"\f");
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Setup_Gyro()
{
 
  Gyroscope_Write(CTRL_REG2,0b00100000); //Normal mode (reset reading HP_RESET_FILTER) ,High pass filter cut off frecuency:8
  Gyroscope_Write(CTRL_REG3,0b00001000);// I2_DRDY,I2_WTM,I2_ORun,I2_Empty etkin ,push-pull--FIFO etkin olmadığı durumda push-pull hariç diğerleri geçersiz
  Gyroscope_Write(CTRL_REG4,0b10010000);//BDU:1,500 dps,self-test disabled(normal mode)
  Gyroscope_Write(CTRL_REG5,0b00001010);//HPF disabled,FIFO disabled
  /*Gyroscope_Write(REFERENCE,0b00000000);
  Gyroscope_Write(INT1_THS_XH,0b00000000);
  Gyroscope_Write(INT1_THS_XL,0b00000000);
  Gyroscope_Write(INT1_THS_YH,0b00000000);
  Gyroscope_Write(INT1_THS_YL,0b00000000);
  Gyroscope_Write(INT1_THS_ZH,0b00000000);
  Gyroscope_Write(INT1_THS_ZL,0b00000000);
  Gyroscope_Write(INT1_DURATION,0b00000000);
  Gyroscope_Write(INT1_CFG,0b00000000);*/
  Gyroscope_Write(CTRL_REG1,0b00011111); // 100Hz,25 cut-off/ X,Y,Z etkin
  delay_ms(252);
         
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Calibrate_Gyro()
{
   float32 X[50]={0};
   float32 Y[50]={0};
   float32 Z[50]={0};
   float32 temp[3]={0};


   //50 adet çiğ değer toplanıyor ve ayrıca bu değerler hafızada tutuluyor.
   for(unsigned int8 i=0;i<50;i++)
   {
      Gyroscope_Values();
      X[i]=Rm[x_eksen];    Y[i]=Rm[y_eksen];    Z[i]=Rm[z_eksen]; 
   // her döngüde farklı değerler farklı eksen dizilerine veriler atandı.
         temp[x_eksen] = temp[x_eksen] + Rm[x_eksen];
         temp[y_eksen] = temp[y_eksen] + Rm[y_eksen];
         temp[z_eksen] = temp[z_eksen] + Rm[z_eksen]; 
         //zero-rate için veri toplanıyor
   }
         //zero-rate belirleniyor
      Ro[x_eksen] = temp[x_eksen] / 50;
      Ro[y_eksen] = temp[y_eksen] / 50;
      Ro[z_eksen] = temp[z_eksen] / 50;
   
   for(int k=0;k<50;k++)
   {//threshold için veri toplanıyor
      Rth[x_eksen] = Rth[x_eksen] +  pow((X[k] - Ro[x_eksen]),2);
      Rth[y_eksen] = Rth[y_eksen] +  pow((Y[k] - Ro[y_eksen]),2);
      Rth[z_eksen] = Rth[z_eksen] +  pow((Z[k] - Ro[z_eksen]),2);
   }
   
      Rth[x_eksen] = 3*sqrt(Rth[x_eksen] / 50);
      Rth[y_eksen] = 3*sqrt(Rth[y_eksen] / 50);
      Rth[z_eksen] = 3*sqrt(Rth[z_eksen] / 50);     
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Gyroscope_Write(unsigned int8 yazilacakAdres,unsigned int8 deger)
{
   i2c_start();
   i2c_write(0xD2);
   i2c_write(yazilacakAdres);
   i2c_write(deger);
   i2c_stop();
   delay_ms(20);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
signed int8 Gyroscope_Read(unsigned int8 okunacakAdres)
{
   signed int8 gonder=0;
   i2c_start();
   i2c_write(0xD2);
   i2c_write(okunacakAdres);
   i2c_start();
   i2c_write(0xD3); 


       gonder = i2c_read(0);
    
   i2c_stop();
   delay_ms(10);
   return(gonder);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&


void Gyroscope_Values_Raw()
{
   unsigned int8 cikis=0,hpfReset,c=0;
   cikis = Gyroscope_Read(STATUS_REG);
   delay_ms(5);


   while(bit_test(cikis,3)==0)
   {
      cikis = Gyroscope_Read(STATUS_REG);
   }
   


   //if(bit_test(cikis,7)==1)  return;
     
         xMSB = Gyroscope_Read(OUT_X_H);
         xLSB = Gyroscope_Read(OUT_X_L);
         yMSB = Gyroscope_Read(OUT_Y_H);
         yLSB = Gyroscope_Read(OUT_Y_L);
         zMSB = Gyroscope_Read(OUT_Z_H);
         zLSB = Gyroscope_Read(OUT_Z_L);
         hpfReset = Gyroscope_Read(REFERENCE);
         Rm[x_eksen] = make16(xMSB,xLSB);
         Rm[y_eksen] = make16(yMSB,yLSB);
         Rm[z_eksen] = make16(zMSB,zLSB);
         delay_ms(10);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Gyroscope_Values()
{
   Rt[x_eksen]=0;   Rt[y_eksen]=0;   Rt[z_eksen]=0;
   float32 deltaR[3]={0};
   Gyroscope_Values_Raw();
   for (int j = 0; j<3; j++)
   {
      deltaR[j] = Rm[j] - Ro[j]; 
      
      if (abs(deltaR[j]) < Rth[j])
            deltaR[j] = 0;
            
      Rt[j] = deltaR[j] * 0.0175 * 0.01;//0.01 ile açısal yer değiştirme bulunur.   
   }
}


//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
/*void Gyroscope_Values_Raw(signed int16 array[])
{
   Gyroscope_Write(FIFO_CTRL_REG,0b00101010);//wtm:10 fifo mod
   unsigned int8 cikis=0;
   cikis = Gyroscope_Read(FIFO_SRC_REG);
      while(bit_test(cikis,7)==0)
        cikis = Gyroscope_Read(FIFO_SRC_REG);
        
       while(bit_test(cikis,5)==0) //empty bit 0 iken verileri alr
       {
         xMSB = Gyroscope_Read(OUT_X_H);
         xLSB = Gyroscope_Read(OUT_X_L);
         yMSB = Gyroscope_Read(OUT_Y_H);
         yLSB = Gyroscope_Read(OUT_Y_L);
         zMSB = Gyroscope_Read(OUT_Z_H);
         zLSB = Gyroscope_Read(OUT_Z_L);
         
         array[0] = make16(xMSB,xLSB);
         array[1] = make16(yMSB,yLSB);
         array[2] = make16(zMSB,zLSB);
         
         cikis = Gyroscope_Read(FIFO_SRC_REG);// empty bitini öğrenmek için güncelleme
    
      delay_ms(5);
       }
     
    Gyroscope_Write(FIFO_CTRL_REG,0b00001010);
}*/


Hey guys ,I have a problem ,accordign to datasheet if  STATUS_REG 7. bit=0  in this case some values overwritten before I read previously ,How I can block this .Actually I dont know my problem is related with this but I should notify .Now I have some values very big that despite I do multiplication with like 0.00000001 ,I m moving with application notes.
Thanks for your helps .

                                   
multiplication
                                   
multiplication

Outcomes