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 CCS Technical Support
L3G4200D Digital Gyroscope + Processing

 
Post new topic   Reply to topic    CCS Forum Index -> Code Library
View previous topic :: View next topic  
Author Message
cleberalbert



Joined: 25 Feb 2014
Posts: 34
Location: Brazil

View user's profile Send private message

L3G4200D Digital Gyroscope + Processing
PostPosted: Fri Apr 11, 2014 2:30 pm     Reply with quote
This driver is for use with L3G4200D Digital Gyroscope using the I2C protocol.
The code was done on CCS 5.018, is commented and is working well, tested by PIC18F4520 and Processing 2.1.1 by serial port on a PC. The code for Processing (.pde) is also below.

This program can give the raw values and the three critical flight dynamics parameters (roll, pitch and yaw=0).

Code:

#include <18F4520.h>
#case

#FUSES HS
#FUSES PUT
#FUSES NOWDT                    //No Watch Dog Timer
#FUSES NOBROWNOUT               //No brownout reset
#FUSES NOLVP                    //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES NOXINST                  //Extended set extension and Indexed Addressing mode disabled (Legacy mode)

#use delay(crystal=20MHz)
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,ERRORS)
#use i2c(master, fast, sda=PIN_C4, scl=PIN_C3)
#use timer(timer=1,tick=1ms,bits=16,noisr)

#define L3G4200D_ADDRESS   0xD2  //I2C address of the L3G4200D
#define GRY_CTRL_REG1 0x20
#define GRY_CTRL_REG2 0x21
#define GRY_CTRL_REG3 0x22
#define GRY_CTRL_REG4 0x23
#define GRY_CTRL_REG5 0x24
#define GRY_CTRL_REG6 0x25
#define GRY_WHO_AM_I 0x0F
#define GYR_OUT_LSB_X 0x28
#define GYR_OUT_MSB_X 0x29
#define GYR_OUT_LSB_Y 0x2A
#define GYR_OUT_MSB_Y 0x2B
#define GYR_OUT_LSB_Z 0x2C
#define GYR_OUT_MSB_Z 0x2D
#define GRY_SAMPLE_TIME 10
#define GRY_SAMPLE_NUM  500

//these 4 variables are used to save the raw values (8bits) and the raw values summed  (16bits; the most significant byte + the least significant byte). This working is done in Arduino Compiler by the class "L3G4200D".
unsigned int8 gyro_data[6];
signed int16 gyr_x=0;
signed int16 gyr_y=0;
signed int16 gyr_z=0;

signed int16 gyr_rate_x;
signed int16 gyr_rate_y;
signed int16 gyr_rate_z;

signed int16 gyr_prev_rate_x=0;
signed int16 gyr_prev_rate_y=0;
signed int16 gyr_prev_rate_z=0;
signed int16 gyr_dc_offset_x=0;
signed int16 gyr_dc_offset_y=0;
signed int16 gyr_dc_offset_z=0;
signed int16 current_time;

float gyr_noise=0;
float gyr_x1=0;
float gyr_y1=0;
float gyr_z1=0;

void initializingL3G4200D();
void gyroscope_angles(); //It gives the tilt angles around each axis (X,Y,Z)
void writeRegisterI2C(int8 deviceAddress, int8 address, int8 val);
int8 readRegisterI2C(int8 deviceAddress, int8 address);

void main(){
   initializingL3G4200D(); //equivalent the setup() function in Arduino Compiler
   set_ticks(0);
   while(TRUE) //equivalent the loop() function in Arduino Compiler
   {
   output_high(PIN_A0);
     
gyroscope_angles();
     
      output_low(PIN_A0);
   }
}

void initializingL3G4200D(){
   int16 n;
   
   writeRegisterI2C(L3G4200D_ADDRESS, GRY_CTRL_REG1, 0x0F);   // 100Hz, 12.5, Power Up
 
  for(n=0;n<GRY_SAMPLE_NUM;n++){
     // Read data from the gyro
     //these block of instructions are used to read the raw values (8bits) and to sum the raw values  (the most significant byte + the least significant byte=16bits). This working is done in Arduino Compiler by the class "L3G4200D".
     gyro_data[0] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_X); //most significant byte
     gyro_data[1] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_X); //least significant byte
     gyro_data[2] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_Y); //most significant byte
     gyro_data[3] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_Y); //least significant byte
     gyro_data[4] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_Z); //most significant byte
     gyro_data[5] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_Z); //least significant byte
     //Concatenate data
     gyr_x=make16(gyro_data[0],gyro_data[1]);
     gyr_y=make16(gyro_data[2],gyro_data[3]);
     gyr_z=make16(gyro_data[4],gyro_data[5]); 
     
     gyr_dc_offset_x+=gyr_x;
     gyr_dc_offset_y+=gyr_y;
     gyr_dc_offset_z+=gyr_z;
   }

   gyr_dc_offset_x=gyr_dc_offset_x/GRY_SAMPLE_NUM;
   gyr_dc_offset_y=gyr_dc_offset_y/GRY_SAMPLE_NUM;
   gyr_dc_offset_z=gyr_dc_offset_z/GRY_SAMPLE_NUM;
   
   for(n=0;n<GRY_SAMPLE_NUM;n++){
     // Read data from the gyro
     //these block of instructions are used to read the raw values (8bits) and to sum the raw values  (the most significant byte + the least significant byte=16bits). This working is done in Arduino Compiler by the class "L3G4200D".
     gyro_data[0] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_X); //most significant byte
     gyro_data[1] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_X); //least significant byte
     gyro_data[2] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_Y); //most significant byte
     gyro_data[3] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_Y); //least significant byte
     gyro_data[4] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_Z); //most significant byte
     gyro_data[5] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_Z); //least significant byte
     //Concatenate data
     gyr_x=make16(gyro_data[0],gyro_data[1]);
     gyr_y=make16(gyro_data[2],gyro_data[3]);
     gyr_z=make16(gyro_data[4],gyro_data[5]);   
     
      if((gyr_z-gyr_dc_offset_z)>gyr_noise) gyr_noise=(float)(gyr_z-gyr_dc_offset_z);
      else if((gyr_z-gyr_dc_offset_z)<-gyr_noise) gyr_noise=(float)(-gyr_z-gyr_dc_offset_z);
   }
   gyr_noise=gyr_noise/100; //gyro returns hundredths of degrees/sec
   delay_ms(10); //wait for the sensor to be ready
}

