问题描述
为什么我的陀螺仪mpu6050 GY521的Z轴(偏航)不能正确输出经过Kalman滤波的数据?我从库中获取了此草图,并对其进行了调整以添加过滤后的Z数据,因为原始数据只有X和Y轴。
我不确定我哪里出错了,请有人帮我找出要纠正或忽略的地方?
#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#define RESTRICT_PITCH
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
Kalman kalmanZ;
/* IMU Data */
double accX,accY,accZ;
double gyroX,gyroY,gyroZ;
int16_t tempRaw;
double gyroXangle,gyroYangle,gyroZangle; // Angle calculate using the gyro only
double compAngleX,compAngleY,compAngleZ; // Calculated angle using a complementary filter
double kalAngleX,kalAngleY,kalAngleZ; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
int XPin=A0;
int YPin=A1;
int SPin=2;
int XVal,YVal,SVal;
int Jx,Jy,Js;
// Todo: Make calibration routine
void setup() {
Serial.begin(115200);
Wire.begin();
pinMode(XPin,INPUT);
pinMode(YPin,INPUT);
pinMode(SPin,INPUT);
digitalWrite(SPin,HIGH);
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_cpu / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // disable FSYNC and set 260 Hz Acc filtering,256 Hz Gyro filtering,8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
while (i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75,1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B,6));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY,accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
double yaw = atan2(accX,accY) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX,accZ) * RAD_TO_DEG;
double yaw = atan2(-accX,accY) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
kalmanZ.setAngle(yaw);
gyroXangle = roll;
gyroYangle = pitch;
gyroZangle = yaw;
compAngleX = roll;
compAngleY = pitch;
compAngleZ = yaw;
timer = micros();
}
void loop() {
// joystickread();
/* Update all the values */
while (i2cRead(0x3B,14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY,accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
double yaw = atan2(-accX,accY) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
double gyroZrate = gyroZ / 131.0;
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll,gyroXrate,dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate,so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch,gyroYrate,dt);
if (abs(kalAngleZ) > 90)
gyroZrate = -gyroZrate; // Invert rate,so it fits the restriced accelerometer reading
kalAngleZ = kalmanZ.getAngle(yaw,gyroZrate,dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch,dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate,so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll,dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleZ) > 90)
gyroZrate = -gyroZrate; // Invert rate,dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
gyroZangle += gyroZrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
if (gyroZangle < -180 || gyroZangle > 180)
gyroZangle = kalAngleZ;
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("\t");
Serial.print(accY); Serial.print("\t");
Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t");
#endif
Serial.print("Ax: "); Serial.println(accX);
Serial.print("Ay: "); Serial.println(accY);
Serial.print("Az: "); Serial.println(accZ);
// Serial.print(roll); Serial.print("\t");
// Serial.print(gyroXangle); Serial.print("\t");
// Serial.print(compAngleX); Serial.print("\t");
Serial.print("X: "); Serial.println(kalAngleX); //Serial.print("\t");
// Serial.print("\t");
// Serial.print(pitch); Serial.print("\t");
// Serial.print(gyroYangle); Serial.print("\t");
// Serial.print(compAngleY); Serial.print("\t");
Serial.print("Y: "); Serial.println(kalAngleY); //Serial.print("\t");
Serial.print("Z: "); Serial.println(kalAngleZ);
#if 0 // Set to 1 to print the temperature
Serial.print("\t");
double temperature = (double)tempRaw / 340.0 + 36.53;
Serial.print(temperature); Serial.print("\t");
#endif
//Serial.print("\r\n");
delay(2);
}
解决方法
暂无找到可以解决该程序问题的有效方法,小编努力寻找整理中!
如果你已经找到好的解决方法,欢迎将解决方案带上本链接一起发送给小编。
小编邮箱:dio#foxmail.com (将#修改为@)