 |
 |
| View previous topic :: View next topic |
| Author |
Message |
Zek_De
Joined: 13 Aug 2016
Posts: 100
|
| How to get gyro Scale Factor |
 Posted: Wed Oct 12, 2016 3:40 am |
 |
|
| I got some writtings about scale factor but they are not enough. While I rotate the device at the same time I must save data until 90 degree. Then what I will do with these data ? Maybe to do scale factor I've to do division with something after the average of data ? Thanks for help. |
|
 |
temtronic
Joined: 01 Jul 2010
Posts: 9246
Location: Greensville,Ontario
|
|
| Posted: Wed Oct 12, 2016 5:12 am |
|
|
Without knowing which PIC and compiler version you have, I can only give you general comments.
1) be sure to test hardware for hours, to confirm the data from it is stable and that EMI of any kind is not present. Until this is done NO amount of code will help, the hardware must work 100% in the real World, simulations are useless.
2) hopefully the PIC you've chosen has sufficient RAM to store the data from a 90* sweep.You don't say what 'gyroscope' you have but the analog ones are nice, encodered ones are too and integrated ones are as well. What type dictates how you code your program.
3) use INTEGER math NOT floating point. It canl be 100sX faster,save program space and is easy to 'scale', if required. Simple to make the FP numbers for humans AFTER all the math is done.
4) re: hardware...be sure proper logic level translation is done between PIC and Gyro. You cannot directly use a 3V Gyro on a 5V PIC. Check the datasheets of both devices before 'magic smoke' comes pouring out !
Jay |
|
|
Zek_De
Joined: 13 Aug 2016
Posts: 100
|
|
| Posted: Wed Oct 12, 2016 5:30 am |
|
|
Thank you Jay I solved it, but is it good solution I don't know. I solved like that
(pic18f4550 -l3g4200d) device say in datasheet if you use 2000 dps I have to 0.07 dps/digit or for another one is different. 250 dps, 500 dps whatever. As a result all values are equal. This is why I rotated the device about 90* and I got last data and this equivalent give solution.
For example on X axis my last output data with X*0.07 generate about 4150 in 90* and I use 4150 = 0.07*deltaR that deltaR = raw data - zerorate and I find deltaR = 59286. I didn't get directly this value due to some code whatever. As a result "90degree = 59286*0.07*SC", SC =0.02168, it works no problem, what do you say for this ? |
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19552
|
|
| Posted: Wed Oct 12, 2016 8:03 am |
|
|
For the figures you quote, just use *9.0.
However you need to understand the chip. It is a _rate_ sensor, not an angle sensor. It tells you how fast it is turning. I'd suspect your code is not actually able to keep up with the read rate of the sensor, so gives you the same result whatever rate is selected. |
|
|
curt2go
Joined: 21 Nov 2003
Posts: 200
|
|
| Posted: Wed Oct 12, 2016 12:27 pm |
|
|
Depends on the chip but usually they use a signed 16 bit register. So you take your full scale and divide by 32767.
eg. 500dps scaler = 500/32767 =0.0152.
Hope that helps. |
|
|
Zek_De
Joined: 13 Aug 2016
Posts: 100
|
|
| Posted: Wed Oct 12, 2016 1:30 pm |
|
|
thanks curt2go  , Ttelmah you are right ,I couldnt notice to measure actually about angular rate,but this device also can give degree but How can I get angular rate because this formula contain t(time).teta=w*t ,what will I write instead of t ,device give me teta now,also then I will use to measure angle is accelerometer ,won't it?Please give light here |
|
|
Zek_De
Joined: 13 Aug 2016
Posts: 100
|
|
| Posted: Wed Oct 12, 2016 1:32 pm |
|
|
Also code below
| 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;
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(9,1);
printf(lcd_putc,"Y:%f",angle[y_eksen]);
lcd_gotoxy(1,2);
printf(lcd_putc,"Z:%f",angle[z_eksen]);
lcd_gotoxy(9,2);
printf(lcd_putc,"C:%Ld",temperature);
delay_ms(5);
if(timer_0 >= 1526) //20 s
{
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 disabled - Data in DataReg and FIFO are non-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();
delay_us(125);
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*0.021686;
}
}
}
unsigned int8 Get_Temperature()
{
unsigned int8 t;
t = (25 - Gyroscope_Read(OUT_TEMP)) + 25;
return t;
}
#INT_TIMER0
void Interrupt_To_Temperature_and_Calibrating()
{
timer_0++;
}
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
/*void Gyroscope_Values_Raw(signed int16 array[]) FIFO ile
{
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);
}*/ |
|
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19552
|
|
| Posted: Wed Oct 12, 2016 2:16 pm |
|
|
You don't tell us your CPU clock rate or show the setup for the I2C. However I'd guess there is no way the processor is actually going to be able to keep up with the processing/sample rate.
I've pointed you to a more efficient atan. You honestly want to be using SPI, and doing everything you can to reduce the time taken for the maths. Use integer where possible. Consider perhaps an integer look-up table for the conversions. You need to be actually doing some thinking about what the maths has to contain, not just throwing functions at the problem. Little things can make a huge difference. For instance your average using 50 samples. Think again. Use 32 or 64. Both of these the processor can solve using a simple rotation, while a float /50 takes over 1600 instructions..... |
|
|
curt2go
Joined: 21 Nov 2003
Posts: 200
|
|
| Posted: Wed Oct 12, 2016 2:41 pm |
|
|
| I am using the complimentary filter that takes raw data without any averaging. It seems to work good. Ttelmah is right about the SPI version. i am just trying to get things to work with I2C but them will be moving to SPI to significantly speed things up and I am running at 140MHz. |
|
|
Zek_De
Joined: 13 Aug 2016
Posts: 100
|
|
| Posted: Wed Oct 12, 2016 4:07 pm |
|
|
First, Ttelmah, you said to me there is no way to get sample rate. I suppose you are right because I tried and thought on it and I didn't find anything. But you guys said to me to use SPI. Yes it is so fast and needs a clock. I know but How can I get sample time with it ? Is there a way?Also you mention about atan but I don't need it because angle[j] already gives in degree with 0.021686, and why 32 or 64 samples ? I couldn't understand sorry.
Second, curt2go, I didn't still filter my raw values. I will later, these ones are just for calibrating because the datasheet informs me the threshold is 4*sigma. Zero-rate is average and complimentary filter is better then kalman or near as far as I see. I researched it a little and it seems simple and usefull. I liked it. Most important one, how can I get sampling time in spi ? Maybe there is a way, I dont know. Maybe I'm asking a bad question. If it is bad I'm sorry, this is my inexperience. |
|
|
Zek_De
Joined: 13 Aug 2016
Posts: 100
|
|
| Posted: Wed Oct 12, 2016 4:08 pm |
|
|
Also,
| Code: |
#include <18F4550.h>
#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) |
|
|
|
Zek_De
Joined: 13 Aug 2016
Posts: 100
|
|
| Posted: Wed Oct 12, 2016 4:23 pm |
|
|
For angular rate, I set timerx and while device overcome threshold the timerx will work and when interrupt come true, I will subtract my start data-current (start data can have a value) and teta/(start data-current data) = w
is it ok? |
|
|
curt2go
Joined: 21 Nov 2003
Posts: 200
|
|
| Posted: Wed Oct 12, 2016 4:40 pm |
|
|
I'm not sure if your is in DegreesPerSecond but it should be ...
gyroReading += newGyroReading / (Your sample time here);
This will give you the angle. Its that simple. But the gyro will drift over time that is why we average with a little part of the Accelerometer data. The complimentary filter. |
|
|
curt2go
Joined: 21 Nov 2003
Posts: 200
|
|
| Posted: Wed Oct 12, 2016 5:53 pm |
|
|
I am a bit wrong. You do need to keep the last gyro data as well.
w = (oldData + newData)/ sample time;
oldData = newData;
Sorry for the confusion. You were more right than me.
Here's a good site.
http://www.hobbytronics.co.uk/accelerometer-gyro |
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19552
|
|
| Posted: Thu Oct 13, 2016 1:43 am |
|
|
| Zek_De wrote: | First, Ttelmah, you said to me there is no way to get sample rate. I suppose you are right because I tried and thought on it and I didn't find anything. But you guys said to me to use SPI. Yes it is so fast and needs a clock. I know but How can I get sample time with it ? Is there a way?Also you mention about atan but I don't need it because angle[j] already gives in degree with 0.021686, and why 32 or 64 samples ? I couldn't understand sorry.
Second, curt2go, I didn't still filter my raw values. I will later, these ones are just for calibrating because the datasheet informs me the threshold is 4*sigma. Zero-rate is average and complimentary filter is better then kalman or near as far as I see. I researched it a little and it seems simple and usefull. I liked it. Most important one, how can I get sampling time in spi ? Maybe there is a way, I dont know. Maybe I'm asking a bad question. If it is bad I'm sorry, this is my inexperience. |
No I did not. I said you could not achieve a 400nSec resolution, and that I didn't think you would keep up with the sample rate you are trying to use.
You need to understand just how slow fp maths is. A single atan, on a 40MHz PIC18, takes about 700uSec. Even on a DSPIC is takes tens of uSecs. Your read is slow (you have not told us what clock rate you are actually using on the I2C), but by the time you have read all the data, and processed it, the next update _will_ almost certainly have been missed. You need to be optimising the code for speed, and then synchronising to the update from the chip. Otherwise the data received will effectively be garbage. I'd suspect the reason it goes badly wrong when you add Z, is that the maths is actually just coping with the 2D solution, and running out of time when you go 3D. |
|
|
|
|
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
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
