CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug reports on this forum. Send them to support@ccsinfo.com

L3G4200D Quaternions to Euler's Angle

 
Post new topic   Reply to topic    CCS Forum Index -> General CCS C Discussion
View previous topic :: View next topic  
Author Message
Zek_De



Joined: 13 Aug 2016
Posts: 100

View user's profile Send private message

L3G4200D Quaternions to Euler's Angle
PostPosted: Mon Oct 10, 2016 5:46 am     Reply with quote

Hey Guys,
I need some help about this problem ,is it true ? quaternion parameters : a,b,c,d is initial values are

a = cos(alfa/2) = start value alfa =0 this is why a=1
b= sin(alfa/2)cos(betax); b=0
c= sin... c=0
d=sin.. d=0 because of sin

if I solve this differantial equation with this numeratically method
http://math.stackexchange.com/questions/189185/quaternion-differentiation

is it gonna be right with this start values.because I m getting 0 ,maybe my program is wrong.

Other problem , how can I get sampling time in this differantial you know ,
I have to use to solve(next value - current value)/deltaT ,how to get this sampling time in CCS C, is it set_tick(); ?
if I sample my values with ODR=100,should I use as sampling time 0.01 ,but somthing wrong if I do this not sense,yes L3G4200D use 0.01 but I get values according to program flow in pic18F...
For example ,
void main()
{
Get_Values(); // if ODR=100 ,in this case deltaT=0.01 (in device)
some process; //pass some time
some process2;// pass some time
solve_differantial(); // then What should I use as deltaT current time now to use in differantial.
}
Last question SC factor ,what is this exactly,and how I found, SC is 0.00875 if 500 Dps ,but in L3G420D everything 3-axes digital Mems gyroscope.pdf refer us to get SC as user .

Please ,help me guys.
temtronic



Joined: 01 Jul 2010
Posts: 9169
Location: Greensville,Ontario

View user's profile Send private message

PostPosted: Mon Oct 10, 2016 7:29 am     Reply with quote

This could probably be easily resolved by letting Google locate what you ned to know. While you may not find CCs C code in the 125,000 hits on 'Quaternions to Euler's Angle', there is more than enough info to convert 'other' code into working CCS C.
Whatever code you do come up with be sure to test against KNOWN standards !

Jay
Zek_De



Joined: 13 Aug 2016
Posts: 100

View user's profile Send private message

PostPosted: Mon Oct 10, 2016 7:54 am     Reply with quote

By the way these are my code. temtronic dude, I already googled, also my english little bad sorry, for this whatever. In all of them the most important one is deltaT matter. I couldn't find it how I get. My quaternion to angle conversion I'm thinking is wrong, because I'm reading y_angle =0.00 or I couldn't think of a good algorithm because I learned this issue just yesterday. I'm gonna work on it but help is going to be good to shorten the time.
Code:

#include <L3G4200D.h>
#include <stdlib.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 OUT_TEMP 0x26
#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 deltaR[3]={0};
signed int16 temperature = 0;
float32 angle[3]={0};
unsigned int16 timer_0=0;
float32 q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0;
float32 qDot1=0.0, qDot2=0.0, qDot3=0.0, qDot4=0.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();
unsigned int8  Get_Temperature();
void           Interrupt_To_Temperature_and_Calibrating();

