Go Down

Topic: Guide to gyro and accelerometer with Arduino including Kalman filtering (Read 314 times) previous topic - next topic

LillSlick

hey, i made a stupid version starting from the data acquisition code you provided :)  they wanted to see my progress so ... now i have until Thursday to finish, and one of them was so impressed he`s gonna send me tomorrow to do some work 210 KM away from home, just to be sure i don`t finish on time. This said, i`ll show u my code, i`ll have to implement that motor control of yours, i hope i`ll succeed.
Lemme show you how it works until now :
http://www.youtube.com/watch?v=hyUH676I7zA

And my stupid code is:
Code: [Select]


#include <Wire.h>
#include <PID_v1.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#define LPWM 3          // left motor PWM
#define RPWM 5         // right motor PWM
#define LDIR 4         // left motor direction
#define RDIR 7         // right motor direction

#define KP 5          // proportional controller gain [LSB/deg/loop]
#define KD 2.5          // derivative controller gain [LSB/deg/loop]

#define W_radius 0.0035  // meters

#define minOutput 0
#define maxOutput 250


Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
double velocity;

double Setpoint = 0, Input, Output;
double aggKp=8, aggKi=0.5, aggKd=2;
double consKp=4, consKi=0.02, consKd=1; 
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
float output = 0.0;

void setup() { 
    // Make sure all motor controller pins start low.
  digitalWrite(LPWM, LOW);
  digitalWrite(RPWM, LOW);
  digitalWrite(LDIR, LOW);
  digitalWrite(RDIR, LOW);
 
  // Set all motor control pins to outputs.
  pinMode(LPWM, OUTPUT);
  pinMode(RPWM, OUTPUT);
  pinMode(LDIR, OUTPUT);
  pinMode(RDIR, OUTPUT);
  pinMode(13, OUTPUT);
 
  // PID library
    myPID.SetOutputLimits(minOutput,maxOutput);
    myPID.SetSampleTime(1);
    myPID.SetMode(AUTOMATIC);
 
// Serial.begin(115200);
  Wire.begin();
  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
 
  while(i2cRead(0x75,i2cData,1));
  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while(1);
  }
 
  delay(100); // Wait for sensor to stabilize
 
  /* Set kalman and gyro starting angle */
  while(i2cRead(0x3B,i2cData,6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
 
  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;
 
  timer = micros();
}

void loop() {
  /* Update all the values */ 
  while(i2cRead(0x3B,i2cData,14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3])/1000;
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]); 
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
 
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
 
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter 
// gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
 
  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
 
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
   
   //temp = ((double)tempRaw + 12412.0) / 340.0;
   
   velocity = accY/W_radius;
   Input = velocity;

 
  double sens = Setpoint-Input;
        if(sens>1)
        {
          myPID.SetControllerDirection(DIRECT);
        }
        else if(sens<-1)
        {
          myPID.SetControllerDirection(REVERSE);
        }
       
        double gap = abs(Setpoint-Input);
       

          if(gap<10)
          { 
            myPID.SetTunings(consKp, consKi, consKd);
          }
          else
          {
             myPID.SetTunings(aggKp, aggKi, aggKd);
          }
       
          myPID.Compute();
        //  Output = map(Output, 0, 1000, 0, 255);
          if(Output<minOutput+20){ Output=0; }
   
     if(sens>=0)
           {
             digitalWrite(LDIR, LOW);
             digitalWrite(RDIR, HIGH);
             analogWrite(LPWM,Output);
             analogWrite(RPWM,Output);
             
           }
           else
           {
             digitalWrite(LDIR, HIGH);
             digitalWrite(RDIR, LOW);             
             analogWrite(LPWM,Output);
             analogWrite(RPWM,Output);           
             
           }
delay(1);
}

It`s not good, but ... it does SOMETHING, as you can observe i`m not using the kalman angle, i loaded that library but the code does not work it always gives me error at that single line :
Code: [Select]

#include <Kalman.h> // Kalman filter library - see:...

So, i`m making progress, but i really need to set that speed, and controller you recommended. I`ll get right on it, i have 2 hours until i have to go to sleep because i`m awfully tired. Keep in touch, what do you say about my code? stupid huh? :))

Lauszus

You should use either the complimentary or Kalman filter or you won't be able to make it balance!
Sounds like you are not that happy about your co-workers? ;) What is your profession?

