How to correctly integrate a curve in C?

Question

I am reading data from a gyroscope (MPU6050). It gives me the angular velocity. I am trying to integrate this data in order to know the angle of the gyroscope. The way I do this is by multiplying the read data with the elapsed time and continuously adding up those results, ie integrating:

But it doesn't seem to work, this is my code and the output I get:

#include <stdio.h>
#include <stdint.h>
#include <time.h>
#include <sys/types.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

#define A_SCALE (16384.0)
#define ANG_SCALE (131.0)

int main(int argc, char *argv[])
{
    int fd;
    int data;
    int i=0;

    float gyroXangle=0;
    float gyroYangle=0;
    float gyroZangle=0;
    float gyroOffset = 151;
    float gyroScale = 0.02;

    struct timeval startTime, stopTime;
    long timeDifference;
    int i=0;

    wiringPiSetup();
    fd = wiringPiI2CSetup(0x68);
    wiringPiI2CWriteReg8(fd, 0x6b, 0);

    if(fd==-1)
    {
        printf("can't setup device\n");
        return -1;
    }
    else
    {

        printf("successfully setup device\n");

        while(1) {
            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+8);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+9);
            short gyroX = msb << 8 | lsb;

            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+10);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+11);
            short gyroY = msb << 8 | lsb;

            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+12);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+13);
            short gyroZ = msb << 8 | lsb;

           //to know elapsed time between two successive readings
            if(i%2==0) {
                gettimeofday(&startTime, NULL);
            }
            else {
                gettimeofday(&stopTime, NULL);
            }

            if(i>=1)
            {
                timeDifference = abs((int)(stopTime.tv_sec - startTime.tv_sec)*1000000 + (stopTime.tv_usec - startTime.tv_usec));
                    printf("timeDifference: %d\n", timeDifference);
            }

            i++;
            gyroXangle = ((gyroX/ANG_SCALE) * timeDifference);
            printf("gyro x: %f x angle: %f \n", gyroX/ANG_SCALE, gyroXangle);
            sleep(1);
        }
    } 

    return 0;
}

corresponding output while the gyroscope is just laying on the table:

gyro x: -0.442748 x angle: -0.000000 
timeDifference: 1006761
gyro x: -0.389313 x angle: -391945.125000 
timeDifference: 1006744
gyro x: -0.389313 x angle: -391938.500000 
timeDifference: 1006755
gyro x: -0.419847 x angle: -422683.406250 
timeDifference: 1006731
gyro x: -0.351145 x angle: -353508.593750 
timeDifference: 1006861
gyro x: -0.267176 x angle: -269008.656250 
timeDifference: 1006716
gyro x: -0.343511 x angle: -345818.468750

Could someone explain me what I may be doing incorrectly?

EDIT

updated code: 
#include <stdio.h>
#include <stdint.h>
#include <time.h>
#include <sys/types.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

#define MPU6050_REG_DATA_START 0x3b
#define A_SCALE (16384.0)
#define ANG_SCALE (131.0)

int main(int argc, char *argv[])
{
    int fd;
    int data;
    int i=0;

    float gyroXangle=0;
    float gyroYangle=0;
    float gyroZangle=0;
    float gyroOffset = 151;
    float gyroScale = 0.02;

    struct timeval startTime, stopTime;
    double timeDifference;

    wiringPiSetup();
    fd = wiringPiI2CSetup(0x68);
    wiringPiI2CWriteReg8(fd, 0x6b, 0);

    if(fd==-1)
    {
        printf("can't setup device\n");
        return -1;
    }
    else
    {
        printf("successfully setup device\n");

        while(1) {
            //start_time = gettime_now.tv_nsec;

            uint8_t msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+8);
            uint8_t lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+9);
            short gyroX = msb << 8 | lsb;

            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+10);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+11);
            short gyroY = msb << 8 | lsb;

            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+12);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+13);
            short gyroZ = msb << 8 | lsb;

            if(i%2==0){
                gettimeofday(&startTime, NULL);
            }
            else{
                gettimeofday(&stopTime, NULL);
            }

            if(i>=1)
            {
                timeDifference = abs((stopTime.tv_sec - startTime.tv_sec)+ (stopTime.tv_usec - startTime.tv_usec)/10.0e6);
                    printf("timeDifference: %d\n", timeDifference);
            }

            i++;
            gyroXangle += ((gyroX/ANG_SCALE) * timeDifference);
            printf("gyro x: %f x angle: %f \n", gyroX/ANG_SCALE, gyroXangle);
            //sleep(1);
        }
    } 

    return 0;
}

