Go Down

Topic: Parallax Gyroscope Module 3-Axis L3G4200D sample code (Read 7712 times) previous topic - next topic

jtbourke

Hello, I just (finally!) got an Arduino and also a Parallax 3-axis gyro.

I found that the code provided by Parallax has some problems:
1. It does not explain how to get degrees per second.
2. It has a bug which results in very noisy readings.
3. There is a misleading comment.

In case anyone else is trying to work with this sensor I thought it would be helpful if I shared what I have so far.  This should get you going much better than what Parallax provided:

Code: [Select]

//
// This is a simple program for testing the STMicroelectrics 3-axis gyroscope sold by Parallax.
//
// If you are new to the sensor you'll find all you need here.  Just rip out what you don't want to save RAM.
//
// If you are using an Arduino Uno R3 like me, connect the SCL line to pin A5.  Connect the SDA line to pin A4.
// I don't know anything about the other boards.  I'm new to Arduino.
//
// The sample code at Parallax is incorrect in a couple of places.  This code follows the application notes from STM.  In
// particular I want to note that I found it is very important to check status register 3 to make sure there is new
// data before trying to request it, or else you will get very noisy data.
//
// You can try modifying NUM_GYRO_SAMPLES and GYRO_SIGMA_MULTIPLE.
// These control the statistical averaging that is used to discard bad values.
// The NUM_GYRO_SAMPLES can be reduced for faster calibration, but accuracy will start to suffer if you go below 50 samples.
// Increase the GYRO_SIGMA_MULTIPLE if you have an application that doesn't need to keep track of small rotations.
// Reduce GYRO_SIGMA_MULTIPLE if small rotations are important and you don't mind more noise in the data.
//
// If you are trying to create a compass from the gyro data then use the heading array I added.  I found it pretty useful.
// If you set the sensor on your desk and only rotate it in yaw it works great.  Turn it upside down and it's a mess, naturally.
//
// If you are putting the sensor in a robot hoping for dead reckoning it might work, but it may be necessary to recalibrate once
// in awhile with the robot stationary.  It depends on how long your robot will be driving around and also how bumpy the
// surface is.
//
// Note that if you do not call updateGyro() often enough then you will miss data.  You can check for this by looking at
// the status register's 7th bit.  If it's on you've missed some data.  It's possible to set up an interrupt to catch it.  If you
// miss data then the report from the sensor isn't going to work as well for you.
//
// Use share and enjoy this however you like!
//
// Jim Bourke 2/6/2013 (RCGroups.com)
//
#include <Wire.h>

#define  CTRL_REG1  0x20
#define  CTRL_REG2  0x21
#define  CTRL_REG3  0x22
#define  CTRL_REG4  0x23
#define  CTRL_REG5  0x24
#define  CTRL_REG6  0x25

int gyroI2CAddr=105;

int gyroRaw[3];                         // raw sensor data, each axis, pretty useless really but here it is.
double gyroDPS[3];                      // gyro degrees per second, each axis

float heading[3]={0.0f};                // heading[x], heading[y], heading [z]

int gyroZeroRate[3];                    // Calibration data.  Needed because the sensor does center at zero, but rather always reports a small amount of rotation on each axis.
int gyroThreshold[3];                   // Raw rate change data less than the statistically derived threshold is discarded.

#define  NUM_GYRO_SAMPLES  50           // As recommended in STMicro doc
#define  GYRO_SIGMA_MULTIPLE  3         // As recommended

float dpsPerDigit=.00875f;              // for conversion to degrees per second

void setup() {
  Serial.begin(115200);
  Wire.begin();
  setupGyro();
  calibrateGyro();
}

void loop() {
  updateGyroValues();
  updateHeadings();
  //testCalibration();

  printDPS();
  Serial.print("   -->   ");
  printHeadings();
  Serial.println();
}

void printDPS()
{
  Serial.print("DPS X: ");
  Serial.print(gyroDPS[0]);
  Serial.print("  Y: ");
  Serial.print(gyroDPS[1]);
  Serial.print("  Z: ");
  Serial.print(gyroDPS[2]);
}

void printHeadings()
{
  Serial.print("Heading X: ");
  Serial.print(heading[0]);
  Serial.print("  Y: ");
  Serial.print(heading[1]);
  Serial.print("  Z: ");
  Serial.print(heading[2]);
}

void updateHeadings()
{
   
  float deltaT=getDeltaTMicros();

  for (int j=0;j<3;j++)
    heading[j] -= (gyroDPS[j]*deltaT)/1000000.0f;
}

