Search code examples
embeddedstm32gyroscopestm32f4discovery

STM32 Gyroscope angle tracking

I'm working with a Gyroscope (L3GD20) with a 2000DPS enter image description here

Correct me if their is a mistake,

I start by reading the values High and Low for the 3 axes and concatenate them. Then I multiply every value by 0.07 to convert them into DPS.

My main goal is to track the angle over time, so I simply implemented a Timer which reads the data every dt = 10 ms to integrate ValueInDPS * 10ms, here is the code line I'm using :

angleX += (resultGyroX)*dt*0.001;   //0.001 to get dt in [seconds]

This should give us the value of the angle in [degree] am I right ? The problem is that the values I'm getting are a little bit weird, for example when I make a rotation of 90°, I get something like 70°...

Solution

  • Your method is a recipe for imprecision and accumulated error.

    • You should avoid using floating point (especially if there is no FPU), and especially also if this code is in the timer interrupt handler.
    • you should avoid unnecessarily converting to degrees/sec on every sample - that conversion is needed only for presentation, so you should perform it only when you need to need the value - internally the integrator should work in gyro sample units.

    Additionally, if you are doing floating point in both an ISR and in a normal thread and you have an FPU, you may also encounter unrelated errors, because FPU registers are not preserved and restored in an interrupt handler. All in all floating point should only be used advisedly.

    So let us assume you have a function gyroIntegrate() called precisely every 10ms:

    static int32_t ax = 0
static int32_t ay = 0
static int32_t az = 0

void gyroIntegrate( int32_t sample_x, int32_t sample_y, int32_t sample_z)
{
    ax += samplex ;
    ay += sampley ;
    az += samplez ;
}

    Not ax etc. are the integration of the raw sample values and so proportional to the angle relative to the starting position.

    To convert ax to degrees:

    degrees = ax × r-1 × s

    Where:

    • r is the gyro resolution in degrees per second (0.07)
    • s is the sample rate (100).

    Now you would do well to avoid floating point and here it is entirely unnecessary; r-1 x s is a constant (1428.571 in this case). So to read the current angle represented by the integrator, you might have a function:

    #define GYRO_SIGMA_TO_DEGREESx10 14286

void getAngleXYZ( int32_t* int32_t, int32_t* ydeg, int32_t* zdeg )
{
    *xdeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;
    *ydeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;
    *zdeg = (ax * 10) / GYRO_SIGMA_TO_DEGREESx10 ;
}

    getAngleXYZ() should be called from the application layer when you need a result - not from the integrator - you do the math at the point of need and have CPU cycles left to do more useful stuff.

    Note that in the above I have ignored the possibility of arithmetic overflow of the integrator. As it is it is good for approximately +/-1.5 million degrees +/-4175 rotations), so it may not be a problem in some applications. You could use an int64_t or if you are not interested in the number of rotations, just the absolute angle then, in the integrator:

    ax += samplex ;
ax %= GYRO_SIGMA_360 ;

    Where GYRO_SIGMA_360 equals 514286 (360 x s / r).