See the following example on how to include the Kalman library: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050.
The simply is to simply add it next to the .ino file and then add it like so: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino#L19.

Hope you will get it working before your deadline!

LillSlick

i'm in the automation department, but the people are envious... i'm still getting that error when i create the Kalman instance at line 43,not at the include line, i'll be working on the robot later... now i'm 200km away, if you would be kind to keep an eye on the forum i will  appreciate your effort! or if you have an instant messaging possibility that would be great :)  i'll catch up with you later, when I get home. thanks a lot for doing this, i would be nothing without your help!

Lauszus

Okay. That's because you haven't included it properly for some reason. Either put it next to the ino file and include it like so:
Code: [Select]

#include "Kalman.h"


Or put it in your libraries folder as any other Arduino library and include it like so:
Code: [Select]

#include <Kalman.h>


You can find me on Google Plus. Simply just search for Kristian Lauszus ;)
Glad you appreciate my help!

Ben91

Hi Kristian ! First of all thank you a lot for your guide ! It is really helpful.

I am a french student in an engeneering school and I am using the crius aiop v2 on my robot. On this board there is an MPU6050 and I am communicating with it thanks to your code. I tested a lot of codes like the one on arduino playground but yours is definetely the best.

However I am still having some issues with this sensor...

Correct me if I am wrong but when I am reading the raw data for the accelerometer I am supposed to get for x, y and z : 0 , 0 and 0.98 right ?
However when I am just reading the right registers (my sensor is put still on my desk) I am getting those values instead :
  accX          accY          accZ
8324.00    4768.00   14496.00      
8208.00    4768.00   14536.00      
8216.00    4704.00   14296.00      
8288.00    4684.00   14608.00      
8356.00    4724.00   14464.00      
8372.00   4784.00   14588.00      
8280.00   4736.00   14408.00      
8332.00   4748.00   14356.00

Since I am using your code, the sensor is set on a scale range of +/-2g which means that the sensitivity of the accelerometer is of 16384 LSB/g. I know that to understand the raw datas I need to divide them with the sensitivity and get something near 0, 0, 0.98 even if there is an error.
However with the values I am having it is no where near from what it should be...
I tried the code from arduino playground and I am having the same raw datas... Your guide is really clear but I must be missing something... I am just reading the right registers but those raw datas doesn't make any sense at all...

So I would like to know if you think those datas are strange and if you happened to find something similar, I happened to find some talk on the internet which says that there is a version of the MPU6050 that doesn't have the same scale ranges and not the same sensitvities... I don't think I have that much of bad luck...

Thank you for your help,
Ben.

hnk1412

Hi everybody!
I'm using Lsm303.it include accelerometer and magnetometer.And i want to use accelerometer to estimate velocity but this is very noise.Can i combine 
accelerometer and magnetometer into kalman filter to filter the signal of accelerometer.Thank!

Lauszus

@Ben91
No the raw data is not in g's, but a 16-bit signed value. See the datasheet: http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A.pdf page 13.
So if you are using a range of +-2 it means that you must divide the value by 16384 to convert it into g's.

@hnk1412
No I don't believe it's possible to estimate the velocity like that without a lot of noise. You might be better of with using a GPS sensor.

gerry87

Hey, great guide! I've been following it at the very early stage of a quadcopter build.

The accelerometer seems to be working fine but I'm having a bit of trouble with the gyroscope values. They don't seem to make sense, they don't go below 0 when spun counterclockwise.

Would someone mind looking through my code and seeing if anything doesn't look right?

Code: [Select]


// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include <Wire.h>
#include <Servo.h>


// I2Cdev and L3G4200D must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include <I2Cdev.h>
#include <L3G4200D.h>
#include <HMC5883L.h>
#include <ADXL345.h>


// default address is 105
// specific I2C address may be passed here
L3G4200D gyro;
ADXL345 accel;
HMC5883L mag;

int16_t magX, magY, magZ;

int16_t accValX, accValY, accValZ;
float accBiasX, accBiasY, accBiasZ;
float accAngleX, accAngleY;
float R;

int16_t gyroX, gyroY, gyroZ;
float gyroBiasX, gyroBiasY, gyroBiasZ;
float gyroRateX, gyroRateY, gyroRateZ;
float gyroBias_oldX, gyroBias_oldY, gyroBias_oldZ;