// this simply returns the elapsed time since the last call.
unsigned long getDeltaTMicros()
{
  static unsigned long lastTime=0;
 
  unsigned long currentTime=micros();
 
  unsigned long deltaT=currentTime-lastTime;
  if (deltaT < 0.0)
     deltaT=currentTime+(4294967295-lastTime);
   
  lastTime=currentTime;
 
  return deltaT;
}

// I called this from the loop function to see what the right values were for the calibration constants.
// If you are trying to reduce the amount of time needed for calibration just try not to go so low that consecutive
// calibration calls give you completely unrelated data.  Some sensors are probably better than others.
void testCalibration()
{
  calibrateGyro();
  for (int j=0;j<3;j++)
  {
    Serial.print(gyroZeroRate[j]);
    Serial.print("  ");
    Serial.print(gyroThreshold[j]);
    Serial.print("  "); 
  }
  Serial.println();
  return;
}

// The settings here will suffice unless you want to use the interrupt feature.
void setupGyro()
{
  gyroWriteI2C(CTRL_REG1, 0x1F);        // Turn on all axes, disable power down
  gyroWriteI2C(CTRL_REG3, 0x08);        // Enable control ready signal
  setGyroSensitivity500();

  delay(100);
}

// Call this at start up.  It's critical that your device is completely stationary during calibration.
// The sensor needs recalibration after lots of movement, lots of idle time, temperature changes, etc, so try to work that in to your design.
void calibrateGyro()
{
  long int gyroSums[3]={0};
  long int gyroSigma[3]={0};

  for (int i=0;i<NUM_GYRO_SAMPLES;i++)
  {
    updateGyroValues();
    for (int j=0;j<3;j++)
    {
      gyroSums[j]+=gyroRaw[j];
      gyroSigma[j]+=gyroRaw[j]*gyroRaw[j];
    }
  }
  for (int j=0;j<3;j++)
  {
    int averageRate=gyroSums[j]/NUM_GYRO_SAMPLES;
   
    // Per STM docs, we store the average of the samples for each axis and subtract them when we use the data.
    gyroZeroRate[j]=averageRate;
   
    // Per STM docs, we create a threshold for each axis based on the standard deviation of the samples times 3.
    gyroThreshold[j]=sqrt((double(gyroSigma[j]) / NUM_GYRO_SAMPLES) - (averageRate * averageRate)) * GYRO_SIGMA_MULTIPLE;   
  }
}

void updateGyroValues() {

  while (!(gyroReadI2C(0x27) & B00001000)){}      // Without this line you will get bad data occasionally
 
  //if (gyroReadI2C(0x27) & B01000000)
  //  Serial.println("Data missed!  Consider using an interrupt");
   
  int reg=0x28;
  for (int j=0;j<3;j++)
  {
    gyroRaw[j]=(gyroReadI2C(reg) | (gyroReadI2C(reg+1)<<8));
    reg+=2;
  }

  int deltaGyro[3];
  for (int j=0;j<3;j++)
  {
    deltaGyro[j]=gyroRaw[j]-gyroZeroRate[j];      // Use the calibration data to modify the sensor value.
    if (abs(deltaGyro[j]) < gyroThreshold[j])
      deltaGyro[j]=0;
    gyroDPS[j]= dpsPerDigit * deltaGyro[j];      // Multiply the sensor value by the sensitivity factor to get degrees per second.
  }
}

void setGyroSensitivity250(void)
{
  dpsPerDigit=.00875f;
  gyroWriteI2C(CTRL_REG4, 0x80);        // Set scale (250 deg/sec)
}

void setGyroSensitivity500(void)
{
  dpsPerDigit=.0175f;
  gyroWriteI2C(CTRL_REG4, 0x90);        // Set scale (500 deg/sec)
}

void setGyroSensitivity2000(void)
{
  dpsPerDigit=.07f;
  gyroWriteI2C(CTRL_REG4,0xA0);
}

int gyroReadI2C (byte regAddr) {
  Wire.beginTransmission(gyroI2CAddr);
  Wire.write(regAddr);
  Wire.endTransmission();
  Wire.requestFrom(gyroI2CAddr, 1);
  while(!Wire.available()) {};
  return (Wire.read());
}

int gyroWriteI2C( byte regAddr, byte val){
  Wire.beginTransmission(gyroI2CAddr);
  Wire.write(regAddr);
  Wire.write(val);
  Wire.endTransmission();
}


Jim Bourke (from RCGroups.com)

jtbourke

And here is a quick stab at making a pitch/roll/yaw sensor.  A friend suggests a filter instead of discarding data.  I might try that next.