corresponding output:

gyro x: -0.442748 x angle: 0
timeDifference: 0
gyro x: -0.389313 x angle: 0
timeDifference: 0
gyro x: -0.389313 x angle: 0
timeDifference: 0
gyro x: -0.419847 x angle: 0
timeDifference: 0
gyro x: -0.351145 x angle: 0
timeDifference: 0
gyro x: -0.267176 x angle: 0
timeDifference: 0
gyro x: -0.343511 x angle: 0

It's not obvious from the output that you are doing anything wrong. How many degrees does an angle of -400000 correspond to? — David Grayson, Apr 20 '17 at 06:37
Don't experiment in an active earth quake region? Sorry, had to. — grek40, Apr 20 '17 at 06:37
it looks like you're experiencing drift, amplified by the large values for your elapsed time. — Andreas Grapentin, Apr 20 '17 at 06:38
@DavidGrayson my gyroscope is just laying on a table and nobody is touching it... — , Apr 20 '17 at 06:42
I'd say sampling more often would be a good thing. I think a phone would sample at least once a second, if not more often? — Paul Stelian, Apr 20 '17 at 06:45
@PaulStelian I perfectly understand the DSP theory behing "sample" more often. But how can I do that in "real life"? I removed the sleep(1), but that didn't solve the issue. — , Apr 20 '17 at 06:47
You seem to be converting seconds -> μs and then integrating. This will blow up your values like crazy. Instead, convert μs to seconds, and the integral will be lowered by a factor of 1e6. — pingul, Apr 20 '17 at 06:49
Also, I don't see any accumulation of data. Shouldn't it be `gyroXangle += (... * timeDifference);`? — pingul, Apr 20 '17 at 06:52
@pingul ok this is what I now have "timeDifference = abs((int)(stopTime.tv_sec - startTime.tv_sec) + (stopTime.tv_usec - startTime.tv_usec)/1000000);". but now the amount of degrees is always zero no matter how the gyroscope is oriented. — , Apr 20 '17 at 06:52
You need to convert to `double`. Make `double timeDifference;`, and divide μs by `1.0e6` and it should work. — pingul, Apr 20 '17 at 06:54
Yes I removed the "+=" because intuitively it does absolutely make sense to do that. But my value just keeps increasing when my gyro is just laying on a table, ie it doesn't show +/- 0 degrees it just keeps increasing — , Apr 20 '17 at 06:54
@pingul updated my code according to your suggestions, but that doesn't work: https://pastebin.com/EF9PUZel , this code just gives an angle of zero all the time no matter what — , Apr 20 '17 at 07:00
Why are you using wall clock time instead of high-resolution timer? Like QueryPerformanceCounter() in Windows or its Linux analog? — Pavel Zhuravlev, Apr 20 '17 at 07:01
@PavelZhuravlev because I didn't think that would be an issue and I don't know hot to do that in Linux. — , Apr 20 '17 at 07:05
It may be an issue if you need precision time measurement (and you do need it here). Try this: http://stackoverflow.com/questions/2657579/c-linux-equivalent-of-windows-queryperformancecounter — Pavel Zhuravlev, Apr 20 '17 at 07:07
You should divide by `1.0e6` and not `10.0e6`. The 0 you get is probably due to precision of your printout, or something is not being correctly converted (doing your `int` conversion on the same line is counter productive, for example). It should also be `fabs` if I'm not mistaken. — pingul, Apr 20 '17 at 07:08
And, as was correctly suggested above, gyros can have a drift. A part I've experimented with several years ago was showing constant rotation arount its axis while standing still. You need to find this false offset somehow and add it to data coming from the part. — Pavel Zhuravlev, Apr 20 '17 at 07:09
@PavelZhuravlev did what you said: https://pastebin.com/jTN85Z70 But this is the result: https://pastebin.com/2zpct5zZ The angle is 0 no matter what... — , Apr 20 '17 at 07:20
As I mentioned before: you need to use `fabs`. `abs` converts the result to an `int`, which will most likely result in 0 for your current cases. — pingul, Apr 20 '17 at 07:24
@pingul there are two issues when I do that: I get the warning "incompatible implicit declaration of built-in function fabs" and the angle just keeps increasing all the time: https://pastebin.com/QNWUCmFP Note that this is just a snippet of the output the values never stop increasing... — , Apr 20 '17 at 07:29
You need to `#include `. And, from Pavel: _And, as was correctly suggested above, gyros can have a drift. ... You need to find this false offset somehow and add it to data coming from the part._ — pingul, Apr 20 '17 at 08:14
to avoid drift is impossible - you'd need a perfect gyro and infinitely fast sample rate. You simply can not rely on accurate position data when measuring acceleration. — Andreas Grapentin, Apr 20 '17 at 09:08
What is the endianess of the gyro input? Is the data 2's complement? If not, why are you using 2's complement format `short`? What is the endianess of the CPU? What kind of CPU is it, 8 bit, 32 bit? — Lundin, Apr 20 '17 at 09:17
@Lundin i don't know the endianess of the gyro and the CPU itself, I use the MPU6050 module. The datasheet also doesn't tell whether data is stored using 2's complement. I use a raspberry pi. — , Apr 20 '17 at 09:22
@Lundin this is what the code currently looks like: https://pastebin.com/HVdstis1 And its results: https://pastebin.com/eh5VQQq6 — , Apr 20 '17 at 09:23
@traducerad If you don't even know the endianess of the MCU you are programming form, then any hardware-related programming is a lost cause (I would guess Raspberry PI means ARM which is little endian by default. You, the programmer, _must_ know these utter basics of the system!) If you don't know all the things mentioned, then there is no way you can write this program and turn the code into something that makes sense. Your current code writes data into the sign bits of the short and seems to assume big endian 2's complement both from the gyro and mcu. — Lundin, Apr 20 '17 at 09:28
@Lundin no need to overreact I am just a student trying to figure things out... This is code that I found for the rpi on github (reading the sensor and storing the results in two int8_t vars): https://github.com/nkolban/PiBook/blob/master/C%20WiringPi%20MPU-6050/mpu6050.c. I didn't change anything to it, the only thing I am trying to do is converting the data to degrees. Are you sure about what you are saying? — , Apr 20 '17 at 09:30
Quick Google for the manual revealed the [MPU-6000 and MPU-6050 Register Map and Descriptions](https://www.invensense.com/wp-content/uploads/.../MPU-6000-Register-Map1.pdf) document. In which it mentions that bit order for the 16 bit values is MSB in the first byte, aka Big Endian. The same document also mentions that the format is 2's complement. You appear to be using a 32 bit ARM processor (?) which likely means Little Endian - it is the default setting. — Lundin, Apr 20 '17 at 09:39
So a nit-pick is that you shouldn't be left shifting data into the sign bits of a `short`, that's undefined behavior. Use an `uint16_t` first to set the bytes, then cast it to a `int16_t` afterwards. — Lundin, Apr 20 '17 at 09:40
@Lundin yes it is a 32 bit processor. I changed it as you recommended: https://pastebin.com/KD0f8phv unfortunately that didn't change anything, results are still the same. — , Apr 20 '17 at 09:51
What values are you reading as msb and lsb then? Do they make any sense? Are you sure this is a programming problem and not related to the I2C communication? — Lundin, Apr 20 '17 at 10:04
You said "my gyroscope is just laying on a table and nobody is touching it". That is irrelevant. Gyros always have a little bit of noise and offset in their readings even if they are sitting still. So when you want to investigate gyro readings, it's important to use real-world units instead of assuming you will get a perfect 0. — David Grayson, Apr 20 '17 at 16:49

score 2 · Answer 1 · edited May 23 '17 at 12:09

Code has at least a few issue.

Code is using timeDifference before it is assigned the first time. This leads to UB.

    if(i>=1) {
        timeDifference = ...
    }
    i++;
    //  First time through the loop  not yet defined/assigned
    //                                vvvvvvvvvvvvvv  
    gyroXangle = ((gyroX/ANG_SCALE) * timeDifference);

Incorrect use of int abs(int) instead of double fabs(double) and wrong constant 10.e6. @pingul

// timeDifference = abs((stopTime.tv_sec - startTime.tv_sec) + 
//    (stopTime.tv_usec - startTime.tv_usec)/10.0e6);
timeDifference = fabs((stopTime.tv_sec - startTime.tv_sec) + 
    (stopTime.tv_usec - startTime.tv_usec)/1.0e6);

Wrong print specifier in edited version. This also implies OP is not using a compiler with warnings fully enabled. Best to enable them to save debug time.
```
double timeDifference;
...
// printf("timeDifference: %d\n", timeDifference);
printf("timeDifference: %e\n", timeDifference);
```
Candidate source for subtle bias: First, I an not confident code is converting the 2 byte input into a short gyroX correctly given endian issues @Lundin, yet let us assume it is correct. Posting many sample gyroX would be useful. An issue is A/D conversion bias. Depending on how the A/D are configured, the reading may represent, not the closest possible reading, but a floored reading with an average bias of +1/2 least significant bit. #define A_SCALE (16384.0) implies that the conversion is 14 bit and so the reading is biased 0.5 part in 2¹⁴. OP may want to fine tune BIAS so as not to continually integrate bias of the readings.
```
#define BIAS (0.5 * 65536.0 / A_SCALE)
gyroXangle += (((gyroX + BIAS)/ANG_SCALE) * timeDifference);
```
I recommend to use double with gyro*angle as these variables are holding an integrated computation, sensitive to accumulation error.

Code's time difference calculation may be simplified. Notice that the first velocity measurement is ignored - as it should be to take care of issue #1

    struct timeval then = {0};
    bool first_time = true;
    while(1) {
      // sample data
      short gyroX  = ...  
      short gyroY  = ...  
      short gyroZ  = ...  

      struct timeval now;
      gettimeofday(&now, NULL);
      long long delta_usec = (now.tv_sec - then.tv_sec)*1000000LL + 
          (now.tv_usec - then.tv_usec);
      then = now;

      if (first_time) delta_usec = 0;
      first_time = false;    
      printf("Time Difference: %lld us\n", delta_usec);

      gyroXangle += (((gyroX + BIAS)/ANG_SCALE) * delta_usec/1.0e6);
      printf("gyro x:%e, x angle:%e\n", gyroX/ANG_SCALE, gyroXangle);

Code could eliminate computational error in `gyro?angle` variables. Use `long long gyroXangle;` and `gyroXangle += delta_usec*(gyroX + iBIAS);` entirely as integer math. Should work for 700+ days before overflow is a concern. — chux - Reinstate Monica, Apr 20 '17 at 15:50
I will implement those interesting remarks ASAP! Do you know how I could cope with gyroscope drift and include it in my/your code. — , Apr 20 '17 at 16:10
@traducerad "drift" would be the difference between this code's reported cumulative number of revolutions `gyroXangle` and the result of some other experiment that counted revolutions. How is that _other_ going to be run? What are some it its results as compared to your code results? How accurate do you need and over what time? At best, I think you can test and apply a fine tuned sub_bias offset to `BIAS`, but that will only reduced the drift error, not eliminate it. To eliminate, system needs a whole turn counter. `gyroXangle` then could provide the sub-revolution info. — chux - Reinstate Monica, Apr 20 '17 at 16:36

score 0 · Answer 2 · edited May 23 '17 at 12:02

the chux's answer cover numerical and encoding/coding issues already so no point to mention them again but I just need to add:

I do not see any coordinate system definition in the OP. I am assuming your gyroscope is at some mobile device (turnable not static) so you need to take that into account too. I expect the gyroscope is returning values in its local coordinate system otherwise it would need some reference frame based on different sensory data which I do not see anywhere. So what to do:

add local coordinate system representation

Ideal for this are transform matrices or basis vectors (reper) for more info see:
- Understanding 4x4 homogenous transform matrices;
init

You need to init/calibrate some start point of your app. For example place your sensor to specified orientation and hit some button/key to tell your program to reset. At that point init your local coordinate system matrix (for example to unit matrix). This can be done once or once in the while to improve precision of measurement in long therm basis.
implement integration in local coordinate system

So instead of integrating
```
gyroXangle += ((gyroX/ANG_SCALE) * timeDifference);
```
You rotate your coordinate system matrix. Then you just compute Euler angles from the resulting matrix forward basis vector.

You might want to see related QA:

Algorithm to Calculate Position of Smartphone - GPS and Sensors

How to correctly integrate a curve in C?

2 Answers2