How to integrate a curve into C?

Question

I am reading data from a gyroscope (MPU6050). This gives me angular speed. I am trying to integrate this data to know the angle of the gyro. The way I do this is to multiply the read data with the elapsed time and continuously add these results, i.e. integration:

But it does not 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: %fx angle: %f \n", gyroX/ANG_SCALE, gyroXangle); sleep(1); } } return 0; } 

the corresponding output when the gyroscope simply lays 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 

Can someone explain to me what I can do wrong?

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: %fx 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 

Answer 1

There are at least a few problems in the code.

• The code uses timeDifference before it is assigned for 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 the 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); 

• Invalid print specifier in the edited version. This also implies that the OP does not use the compiler with warnings enabled. It’s best that they can save debugging time.

   double timeDifference; ... // printf("timeDifference: %d\n", timeDifference); printf("timeDifference: %e\n", timeDifference); 

• The source of the candidate for subtle bias: firstly, I'm not sure that the code will convert 2-byte input to short gyroX correctly defined short gyroX endian problems, but let's assume that this is correct. It would be helpful to post many gyroX examples. The problem is offset A / D conversion. Depending on how A / D is configured, the reading may not be the closest possible reading, but a reading of half with an average offset of the least significant bit +1/2. #define A_SCALE (16384.0) implies that the conversion is 14 bits, and therefore the reading is offset by 0.5 parts in 2 ¹⁴ . The OP may want to fine-tune BIAS to not constantly integrate the bias.

   #define BIAS (0.5 * 65536.0 / A_SCALE) gyroXangle += (((gyroX + BIAS)/ANG_SCALE) * timeDifference); 

• I recommend using double with gyro*angle , as these variables contain an integrated calculation that is sensitive to accumulation errors.

---

The calculation of the time difference can be simplified. Please note that the first measurement of speed is ignored - as it should take care of problem No. 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); 

Answer 2

The answer to chux of answering questions with numbers and coding / coding is no longer worth mentioning them again, but I just need to add:

I do not see the definition of any coordinate system in the OP. I assume that your gyroscope is on some kind of mobile device (it may be unsteady), so you need to consider this as well. I expect the gyroscope to return the values ​​to its local coordinate system, otherwise it will need some reference frame based on different sensory data, which I do not see anywhere else. So what to do:

• add a representation of the local coordinate system

  Ideal for this are transformation matrices or basis vectors (reper) for more information:

  • Understanding 4x4 homogeneous transformation matrices ;

• Init

  You need to initialize / calibrate some starting point of your application. For example, place your sensor in the indicated orientation and press the button / key to tell your program to reset. At this point, run your local coordinate system (for example, in the matrix unit). This can be done once or once in a while to improve measurement accuracy in long term terminology.

• implement integration in a local coordinate system

  So instead of integrating

   gyroXangle += ((gyroX/ANG_SCALE) * timeDifference); 

  You rotate the coordinate system matrix. Then you simply calculate the Euler angles from the resulting matrix basis vector.

You might want to see the associated QA :

• Algorithm for calculating the position of a smartphone - GPS and sensors