I've only tested this on my desktop so I don't know how reliable it is, but I figure somewhere out there somebody might find it helpful.  I'll work on improving it so feedback is appreciated.

Enjoy!

Jim

Code: [Select]

//
// Quick stab at a roll/pitch/yaw indicator using quaternions
//
// Jim Bourke 2/6/2013 (RCGroups.com)
//
#include <Wire.h>

#define  CTRL_REG1  0x20
#define  CTRL_REG2  0x21
#define  CTRL_REG3  0x22
#define  CTRL_REG4  0x23
#define  CTRL_REG5  0x24
#define  CTRL_REG6  0x25

int gyroI2CAddr=105;

int gyroRaw[3];                         // raw sensor data, each axis, pretty useless really but here it is.
double gyroDPS[3];                      // gyro degrees per second, each axis

float heading[3]={0.0f};                // heading[x], heading[y], heading [z]
float quaternion[4]={1.0f,0.0f,0.0f,0.0f};
float euler[3]={0.0f};

int gyroZeroRate[3];                    // Calibration data.  Needed because the sensor does center at zero, but rather always reports a small amount of rotation on each axis.
int gyroThreshold[3];                   // Raw rate change data less than the statistically derived threshold is discarded.

#define  NUM_GYRO_SAMPLES  50           // As recommended in STMicro doc
#define  GYRO_SIGMA_MULTIPLE  0         // As recommended

float dpsPerDigit=.00875f;              // for conversion to degrees per second

void setup() {
  Serial.begin(115200);
  Wire.begin();
  setupGyro();
  calibrateGyro();
}

void loop() {
  updateGyroValues();
  updateHeadings();
  //testCalibration();

//  printDPS();
// Serial.print("   -->   ");
// printHeadings();
// Serial.println();
 
  printQuaternion();
  Serial.print("   -->   ");
  printEuler();
  Serial.println();
}

void printQuaternion()
{
  Serial.print("Quat W: ");
  Serial.print(quaternion[0]);
  Serial.print("  Quat X: ");
  Serial.print(quaternion[1]);
  Serial.print("  Quat Y: ");
  Serial.print(quaternion[2]);
  Serial.print("  Quat Z: ");
  Serial.print(quaternion[3]);
}

void printEuler()
{
  Serial.print("Euler R: ");
  Serial.print(euler[0]);
  Serial.print("  P: ");
  Serial.print(euler[1]);
  Serial.print("  Y: ");
  Serial.print(euler[2]);
}

void printDPS()
{
  Serial.print("DPS X: ");
  Serial.print(gyroDPS[0]);
  Serial.print("  Y: ");
  Serial.print(gyroDPS[1]);
  Serial.print("  Z: ");
  Serial.print(gyroDPS[2]);
}

void printHeadings()
{
  Serial.print("Heading X: ");
  Serial.print(heading[0]);
  Serial.print("  Y: ");
  Serial.print(heading[1]);
  Serial.print("  Z: ");
  Serial.print(heading[2]);
}

float safe_asin(float v)
{
        if (isnan(v)) {
                return 0;
        }
        if (v >= 1.0) {
                return PI/2;
        }
        if (v <= -1.0) {
                return -PI/2;
        }
        return asin(v);
}