void main() //Ana Fonksiyon
{
   init();
   Setup_Gyro();
   Calibrate_Gyro();
   while(true)
   {
      Gyroscope_Values();
      printf(lcd_putc,"\f");
     // lcd_gotoxy(1,1);
     // printf(lcd_putc,"X:%f",angle[x_eksen]);
      lcd_gotoxy(1,2);
      printf(lcd_putc,"Y:%f",angle[y_eksen]);
      lcd_gotoxy(9,1);
     // printf(lcd_putc,"Z:%f",angle[z_eksen]);
      lcd_gotoxy(9,2);
      printf(lcd_putc,"Z:%Ld",temperature);
      delay_ms(5);
     
      if(timer_0 >= 1526) //20 s'de bir if içine girer.
      {
         printf(lcd_putc,"\f");
         lcd_gotoxy(1,1);
         printf(lcd_putc,"Stop!");
         delay_ms(1000);
         
         Calibrate_Gyro();
         temperature = Get_Temperature();
         timer_0=0;
         angle[x_eksen]=0;
         angle[y_eksen]=0;
         angle[z_eksen]=0;
         
         printf(lcd_putc,"\f");
         lcd_gotoxy(1,1);
         printf(lcd_putc,"Done...");
         delay_ms(500);
      }           
   }
}
//#############################################################################
void init() //Denetleyici ayarları
{
   setup_psp(PSP_DISABLED);
   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);
   //Timer0 kuruluyor
   setup_timer_0(RTCC_INTERNAL | RTCC_DIV_256 |RTCC_8_BIT);
   set_timer0(0);
   enable_interrupts(INT_timer0);
   enable_interrupts(GLOBAL);
   //Timer0 kuruldu.
   lcd_init();
   delay_ms(20);
   lcd_gotoxy(1,1);
   printf(lcd_putc,"Don't Move!");
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Setup_Gyro()
{
 
  Gyroscope_Write(CTRL_REG2,0b00100100);//Normal mode ,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,0b10110000);//BDU:1,2000 dps,self-test disabled(normal mode)
  Gyroscope_Write(CTRL_REG5,0b00000000);//HPF enabled - Data in DataReg and FIFO are high-passfiltered - Data in DataReg and FIFO are non-highpass- filtered
  /*
  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,0b11001111); // 800Hz,30 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.(standart sapma formülünde her biri tek tek kullanılacak.)
   for(unsigned int8 i=0;i<50;i++)
   {
      Gyroscope_Values_Raw();
      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(Moving Avarage Filter)
      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 Calibrate_Gyro() //ALLAN VARIANCE Calibrating
{
   float32 X[50]={0};
   float32 Y[50]={0};
   float32 Z[50]={0};
   float32 temp[3]={0};
   unsigned int8 i=0;
  for(i=0;i<50;i++)
   {
      Gyroscope_Values_Raw();
      X[i]=Rm[x_eksen];    Y[i]=Rm[y_eksen];    Z[i]=Rm[z_eksen];
     
      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];
   }
      Ro[x_eksen] = temp[x_eksen] / 50;  //Zero-Rate bulunuyor.
      Ro[y_eksen] = temp[y_eksen] / 50;
      Ro[z_eksen] = temp[z_eksen] / 50;
  for(i=0;i<47;i++)
  {
    temp[x_eksen] = (X[i] - 2*X[i] + X[i]) * (X[i] - 2*X[i] + X[i]);
    temp[y_eksen] = (Y[i] - 2*Y[i] + Y[i]) * (Y[i] - 2*Y[i] + Y[i]);
    temp[z_eksen] = (Z[i] - 2*Z[i] + Z[i]) * (Z[i] - 2*Z[i] + Z[i]);
  }
     
      Rth[x_eksen] = 3*(1/(2*0.01*48)) * temp[x_eksen];
      Rth[y_eksen] = 3*(1/(2*0.01*48)) * temp[y_eksen];
      Rth[z_eksen] = 3*(1/(2*0.01*48)) * temp[z_eksen];
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
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();
   return(gonder);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

void Gyroscope_Values_Raw()
{
   unsigned int8 cikis=0;
   //unsigned int8  hpfReset;
   cikis = Gyroscope_Read(STATUS_REG);


   while(bit_test(cikis,3)==0)
   {
      cikis = Gyroscope_Read(STATUS_REG);
   }
   
         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(20);
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
void Gyroscope_Values()
{
   Gyroscope_Values_Raw();
    for (int j = 0; j<3; j++)
   {
      deltaR[j] = (float32)Rm[j] - Ro[j];
     
      if (abs(deltaR[j]) > Rth[j])
      {
             
            //angle[j] = angle[j] + deltaR[j] * 0.07;
            deltaR[j] = deltaR[j] * 0.07;
         
      }
     
   }
   qDot1 = q0 + 0.5 * (-q1 * deltaR[x_eksen] - q2 * deltaR[y_eksen] - q3 * deltaR[z_eksen])* (1 / 800);
   qDot2 = q1 + 0.5 * (q0 * deltaR[x_eksen] + q2 * deltaR[z_eksen] - q3 * deltaR[y_eksen])* (1 / 800);
   qDot3 = q2 + 0.5 * (q0 * deltaR[y_eksen] - q1 * deltaR[z_eksen] + q3 * deltaR[x_eksen])* (1 / 800);
   qDot4 = q3 + 0.5 * (q0 * deltaR[z_eksen] + q1 * deltaR[y_eksen] - q2 * deltaR[x_eksen])* (1 / 800);
   
   angle[y_eksen] = (180 * asin(-(2*qDot2*qDot4-qDot1*qDot3))) / 3.14;
   
}
unsigned int8 Get_Temperature()
{
   unsigned int8 t;
   t = (25 - Gyroscope_Read(OUT_TEMP)) + 25;
   return t;
}

#INT_TIMER0
void Interrupt_To_Temperature_and_Calibrating()
{
      set_timer0(0);
      timer_0++;   
}
Ttelmah



Joined: 11 Mar 2010
Posts: 19358

View user's profile Send private message

PostPosted: Mon Oct 10, 2016 8:22 am     Reply with quote

Start with timer0.

You are clearing this in the interrupt. Don't. The interrupt is called when timer0 wraps to zero. You are clearing it a few instructions later than this happens which will degrade your accuracy...

Now you don't show timer0 being used for anything else. Why then have it at all?. Get rid of it. We'll use it instead to give a time.

Now, you don't tell us what PIC you are using, or what clock rate it is running?.
The easiest thing is probably to run a timer, as a 'tick'. So given you were using timer0, add:

#USE TIMER (TIMER=0, TICK=1uS, BITS=32, ISR)
Just after your clock declaration in the code.

Then just have enable_interrupts(GLOBAL); don't have enable_interrupts(INT_TIMER0); the compiler will do the rest to create an ISR to give a 32bit timer.

Then when you finish reading the angles, use 'get_ticks()'. This will return a 32bit 'time' in uSec. The delta is simply the count between this and the last one you read. Scale your maths to work in uSec, rather than in seconds.
Zek_De



Joined: 13 Aug 2016
Posts: 100

View user's profile Send private message

PostPosted: Mon Oct 10, 2016 8:40 am     Reply with quote

Sorry dude these are my settings. I will read better your answer. I have to go time to Uni now.
Code:

#include <18F4550.h>
//#device high_ints=true
#device ADC=16


#FUSES WDT128        //Watch Dog Timer uses 1:128 Postscale
#fuses HS,NOWDT,NOBROWNOUT,NOLVP,NOXINST,NODEBUG,NOCPD,NOPUT,NOPROTECT
#use delay(crystal=20MHz)

#use I2C(master,scl=PIN_B1 ,sda=PIN_B0, fast=400000)

#define LCD_ENABLE_PIN PIN_D3
#define LCD_RS_PIN PIN_D2
#define LCD_RW_PIN PIN_D1
#define LCD_DATA4 PIN_D4
#define LCD_DATA5 PIN_D5
#define LCD_DATA6 PIN_D6
#define LCD_DATA7 PIN_D7
Zek_De



Joined: 13 Aug 2016
Posts: 100

View user's profile Send private message

PostPosted: Mon Oct 10, 2016 9:43 am     Reply with quote

(Ttelmah) Dude, I don't understand excatly what you mean these sentences,
Quote:

You are clearing this in the interrupt. Don't. The interrupt is called when timer0 wraps to zero. You are clearing it a few instructions later than this happens which will degrade your accuracy...

Now you don't show timer0 being used for anything else. Why then have it at all?. Get rid of it. We'll use it instead to give a time.


I would call " Interrupt_To_Temperature_and_Calibrating(); " for every 20 second.
Second one, what is wraps to zero and ISR ?
Other one, using get_ticks();--> I'll read angles --> using get_ticks();--> and second -first =deltaT , is it true? and thank you so much for the help.
temtronic



Joined: 01 Jul 2010
Posts: 9169
Location: Greensville,Ontario

View user's profile Send private message

PostPosted: Mon Oct 10, 2016 10:23 am     Reply with quote

this....
Code:

#INT_TIMER0
void Interrupt_To_Temperature_and_Calibrating()
{
set_timer0(0);
timer_0++;
}

is the timer0 ISR.

this...
Code:
set_timer0(0);

clears (makes it zero).

and it what Mr T. says is not needed.

Jay
Ttelmah



Joined: 11 Mar 2010
Posts: 19358

View user's profile Send private message

PostPosted: Mon Oct 10, 2016 11:51 am     Reply with quote

If you have already got a timer tick, then use it. This is your time. Just get rid of clearing the timer. The interrupt is triggered by the timer going to zero. Doing it again (later, since it takes time to get into an interrupt), just reduces the accuracy.

However as a general comment, it is much more efficient in a microprocessor to use atan2, rather than sin & cos.
Look at the equations given here:
<http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/>

Do a search here. I have posted a more efficient atan2 routine.

Update:
Just went and located it:
<http://www.ccsinfo.com/forum/viewtopic.php?t=50920&highlight=atan>
Zek_De



Joined: 13 Aug 2016
Posts: 100

View user's profile Send private message

PostPosted: Tue Oct 11, 2016 4:11 am     Reply with quote

Now I'm using this setting:
Code:
#USE TIMER(TIMER=1,TICK=1ns,BITS=16,ISR)

I was thinking 1 tick is 1 ns but when I compile the code, the compiler reports "Timer 1 tick time is 400 ns". Is it ok to get deltaT .I2C 400kHz, 20 Mhz Crystal ?
Ttelmah



Joined: 11 Mar 2010
Posts: 19358

View user's profile Send private message

PostPosted: Tue Oct 11, 2016 4:23 am     Reply with quote

Your hardware can't do 100nSec. That's why I suggested 1uSec.

You can use any value you want. Just scale the final maths result to suit. Your original 32KHz would also be fine.
curt2go



Joined: 21 Nov 2003
Posts: 200

View user's profile Send private message

PostPosted: Wed Oct 12, 2016 1:18 pm     Reply with quote

Why do you want to go to euler angles? From what I have been reading its best to stay with quaternions. You just multiply they to find the difference.

Here is a really good simulator you can put your numbers in.
http://quaternions.online/

Also here is a good article with c programming.
http://www.cprogramming.com/tutorial/3d/quaternions.html

I am still really struggling with getting my z axis right. My Yaw is not working out. If you have any suggestions I started a post above yours.
curt2go



Joined: 21 Nov 2003
Posts: 200

View user's profile Send private message

PostPosted: Wed Oct 12, 2016 9:06 pm     Reply with quote

I found this on some of my many calculataions. I have no idea if it works but you can try it.
Code:

void  quaternionToEuler(){
    double sqw = newQw*newQw;
    double sqx = newQx*newQx;
    double sqy = newQy*newQy;
    double sqz = newQz*newQz;
   double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
   double test = newQx*newQy + newQz*newQw;
   if (test > 0.499*unit) { // singularity at north pole
      newHeading = 2 * atan2(newQx,newQw);
      newAttitude = PI/2;
      newBank = 0;
      return;
   }
   if (test < -0.499*unit) { // singularity at south pole
      newHeading = -2 * atan2(newQx,newQw);
      newAttitude = PI/2;
      newBank = 0;
      return;
   }
    newHeading = atan2(2*newQy*newQw-2*newQx*newQz , sqx - sqy - sqz + sqw);
   newAttitude = asin(2*test/unit);
   newBank = atan2(2*newQx*newQw-2*newQy*newQz , -sqx + sqy - sqz + sqw);
/*
 double sqw = q1.w*q1.w;
    double sqx = q1.x*q1.x;
    double sqy = q1.y*q1.y;
    double sqz = q1.z*q1.z;
   double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
   double test = q1.x*q1.y + q1.z*q1.w;
   if (test > 0.499*unit) { // singularity at north pole
      heading = 2 * atan2(q1.x,q1.w);
      attitude = Math.PI/2;
      bank = 0;
      return;
   }
   if (test < -0.499*unit) { // singularity at south pole
      heading = -2 * atan2(q1.x,q1.w);
      attitude = -Math.PI/2;
      bank = 0;
      return;
   }
    heading = atan2(2*q1.y*q1.w-2*q1.x*q1.z , sqx - sqy - sqz + sqw);
   attitude = asin(2*test/unit);
   bank = atan2(2*q1.x*q1.w-2*q1.y*q1.z , -sqx + sqy - sqz + sqw)
*/

}
Ttelmah