float pitchComp, rollComp;

double t_0, t_1;

const float alpha = 0.5;
Servo magYservo;  // create servo object to control a servo
int val = 0;    // variable to read the value from the analog pin
int throttle = 0;
double gyro_sensitivity = 70; //From datasheet, depends on Scale, 2000DPS = 70, 500DPS = 17.5, 250DPS = 8.75.


void setSpeed(int speed, int throttle){
  int angle = map(speed, 0, 80, 59, 133);
  magYservo.write(angle + throttle);
  //Serial.println(angle);
}


void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();

  // initialize serial communication
  // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
  // it's really up to you depending on your project)
  Serial.begin(14400);
  magYservo.attach(9);
  delay(1000);
  setSpeed(0, 0);
  delay(1000);

  // initialize device
  //Serial.println("Initializing I2C devices...");
  gyro.initialize();
  accel.initialize();
  mag.initialize();

  // verify connection
  // Serial.println("Testing device connections..."); 
//   Serial.println(accel.testConnection() ? "ADXL345 (accelerometer) connection successful" : "ADXL345 (accelerometer) connection failed");
//   Serial.println(mag.testConnection() ? "HMC5883L (magnetometer) connection successful" : "HMC5883L (magnetometer) connection failed");
//   Serial.println(gyro.testConnection() ? "L3G4200D (gyro) connection successful" : "L3G4200D connection failed");

   
  // data seems to be best when full scale is 2000
  gyro.setFullScale(2000);

  // Calculate bias for the Gyro i.e. the values it gives when it's not moving
  for(int i=1; i < 100; i++){
    gyroBiasX += gyro.getAngularVelocityX();
    gyroBiasY += gyro.getAngularVelocityY();
    gyroBiasZ += gyro.getAngularVelocityZ(); 

    accBiasX += accel.getAccelerationX();
    accBiasY += accel.getAccelerationY();
    accBiasZ += accel.getAccelerationZ();
   
    delay(1);
  }
  gyroBiasX = gyroBiasX / 100;
  gyroBiasY = gyroBiasY / 100;
  gyroBiasZ = gyroBiasZ / 100;

  accBiasX = accBiasX / 100;
  accBiasY = accBiasY / 100;
  accBiasZ = accBiasZ / 100;

  // Calculate true sensitivity
  gyro_sensitivity = ((gyro_sensitivity/1000)/3.3)*1023; //16-bit resolution gyro. 2^16-1 = 65535

  t_0 = millis();
}


void loop() {

  //////////////////////
  //  Accelerometer   //
  //////////////////////
  double accPitch, accRoll;

  accel.getAcceleration(&accValX, &accValY, &accValZ);
 
   R = sqrt(sq(accValX)+sq(accValY)+sq(accValZ));
   accPitch = acos(accValX/R)*RAD_TO_DEG-90;
   accRoll= -acos(accValY/R)*RAD_TO_DEG-90;
   
  // Seems to do the same as above
  //  accPitch = -(atan2(accValX, sqrt(accValY*accValY + accValZ*accValZ))*180)/M_PI;
  //  accRoll = (atan2(-accValY, accValZ)*180)/M_PI;

  if(accValZ < 0)//360 degrees
  {
    if(accPitch < 0)
    {
      accPitch = -180-accPitch;
    }
    else
    {
      accPitch = 180-accPitch;
    }

    if(accRoll < 0)
    {
      accRoll = -180-accRoll;
    }
    else
    {
      accRoll = 180-accRoll;
    }
  }


  //////////////////////
  //      GYRO        //
  //////////////////////
 
  double gyroPitch, gyroRoll;
  // read raw angular velocity measurements from device
  gyro.getAngularVelocity(&gyroX, &gyroY, &gyroZ);

  gyroRateX =  (gyroX - gyroBiasX)/gyro_sensitivity;
  gyroRateY =  (gyroY - gyroBiasY)/gyro_sensitivity;

  t_1 = millis();
  gyroPitch += gyroRateX * (t_1 - t_0)/1000;
  gyroRoll += gyroRateY * (t_1 - t_0)/1000;
  t_0 = millis();


  //////////////////////
  //    MAGNOMETER    //
  //////////////////////

  mag.getHeading(&magX, &magY, &magZ);
 

  //Heading Estimation
  float magYaw = atan2(magY, magX);
  if(magYaw < 0)
    magYaw += 2 * M_PI;
  magYaw = (magYaw*180)/M_PI;
 
 
  //Complementary Filter
 
  pitchComp = 0.98 *(pitchComp+gyroPitch) + 0.02 *accPitch;
  rollComp = 0.98 *(rollComp+gyroRoll) + 0.02 * accRoll;


  Serial.print(accPitch);
  Serial.print(",");
  Serial.print(accRoll); 
  Serial.print(",");
  Serial.print(magYaw);
  Serial.print("\n");

  if (Serial.available()) {
    throttle = Serial.parseInt();
  }

  setSpeed(accPitch, throttle);

  delay(50);

}




