c - 如何正确地在 C 中整合曲线？

Question

我正在从陀螺仪 (MPU6050) 读取数据。它给了我角速度。我正在尝试整合这些数据以了解陀螺仪的角度。我这样做的方法是将读取的数据乘以耗时，并不断将这些结果相加，即积分:

但它似乎不起作用，这是我的代码和我得到的输出:

#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;
}

陀螺仪平放在桌面上对应的输出:

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

有人可以解释一下我做错了什么吗？

编辑

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;
}

对应输出:

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

代码至少有几个问题。

1. 代码在第一次分配之前使用 timeDifference。这导致 UB。

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

2. 错误地使用了 int abs(int) 而不是 double fabs(double) 和错误的常量 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);
   

3. 编辑版本中的错误打印说明符。这也意味着 OP 没有使用完全启用警告的编译器。最好启用它们以节省调试时间。

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

4. 微妙偏差的候选来源:首先，我不确定代码是否正确地将 2 字节输入转换为 short gyroX 给定字节顺序问题 @Lundin ，但让我们假设它是正确的。发布许多示例 gyroX 会很有用。一个问题是 A/D 转换偏差。根据 A/D 的配置方式，读数可能代表的不是最接近的读数，而是平均偏差为 +1/2 最低有效位的floored 读数。 #define A_SCALE (16384.0) 表示转换为 14 位，因此读数在 2¹⁴ 中有 0.5 个偏差。 OP 可能想要微调 BIAS 以免持续整合读数的偏差。

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

5. 我建议将 double 与 gyro*angle 一起使用，因为这些变量进行综合计算，对累积误差敏感。

---

代码的时间差计算可能会被简化。请注意，第一个速度测量值被忽略了——因为它应该处理问题 #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);