Joined: 11 Mar 2010
Posts: 19358

View user's profile Send private message

PostPosted: Thu Oct 13, 2016 1:23 am     Reply with quote

As a comment on this, if you check online, you will find people saying that on these chips you must either use the interrupt pin from the gyro to say that data is ready, or poll the status register and check status register bit 3, to verify that new data _is_ available before reading it, and also bit 7, to ensure you have not missed data. Otherwise the readings you get are effectively 'pointless'.....
You need to be synchronising to what the sensor is doing, not just reading it and expecting there to be new data.
Zek_De



Joined: 13 Aug 2016
Posts: 100

View user's profile Send private message

PostPosted: Thu Oct 13, 2016 2:34 am     Reply with quote

curt2go, I'm sorry I couldn't understand your codes exactly. Also I read your website and purpose is to block gimble lock. Yes, also I wanted to try my code ability solving this quaternion, but for now I failed. I will try again later whatever. All I know how to convert is in there. Go to
http://www.st.com/content/st_com/en/products/mems-and-sensors/gyroscopes/l3g4200d.html
and find Everything about STMicroelectronics 3-axis digital MEMS gyroscopes in it --> go to 6.2 Gyroscope-enabled navigation.
You will see some information I don't know it helps you and also these
http://stackoverflow.com/questions/23503151/how-to-update-quaternion-based-on-3d-gyro-data

http://math.stackexchange.com/questions/189185/quaternion-differentiation

Also what is 0.499
Zek_De



Joined: 13 Aug 2016
Posts: 100

View user's profile Send private message

PostPosted: Thu Oct 13, 2016 2:36 am     Reply with quote

İf you solve this please send it. I don't know what I will give instead of deltaT.
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> General CCS C Discussion All times are GMT - 6 Hours
Page 1 of 1

 
Jump to:  
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum


Powered by phpBB © 2001, 2005 phpBB Group