如何在C中正确整合曲线？
我正在从陀螺仪(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
代码至少有一些问题。
timeDifference代码在第一次分配之前正在使用。这导致了UB。
Run Code Online (Sandbox Code Playgroud)if(i>=1) { timeDifference = ... } i++; // First time through the loop not yet defined/assigned // vvvvvvvvvvvvvv gyroXangle = ((gyroX/ANG_SCALE) * timeDifference);不正确使用
int abs(int)代替double fabs(double)和错误常量10.e6。 @平古尔
Run Code Online (Sandbox Code Playgroud)// 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);编辑版本中的打印说明符错误。这也意味着 OP 没有使用完全启用警告的编译器。最好让他们节省调试时间。
Run Code Online (Sandbox Code Playgroud)double timeDifference; ... // printf("timeDifference: %d\n", timeDifference); printf("timeDifference: %e\n", timeDifference);微妙偏差的候选来源：首先，我不自信的代码正在将 2 字节输入转换为
short gyroX正确给定的字节序问题@Lundin，但让我们假设它是正确的。发布许多示例gyroX会很有用。一个问题是 A/D 转换偏差。根据 A/D 的配置方式，读数可能代表的不是最接近的可能读数，而是平均偏差为 +1/2 最低有效位的 底读数。#define A_SCALE (16384.0)意味着转换为 14 位，因此读数在 2 14中偏差了 0.5 部分。OP 可能需要进行微调BIAS，以免不断整合读数的偏差。
Run Code Online (Sandbox Code Playgroud)#define BIAS (0.5 * 65536.0 / A_SCALE) gyroXangle += (((gyroX + BIAS)/ANG_SCALE) * timeDifference);我建议使用
doublewithgyro*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);