Lauszus

@gerry87
Please send me a link to the library you are using, so I can see the code.

Also please see this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino. It's shows a better way to calculate the angle using the accelerometers.

gerry87


@gerry87
Please send me a link to the library you are using, so I can see the code.

Also please see this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino. It's shows a better way to calculate the angle using the accelerometers.


https://github.com/jrowberg/i2cdevlib/tree/master/Arduino here is the library, and here's the specific one for the L3G4200D https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/L3G4200D

The better way you mention to calculate the angle is it:
Code: [Select]

accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;


Thanks, I'll try it when I get back home. Does this account for the 360 degrees (inverted)?

Lauszus

It looks alright. Try to print the raw gyro values (gyroX, gyroY and gyroZ) and see if they look okay. Also try to run the provided example: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino and see if that works.

Quote

Thanks, I'll try it when I get back home. Does this account for the 360 degrees (inverted)?

Yes ;)

gerry87

When i run this, here's a sample of the output from leaving it sitting, picking it up, spinning clockwise (x-axis) and then anti-clockwise (x-axis). What I'm wondering is should the values go negative when being spun anti-clockwise? They all seem to be between 0 and 255. Is my sensitivity correct (gyro_sensitivity = ((gyro_sensitivity/1000)/3.3)*1023) ? :

Code: [Select]

