Go Down

Topic: Guide to gyro and accelerometer with Arduino including Kalman filtering (Read 776252 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 :

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 250Kalman kalmanX; // Create the Kalman instancesKalman kalmanY;/* IMU Data */int16_t accX, accY, accZ;int16_t tempRaw;int16_t gyroX, gyroY, gyroZ;double accXangle, accYangle; // Angle calculate using the accelerometerdouble temp; // Temperaturedouble gyroXangle, gyroYangle; // Angle calculate using the gyrodouble compAngleX, compAngleY; // Calculate the angle using a complementary filterdouble kalAngleX, kalAngleY; // Calculate the angle using a Kalman filterdouble 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 datafloat 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!

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.

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

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...

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 hereL3G4200D 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.

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