void updateHeadings()
{
   
  float deltaT=getDeltaTMicros();

  double gyroDPSDelta[3];

  for (int j=0;j<3;j++)
    {
      gyroDPSDelta[j]=(gyroDPS[j]*deltaT)/1000000.0;
      heading[j] -= gyroDPSDelta[j];
    }
   
  // get radians per second
  // z is yaw
  // y is pitch
  // x is bank
  double rps[3]={0.0};
  for (int j=0;j<3;j++)
    rps[j]=(gyroDPSDelta[j]/57.2957795);
 
  // convert the radians per second to a quaternion
  double c1 = cos(rps[0]/2);
  double s1 = sin(rps[0]/2);
  double c2 = cos(rps[1]/2);
  double s2 = sin(rps[1]/2);
  double c3 = cos(rps[2]/2);
  double s3 = sin(rps[2]/2);
  double c1c2 = c1*c2;
  double s1s2 = s1*s2;
  double quatDelta[4];
  quatDelta[0] =c1c2*c3 - s1s2*s3;
  quatDelta[1] =c1c2*s3 + s1s2*c3;
  quatDelta[2] =s1*c2*c3 + c1*s2*s3;
  quatDelta[3] =c1*s2*c3 - s1*c2*s3;
 
  // now we just multiply quaternion by quatDelta
  float newQuat[4];
  newQuat[0]=(quaternion[0]*quatDelta[0] - quaternion[1]*quatDelta[1] - quaternion[2]*quatDelta[2] - quaternion[3]*quatDelta[3]);
  newQuat[1]=(quaternion[0]*quatDelta[1] + quaternion[1]*quatDelta[0] + quaternion[2]*quatDelta[3] - quaternion[3]*quatDelta[2]);
  newQuat[2]=(quaternion[0]*quatDelta[2] - quaternion[1]*quatDelta[3] + quaternion[2]*quatDelta[0] + quaternion[3]*quatDelta[1]);
  newQuat[3]=(quaternion[0]*quatDelta[3] + quaternion[1]*quatDelta[2] - quaternion[2]*quatDelta[1] + quaternion[3]*quatDelta[0]);
  for (int j=0;j<4;j++)
    quaternion[j]=newQuat[j];
   
  // Normalize quaternion
  float magnitude=sqrt(quaternion[0]*quaternion[0] + quaternion[1]*quaternion[1]+quaternion[2]*quaternion[2]+quaternion[3]*quaternion[3]);
  if (magnitude > 50.0)
  {
    Serial.println("Normalizing");
    for (int j=0;j<4;j++)
      quaternion[j]=quaternion[j]/magnitude;
  }
 
  // Convert back to euler angles
  euler[2]=atan2(2.0*(quaternion[0]*quaternion[1]+quaternion[2]*quaternion[3]), 1-2.0*(quaternion[1]*quaternion[1]+quaternion[2]*quaternion[2]));
  euler[1]=safe_asin(2.0*(quaternion[0]*quaternion[2] - quaternion[3]*quaternion[1]));
  euler[0]=atan2(2.0*(quaternion[0]*quaternion[3]+quaternion[1]*quaternion[2]), 1-2.0*(quaternion[2]*quaternion[2]+quaternion[3]*quaternion[3]));

  // convert back to degrees
  for (int j=0;j<3;j++)
    euler[j]=euler[j] * 57.2957795;
}

// this simply returns the elapsed time since the last call.
unsigned long getDeltaTMicros()
{
  static unsigned long lastTime=0;
 
  unsigned long currentTime=micros();
 
  unsigned long deltaT=currentTime-lastTime;
  if (deltaT < 0.0)
     deltaT=currentTime+(4294967295-lastTime);
   
  lastTime=currentTime;
 
  return deltaT;
}

// I called this from the loop function to see what the right values were for the calibration constants.
// If you are trying to reduce the amount of time needed for calibration just try not to go so low that consecutive
// calibration calls give you completely unrelated data.  Some sensors are probably better than others.
void testCalibration()
{
  calibrateGyro();
  for (int j=0;j<3;j++)
  {
    Serial.print(gyroZeroRate[j]);
    Serial.print("  ");
    Serial.print(gyroThreshold[j]);
    Serial.print("  "); 
  }
  Serial.println();
  return;
}

// The settings here will suffice unless you want to use the interrupt feature.
void setupGyro()
{
  gyroWriteI2C(CTRL_REG1, 0x1F);        // Turn on all axes, disable power down
  gyroWriteI2C(CTRL_REG3, 0x08);        // Enable control ready signal
  setGyroSensitivity500();

  delay(100);
}

// Call this at start up.  It's critical that your device is completely stationary during calibration.
// The sensor needs recalibration after lots of movement, lots of idle time, temperature changes, etc, so try to work that in to your design.
void calibrateGyro()
{
  long int gyroSums[3]={0};
  long int gyroSigma[3]={0};

  for (int i=0;i<NUM_GYRO_SAMPLES;i++)
  {
    updateGyroValues();
    for (int j=0;j<3;j++)
    {
      gyroSums[j]+=gyroRaw[j];
      gyroSigma[j]+=gyroRaw[j]*gyroRaw[j];
    }
  }
  for (int j=0;j<3;j++)
  {
    int averageRate=gyroSums[j]/NUM_GYRO_SAMPLES;
   
    // Per STM docs, we store the average of the samples for each axis and subtract them when we use the data.
    gyroZeroRate[j]=averageRate;
   
    // Per STM docs, we create a threshold for each axis based on the standard deviation of the samples times 3.
    gyroThreshold[j]=sqrt((double(gyroSigma[j]) / NUM_GYRO_SAMPLES) - (averageRate * averageRate)) * GYRO_SIGMA_MULTIPLE;   
  }
}