Initializing I2C devices...
Testing device connections...
L3G4200D connection successful
183 213 32
8 253 19
9 255 23
11 0 26
8 252 17
11 255 24
11 253 16
7 252 20
8 253 17
12 254 17
8 1 18
10 255 17
12 250 19
5 251 19
13 0 22
10 254 19
13 254 18
10 254 18
13 254 15
13 1 25
10 254 20
14 255 17
13 250 20
18 255 21
11 0 19
14 1 14
14 255 16
12 0 17
7 255 19
10 252 19
13 254 18
10 248 14
7 254 15
10 252 20
8 253 19
10 251 14
11 254 18
11 3 19
10 255 12
9 252 18
13 3 20
9 255 18
9 4 24
13 1 18
8 4 17
16 0 16
10 0 22
7 1 12
15 255 16
10 1 21
11 255 23
6 0 19
9 252 23
11 4 15
7 252 20
8 1 20
5 1 20
11 254 15
7 252 16
15 255 15
11 254 18
14 250 20
14 254 20
6 255 25
8 250 21
7 255 22
12 253 23
11 0 14
11 252 20
8 253 24
10 251 16
5 251 18
12 254 20
18 254 18
15 254 24
3 255 22
5 251 15
9 0 18
7 249 20
12 255 22
7 252 21
12 3 19
11 253 22
13 253 17
13 1 15
13 1 21
8 251 16
7 255 16
13 250 23
8 255 16
10 252 18
9 1 20
10 255 17
14 252 18
9 251 13
11 254 19
12 253 22
9 1 15
13 251 19
9 255 20
6 249 16
8 253 17
10 1 24
8 253 18
9 253 16
10 248 22
7 251 21
8 3 21
10 253 19
10 0 23
10 250 19
5 255 19
11 250 20
11 252 26
10 250 19
11 251 12
10 254 15
12 0 18
10 255 23
7 0 21
9 250 22
13 251 16
8 248 22
9 0 18
9 255 24
15 254 17
7 255 25
10 255 19
14 253 19
13 252 25
11 2 17
9 255 18
12 5 21
13 254 20
13 254 17
13 0 20
12 0 20
11 250 25
6 255 16
16 1 17
14 253 19
10 255 19
11 255 19
9 254 24
10 1 17
12 250 18
7 5 13
9 255 21
4 249 20
15 255 15
9 245 16
7 248 18
9 1 17
9 252 21
7 0 16
14 252 17
9 255 18
10 1 20
12 251 17
14 253 19
10 1 17
12 255 13
10 252 14
10 254 16
11 255 17
7 255 21
10 0 21
10 254 21
9 0 20
9 254 16
10 0 15
12 254 13
8 250 16
14 0 11
12 255 16
12 3 19
7 251 25
11 255 18
10 255 22
9 255 15
11 254 19
12 251 17
13 255 18
8 253 19
14 0 17
9 254 19
16 250 19
7 1 20
11 1 17
8 255 20
7 1 16
11 0 15
17 1 19
11 0 22
12 254 20
13 253 17
12 255 17
12 254 28
11 252 21
13 252 20
14 254 18
12 251 18
11 251 22
10 253 18
9 0 23
15 248 20
13 0 15
10 254 20
10 255 19
10 254 18
7 249 21
10 251 21
7 1 23
7 252 15
6 253 19
9 254 18
13 253 18
8 246 21
11 255 14
12 0 13
7 254 22
12 252 20
7 254 20
13 251 18
12 254 13
9 249 17
8 253 20
13 255 20
11 253 17
14 254 21
16 2 19
12 255 15
10 255 17
11 0 20
9 253 17
12 0 21
11 247 16
11 0 22
8 0 15
8 254 17
13 253 20
9 1 14
13 253 17
8 252 19
13 255 18
11 252 23
9 252 18
14 253 16
9 4 24
18 253 25
9 251 18
6 0 18
12 1 21
18 254 17
10 2 19
14 254 17
10 255 24
9 251 18
10 0 16
11 3 18
7 254 24
8 1 16
7 255 18
10 1 16
9 254 20
15 249 20
6 0 14
13 252 23
10 253 18
7 254 20
10 254 19
10 1 19
11 255 20
13 250 17
17 253 15
13 251 18
6 255 19
7 249 15
11 253 16
12 252 16
9 254 18
10 2 19
7 3 23
10 252 19
8 2 12
6 255 17
14 254 15
11 251 16
14 2 14
7 1 22
9 253 17
14 0 17
15 252 16
11 250 21
5 255 16
8 253 18
8 253 25
11 253 18
10 253 13
9 252 15
7 0 20
11 255 19
11 253 21
5 255 16
13 251 15
13 254 19
10 5 9
9 1 17
4 254 15
12 254 11
9 1 17
10 254 19
11 254 22
13 254 16
12 5 20
11 1 20
13 252 21
13 252 18
8 254 22
16 253 20
10 253 22
8 252 17
9 253 15
14 255 17
11 0 14
8 252 14
11 0 17
8 249 20
8 247 17
12 1 20
9 255 21
11 0 26
10 255 22
10 3 15
9 0 18
8 253 17
10 252 14
6 255 15
11 248 21
10 255 25
5 254 20
8 254 17
13 252 21
13 254 17
9 254 17
7 1 23
4 253 20
13 254 20
9 254 18
5 251 21
10 2 17
9 2 21
16 252 16
15 253 16
6 255 15
12 254 20
11 0 19
10 254 17
13 1 19
11 255 17
11 251 21
10 0 19
9 2 21
10 0 21
14 249 18
14 250 16
14 250 18
12 1 14
13 254 16
9 254 16
10 252 19
9 250 13
6 255 17
7 251 16
16 2 23
13 253 20
8 2 20
9 3 22
12 0 27
16 255 94
11 1 120
13 3 104
238 88 101
0 226 240
52 142 52
242 158 234
66 128 179
137 235 17
234 154 7
117 1 152
25 76 231
189 157 44
1 121 104
122 198 127
102 126 232
216 201 119
95 174 108
255 122 49
115 129 78
108 56 218
140 135 143
121 206 161
61 187 0
13 216 107
80 45 51
165 187 158
69 135 29
44 97 142
9 171 119
6 49 157
81 109 232
41 185 148
5 232 242
146 242 217
20 252 146
103 211 101
27 135 253
235 202 30
173 252 118
41 127 59
203 43 63
102 45 105
189 218 142
121 158 213
15 190 44
39 82 34
173 69 128
239 221 38
13 123 193
110 170 216
114 32 34
144 115 36
17 76 104
251 224 5
1 240 197
74 61 36
166 236 64
3 59 236
163 120 138
123 107 236
192 27 0
147 51 196
118 125 191
9 227 224
220 170 24
61 189 61
13 249 96
143 116 131
21 32 56
15 3 44
18 10 40
25 3 52
23 251 148
18 249 88
18 250 64
136 168 78
9 250 170
19 238 162
12 247 52
6 220 15
10 15 19
12 17 16
7 1 20
8 247 20
8 248 23
15 253 16
12 12 18
10 255 17
9 254 18
13 2 19
14 249 17
9 0 18
11 252 16
10 4 18
13 251 14
11 0 18
6 0 20
12 0 24
8 255 17
14 252 22
12 252 20
11 0 21
8 255 19
11 252 19
8 0 14
6 0 19
5 6 19
11 254 16
4 250 207
13 0 25
9 250 26
13 252 25
9 254 20
17 7 40
10 0 246
15 2 245
12 248 18
5 249 25
10 0 20
11 3 35
10 6 251
8 255 62
11 0 75
10 245 95
9 238 47
11 249 18
14 245 23
40 225 47
128 229 66
224 64 96
196 252 72
41 230 164
162 125 48
227 241 27
226 245 249
237 230 232
120 96 120
162 100 67
7 9 241
112 172 13
30 224 140
102 162 244
63 53 56
89 228 166
248 173 152
14 94 190
153 6 96
137 227 199
124 199 77
56 146 228
68 250 158
3 5 135
165 210 67
13 13 78
32 17 225
150 208 168
129 112 69
184 240 152
236 231 29
42 129 193
207 8 32
70 206 85
57 223 14
111 228 242
176 6 141
121 130 167
55 103 163
245 62 71
133 246 243
69 210 46
51 182 4
118 126 223
14 60 213
223 71 224
237 16 252
10 13 11
29 66 20