void writeRegisterI2C(int8 deviceAddress, int8 address, int8 val){
   i2c_start();               // start transmission to device
   i2c_write(deviceAddress);  // send device address
   i2c_write(address);        // send register address
   i2c_write(val);            // send value to write
   i2c_stop();                // end transmission
}

int8 readRegisterI2C(int8 deviceAddress, int8 address){
   int v;
   i2c_start();                  // start transmission to device
   i2c_write(deviceAddress);     // This is where you have to _write_ the register number you want
   i2c_write(address);           // register to read
   i2c_start();                  // restart - the bus is now set to _read_
   i2c_write(deviceAddress + 1); // now turn the bus round
   v = i2c_read(0);
   i2c_stop();                   // This will update x, y, and z with new values
   return v;
}

void gyroscope_angles(){
   current_time=get_ticks();
   if(current_time > GRY_SAMPLE_TIME){
      set_ticks(0);  // reset sample timer
     
     // Read data from the gyro
     //these block of instructions are used to read the raw values (8bits) and to sum the raw values  (the most significant byte + the least significant byte=16bits). This working is done in Arduino Compiler by the class "L3G4200D".
     gyro_data[0] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_X); //most significant byte
     gyro_data[1] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_X); //least significant byte
     gyro_data[2] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_Y); //most significant byte
     gyro_data[3] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_Y); //least significant byte
     gyro_data[4] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_MSB_Z); //most significant byte
     gyro_data[5] = readRegisterI2C(L3G4200D_ADDRESS,GYR_OUT_LSB_Z); //least significant byte
     //Concatenate data
     gyr_x=make16(gyro_data[0],gyro_data[1]);
     gyr_y=make16(gyro_data[2],gyro_data[3]);
     gyr_z=make16(gyro_data[4],gyro_data[5]); 
//     printf("%ld;%ld;%ld\n",gyr_x,gyr_y,gyr_z);

      gyr_rate_x=(gyr_x-gyr_dc_offset_x)/100;
      gyr_rate_y=(gyr_y-gyr_dc_offset_y)/100;
      gyr_rate_z=(gyr_z-gyr_dc_offset_z)/100;
     
      if((gyr_rate_x>=gyr_noise) || (gyr_rate_x<=-gyr_noise)) gyr_x1 += ((float)(gyr_prev_rate_x + gyr_rate_x) * GRY_SAMPLE_TIME * 1.32) / 2000;
      if(gyr_rate_y >= gyr_noise || gyr_rate_y <= -gyr_noise) gyr_y1 += ((float)(gyr_prev_rate_y + gyr_rate_y) * GRY_SAMPLE_TIME * 1.32) / 2000;
      if(gyr_rate_z >= gyr_noise || gyr_rate_z <= -gyr_noise) gyr_z1 += ((float)(gyr_prev_rate_z + gyr_rate_z) * GRY_SAMPLE_TIME * 1.32) / 2000;
     
      // remember the current speed for the next loop rate integration.
      gyr_prev_rate_x = gyr_rate_x;
      gyr_prev_rate_y = gyr_rate_y;
      gyr_prev_rate_z = gyr_rate_z;
     
      // Keep our angle between 0-359 degrees
      if (gyr_x1 < 0) gyr_x1 += 360;
      else if (gyr_x1 >= 360) gyr_x1 -= 360;
      if (gyr_y1 < 0) gyr_y1 += 360;
      else if (gyr_y1 >= 360) gyr_y1 -= 360;
      if (gyr_z1 < 0) gyr_z1 += 360;
      else if (gyr_z1 >= 360) gyr_z1 -= 360;
     
      printf("%f;%f;%f\n",gyr_x1,gyr_y1,gyr_z1);
      delay_ms(1); //Just here to slow down the serial to make it more readable
   }
}


Processing code (.pde):

Code:

import processing.serial.*;
Serial myPort;    // The serial port
float xin=0;
float yin=0;
float zin=0;
float x, y, z;
PFont f;

void setup() {
  size(800, 800,P3D);
  myPort = new Serial(this, "COM6", 9600);
  myPort.bufferUntil('\n');
 f = createFont("Arial",16,true);
}
void draw() {
  background(255);
 
  stroke(175);
  line(width,0,width,height);
  textFont(f);       
  fill(0);
  textAlign(CENTER);
  text("X:", width/3,145);
  text("Y:", width/2,145);
  text("Z:", width*2/3,145);

  text(xin, width/3,160);
  text(yin, width/2,160);
  text(zin, width*2/3,160);
  fill(0,0,139); 
  translate(width/2, height/2);;
  rotateX(xin/55);
  rotateY(yin/55);
  rotateZ(zin/55);
  pushMatrix();
  box(100,200,10);

  popMatrix();
}

void serialEvent(Serial myPort)
{
  String inString = myPort.readString();
  float[] vals = float(split(inString,";"));
  xin=vals[0];
  yin=vals[1];
  zin=vals[2];
  vals[0]=0;
  vals[1]=0;
  vals[2]=0;
}
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> Code Library 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