void updateGyroValues() {

  while (!(gyroReadI2C(0x27) & B00001000)){}      // Without this line you will get bad data occasionally
 
  //if (gyroReadI2C(0x27) & B01000000)
  //  Serial.println("Data missed!  Consider using an interrupt");
   
  int reg=0x28;
  for (int j=0;j<3;j++)
  {
    gyroRaw[j]=(gyroReadI2C(reg) | (gyroReadI2C(reg+1)<<8));
    reg+=2;
  }

  int deltaGyro[3];
  for (int j=0;j<3;j++)
  {
    deltaGyro[j]=gyroRaw[j]-gyroZeroRate[j];      // Use the calibration data to modify the sensor value.
    if (abs(deltaGyro[j]) < gyroThreshold[j])
      deltaGyro[j]=0;
    gyroDPS[j]= dpsPerDigit * deltaGyro[j];      // Multiply the sensor value by the sensitivity factor to get degrees per second.
  }
}

void setGyroSensitivity250(void)
{
  dpsPerDigit=.00875f;
  gyroWriteI2C(CTRL_REG4, 0x80);        // Set scale (250 deg/sec)
}

void setGyroSensitivity500(void)
{
  dpsPerDigit=.0175f;
  gyroWriteI2C(CTRL_REG4, 0x90);        // Set scale (500 deg/sec)
}

void setGyroSensitivity2000(void)
{
  dpsPerDigit=.07f;
  gyroWriteI2C(CTRL_REG4,0xA0);
}

int gyroReadI2C (byte regAddr) {
  Wire.beginTransmission(gyroI2CAddr);
  Wire.write(regAddr);
  Wire.endTransmission();
  Wire.requestFrom(gyroI2CAddr, 1);
  while(!Wire.available()) {};
  return (Wire.read());
}

int gyroWriteI2C( byte regAddr, byte val){
  Wire.beginTransmission(gyroI2CAddr);
  Wire.write(regAddr);
  Wire.write(val);
  Wire.endTransmission();
}

michinyon

Not sure that will work,    the change in your orientation quaternion depends not only on the turn rate
but also on the (constant or varying) algorithm update cycle time.

Edited:  it's OK,  you did that bit somewhere else.

michinyon

Now when I compare your calculation to mine,    and I compare to the application of three
separate axis rotations ,   I get different answers.

I have a similar expression to your "quatdelta",   but mine has extra terms in it.   Your calculation
would appear to only be correct if the object is aligned to the axes originally.

michinyon

Actually,  I think your calculation is wrong.   I think you have missed out a whole layer
of the quaternion calculation near "quatdelta".   I don't think this can be explained by
you having used a different axis paradigm.

If your vehicle has an initial orientation with yaw=+90 degrees,  roll=0,  pitch=0,  then
the orientation quaternion is (0.707, 0, 0, 0.707 )    where there is actually more decimal
places,  the number happens to be sqrt(2).

If your gyro is detecting 0 yaw rate,  1 deg/sec pitch rate,  and 0 roll rate,   and your time
step is one second,    then after 1 second has passed,   your yaw position should still be 90,
and your pitch should be +1 degree,  and your roll position should be zero.

The variables c1,c2 and c3 in your program will be   1,  0.99996123,  1
and your s1,s2,s3  will be   0,   0.00872,  0

and your quatdelta will be (  0.99996, 0, 0, 0.00872 )   and your updated orientation
quaternion will be  ( 0.7009, 0, 0, 0.7132 )    which corresponds to a yaw of 91 degrees
and a roll and pitch of zero,   which is not what  the gyro rate is telling you.

In my algorithm,  the calculation of quatdelta includes terms which come from the current
orientation.  You have left these out.

Now maybe your algorithm will work in some alternate paradigm where the aircraft is stationary and
the whole universe is considered to be spinning around it,   but I don't think so.   Even in that
paradigm,  if only your pitch gyro is showing any movement,   then the pitch changes, not the yaw.






bytedisorder

I'm looking at using this in a walking robot but completely out of my depth on how to handle it. The first piece of code at a glance look like it will work expcluding the fact that the x/y/z axis remain the same if the robot turns. In other words what is the x-axis when the robot it waling forward becomes the y axis when it turns. I want the robot to be able to recover from a knockdown using this data and not quite sure on how to do it?
Looking to host an Arduino related site? Check us out:

http://www.bitronictech.net/

acannady

Hello jtbourke,

So I'm thinking of using this code in a quadcopter project I have going and I was wondering if you have addressed the issues brought up in this topic? I haven't gone through your code yet, I'll do that now, I just wanted to see if you had a updated version of it and wanted to help a guy out.

Thanks

Go Up