Quote

Thanks, I'll try it when I get back home. Does this account for the 360 degrees (inverted)?

Yes ;)
[/quote]
Tried it there, nice and elegant!

I noticed another error in my code, I changed

Code: [Select]

//Complementary Filter
 
 pitchComp = 0.98 *(pitchComp+gyroPitch) + 0.02 *accPitch;
 rollComp = 0.98 *(rollComp+gyroRoll) + 0.02 * accRoll;


to

Code: [Select]


 pitchComp = 0.5 *(pitchComp + gyroRateX * (t_1 - t_0)/1000) + 0.5 *accPitch;
 rollComp = 0.5 *(rollComp + gyroRateY * (t_1 - t_0)/1000) + 0.5 * accRoll;



since gyroPitch and gyroRoll were already the accumulated values.

Is IMU6DOFVer3 on the front page the latest version you have?

Thanks for the help here.

Lauszus

I don't understand why you want to calculate the sensitivity like that. Simply just divide the output by the one given in the datasheet: http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf page 10.

Also please try to run the example code: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino and see if it works.

The newest version can be found at Github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter, as I explain in the guide.

gerry87


I don't understand why you want to calculate the sensitivity like that. Simply just divide the output by the one given in the datasheet: http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf page 10.

Also please try to run the example code: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino and see if it works.

The newest version can be found at Github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter, as I explain in the guide.


Sorry, I wasn't very clear. When I run https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino, it gets values from the gyro fine, but I get the raw output below (with the words 'angular velocity:' removed because the post was too big...):

The values coming from the X-axis are in the first column. When it's at rest they hover around 15 (gyroXZero), when I pick it up and spin it clockwise they go up to >200, when I spin anti-clockwise they also go up to >200 (Min is 0 and Max is 255). Is this what is expected from the raw output?

To convert these values to Degrees Per Second, it seems that I take (gyroXRaw - gyroXZero)/70 (sensitivity from the datasheet).

Sorry for all the questions, I've found a lot of information on google, but I can't seem to find anything to clear this up.

Thanks again.

Code: [Select]

