Integrate readings from a 3 axis Gyroscope?

问题:

So I have written a program in C to print values of 3 axis gyro at a very high data rate. The values are instantaneous values of angular speed that rise on movement and fall if the device is still. I need it to hold the high values and come back to zero when returned to position. If I integrate the values over a fixed sample time, would I achieve this? How do I go about integrating? I am a beginner in C programming. Thank you

回答1:

Angular velocity is:

ω = Δφ/Δt = (φ_n − φ_n−1)/(t_n − t_n−1)

So the current angle can be calculated from the previous angle as:

φ_n = φ_n−1 + ω·Δt

And the corresponding C code is …

double angle = 0;  /* or any initial value */
for ( ;; ) {
    double omega = get_angular_velocity();
    angle += omega * timestep;
}

… provided that the function get_angular_velocity returns a value regularly at timestep intervals.

回答2:

First off you need to understand the physical aspect of a digital gyroscope: it suffers from drift (it won't return to 0, but that usually happens over long periods of time) and equally important it usually is biased; this is a calibration issue.

The gyro bias can be observed by reading it's axis value when it's still. If it reads anything other than 0, that's your bias. And you need to compensate for that. How do you do it? - calibration

Gyro calibration: read the axis values for a given period of time (1000 samples should do it). Average the samples to get the bias value. You have to subtract this bias value from all readings when running you code to get a valid reading. You'll know you have done it right if you read zeros while the gyro is not moving.

Next up, interpreting the gyro data: the gyro gives you angular velocity values. These are not of much use to you in their raw form; so, you integrate them over time to get angle values, but before you can do that you'll need a few things:

1. the gyro gain; this is specified in the datasheet as a value associated to it's sensitivity (i.e. GAIN = 0.07 for a sensitivity of 2000dps) - it should all be in a table in the data sheet and it depends on how the gyro is configured.
2. you need to choose a time interval over which you integrate the values, this again depends on the gyro configuration (see the dps settings) but you should be fine with 10 or 20 ms.
3. make sure you read the gyro values with a good periodicity (i.e. exactly one every 10ms)

4. write some code to put it all together- like so:

   while(1)
   {
       startInt = mymillis();// this function returns the current time in ms
   
       gyroRaw = readGyro(); 
   
       //Convert Gyro raw to degrees per second
       rate_gyro = (float) gyrRaw * GYRO_GAIN;
   
       //Calculate the angles from the gyro
       gyroAangle += rate_gyro * DT;
   
       //print the gyro angle ...or anything you find useful
   
       //Each loop should be at least 20ms.
       while(mymillis() - startInt < 20)
       {
          usleep(100);
       }
   }
   

This should get you started on the right path, I hope :)

回答3:

This is actually not much about C, but signal processing in general, so it is a good idea to read about that.

Some integration might not be wrong to remove noise. There are different algorithms you can use.

To "hold the high values", you should just think a bit what that actually means: Detecting and storing the maxima of the curve. With some discrete differentiation, you can detect spikes, e.g. due to dropping in a table. This is a wide field, just do some experiments.

A good starter to see patterns is to show them as curves, similar to an oscilloscope. If you do not want the graphical processing in C, you might pipe the values to a socket and use a high level language like Python to draw and do some processing. Just an idea!