Initializing I2C devices...
Testing device connections...
L3G4200D connection successful
183 213 32
8 253 19
9 255 23
11 0 26
8 252 17
11 255 24
11 253 16
7 252 20
8 253 17
12 254 17
8 1 18
10 255 17
12 250 19
5 251 19
13 0 22
10 254 19
13 254 18
10 254 18
13 254 15
13 1 25
10 254 20
14 255 17
13 250 20
18 255 21
11 0 19
14 1 14
14 255 16
12 0 17
7 255 19
10 252 19
13 254 18
10 248 14
7 254 15
10 252 20
8 253 19
10 251 14
11 254 18
11 3 19
10 255 12
9 252 18
13 3 20
9 255 18
9 4 24
13 1 18
8 4 17
16 0 16
10 0 22
7 1 12
15 255 16
10 1 21
11 255 23
6 0 19
9 252 23
11 4 15
7 252 20
8 1 20
5 1 20
11 254 15
7 252 16
15 255 15
11 254 18
14 250 20
14 254 20
6 255 25
8 250 21
7 255 22
12 253 23
11 0 14
11 252 20
8 253 24
10 251 16
5 251 18
12 254 20
18 254 18
15 254 24
3 255 22
5 251 15
9 0 18
7 249 20
12 255 22
7 252 21
12 3 19
11 253 22
13 253 17
13 1 15
13 1 21
8 251 16
7 255 16
13 250 23
8 255 16
10 252 18
9 1 20
10 255 17
14 252 18
9 251 13
11 254 19
12 253 22
9 1 15
13 251 19
9 255 20
6 249 16
8 253 17
10 1 24
8 253 18
9 253 16
10 248 22
7 251 21
8 3 21
10 253 19
10 0 23
10 250 19
5 255 19
11 250 20
11 252 26
10 250 19
11 251 12
10 254 15
12 0 18
10 255 23
7 0 21
9 250 22
13 251 16
8 248 22
9 0 18
9 255 24
15 254 17
7 255 25
10 255 19
14 253 19
13 252 25
11 2 17
9 255 18
12 5 21
13 254 20
13 254 17
13 0 20
12 0 20
11 250 25
6 255 16
16 1 17
14 253 19
10 255 19
11 255 19
9 254 24
10 1 17
12 250 18
7 5 13
9 255 21
4 249 20
15 255 15
9 245 16
7 248 18
9 1 17
9 252 21
7 0 16
14 252 17
9 255 18
10 1 20
12 251 17
14 253 19
10 1 17
12 255 13
10 252 14
10 254 16
11 255 17
7 255 21
10 0 21
10 254 21
9 0 20
9 254 16
10 0 15
12 254 13
8 250 16
14 0 11
12 255 16
12 3 19
7 251 25
11 255 18
10 255 22
9 255 15
11 254 19
12 251 17
13 255 18
8 253 19
14 0 17
9 254 19
16 250 19
7 1 20
11 1 17
8 255 20
7 1 16
11 0 15
17 1 19
11 0 22
12 254 20
13 253 17
12 255 17
12 254 28
11 252 21
13 252 20
14 254 18
12 251 18
11 251 22
10 253 18
9 0 23
15 248 20
13 0 15
10 254 20
10 255 19
10 254 18
7 249 21
10 251 21
7 1 23
7 252 15
6 253 19
9 254 18
13 253 18
8 246 21
11 255 14
12 0 13
7 254 22
12 252 20
7 254 20
13 251 18
12 254 13
9 249 17
8 253 20
13 255 20
11 253 17
14 254 21
16 2 19
12 255 15
10 255 17
11 0 20
9 253 17
12 0 21
11 247 16
11 0 22
8 0 15
8 254 17
13 253 20
9 1 14
13 253 17
8 252 19
13 255 18
11 252 23
9 252 18
14 253 16
9 4 24
18 253 25
9 251 18
6 0 18
12 1 21
18 254 17
10 2 19
14 254 17
10 255 24
9 251 18
10 0 16
11 3 18
7 254 24
8 1 16
7 255 18
10 1 16
9 254 20
15 249 20
6 0 14
13 252 23
10 253 18
7 254 20
10 254 19
10 1 19
11 255 20
13 250 17
17 253 15
13 251 18
6 255 19
7 249 15
11 253 16
12 252 16
9 254 18
10 2 19
7 3 23
10 252 19
8 2 12
6 255 17
14 254 15
11 251 16
14 2 14
7 1 22
9 253 17
14 0 17
15 252 16
11 250 21
5 255 16
8 253 18
8 253 25
11 253 18
10 253 13
9 252 15
7 0 20
11 255 19
11 253 21
5 255 16
13 251 15
13 254 19
10 5 9
9 1 17
4 254 15
12 254 11
9 1 17
10 254 19
11 254 22
13 254 16
12 5 20
11 1 20
13 252 21
13 252 18
8 254 22
16 253 20
10 253 22
8 252 17
9 253 15
14 255 17
11 0 14
8 252 14
11 0 17
8 249 20
8 247 17
12 1 20
9 255 21
11 0 26
10 255 22
10 3 15
9 0 18
8 253 17
10 252 14
6 255 15
11 248 21
10 255 25
5 254 20
8 254 17
13 252 21
13 254 17
9 254 17
7 1 23
4 253 20
13 254 20
9 254 18
5 251 21
10 2 17
9 2 21
16 252 16
15 253 16
6 255 15
12 254 20
11 0 19
10 254 17
13 1 19
11 255 17
11 251 21
10 0 19
9 2 21
10 0 21
14 249 18
14 250 16
14 250 18
12 1 14
13 254 16
9 254 16
10 252 19
9 250 13
6 255 17
7 251 16
16 2 23
13 253 20
8 2 20
9 3 22
12 0 27
16 255 94
11 1 120
13 3 104
238 88 101
0 226 240
52 142 52
242 158 234
66 128 179
137 235 17
234 154 7
117 1 152
25 76 231
189 157 44
1 121 104
122 198 127
102 126 232
216 201 119
95 174 108
255 122 49
115 129 78
108 56 218
140 135 143
121 206 161
61 187 0
13 216 107
80 45 51
165 187 158
69 135 29
44 97 142
9 171 119
6 49 157
81 109 232
41 185 148
5 232 242
146 242 217
20 252 146
103 211 101
27 135 253
235 202 30
173 252 118
41 127 59
203 43 63
102 45 105
189 218 142
121 158 213
15 190 44
39 82 34
173 69 128
239 221 38
13 123 193
110 170 216
114 32 34
144 115 36
17 76 104
251 224 5
1 240 197
74 61 36
166 236 64
3 59 236
163 120 138
123 107 236
192 27 0
147 51 196
118 125 191
9 227 224
220 170 24
61 189 61
13 249 96
143 116 131
21 32 56
15 3 44
18 10 40
25 3 52
23 251 148
18 249 88
18 250 64
136 168 78
9 250 170
19 238 162
12 247 52
6 220 15
10 15 19
12 17 16
7 1 20
8 247 20
8 248 23
15 253 16
12 12 18
10 255 17
9 254 18
13 2 19
14 249 17
9 0 18
11 252 16
10 4 18
13 251 14
11 0 18
6 0 20
12 0 24
8 255 17
14 252 22
12 252 20
11 0 21
8 255 19
11 252 19
8 0 14
6 0 19
5 6 19
11 254 16
4 250 207
13 0 25
9 250 26
13 252 25
9 254 20
17 7 40
10 0 246
15 2 245
12 248 18
5 249 25
10 0 20
11 3 35
10 6 251
8 255 62
11 0 75
10 245 95
9 238 47
11 249 18
14 245 23
40 225 47
128 229 66
224 64 96
196 252 72
41 230 164
162 125 48
227 241 27
226 245 249
237 230 232
120 96 120
162 100 67
7 9 241
112 172 13
30 224 140
102 162 244
63 53 56
89 228 166
248 173 152
14 94 190
153 6 96
137 227 199
124 199 77
56 146 228
68 250 158
3 5 135
165 210 67
13 13 78
32 17 225
150 208 168
129 112 69
184 240 152
236 231 29
42 129 193
207 8 32
70 206 85
57 223 14
111 228 242
176 6 141
121 130 167
55 103 163
245 62 71
133 246 243
69 210 46
51 182 4
118 126 223
14 60 213
223 71 224
237 16 252
10 13 11
29 66 20


gerry87

Just updating this for anyone with the same issue, I tried out the https://github.com/pololu/L3G/tree/master/L3G library instead of the i2cdev library and I'm now getting values between -65,535 and 65,536 with negative values for anti-clockwise rotations instead of 0 - 255 with no difference between anti-clockwise and clockwise.

There's either an issue in the i2cdev library for the L3G4200D or I was just using it incorrectly.

Go Up