问题描述
我有一个奇怪的情况，使用您的库读取摩托车上的倾角。
下面我在同一时间在道路的同一部分进行了相同的实验，红色是使用 HyperIMU 应用程序的智能手机，蓝色是连接到 GY-521 mpu6050 板的 Arduino Beetle。
正如你所看到的，当我转弯时，以正常速度行驶时，红色曲线非常水平，这意味着我保持与我所做的相同的倾斜角。
但奇怪的是，我在 mpu6050 上得到的测量值似乎略微漂移为零。
事实上，当我移动到桌子上的黑板时，一切都很好。这个问题只发生在我在我的摩托车上使用它时，我猜这可能是由转弯过程中的“高”重力引起的，这可能导致防漂移功能使陀螺仪读数过载。
如您所见，问题在转弯期间引起的偏移或多或少会在下一个转弯 (h) 中找到。
您遇到过这样的问题吗？ 如果是，你是怎么解决的？
我的代码如下：
/**
* From https://github.com/jrowberg/i2cdevlib/issues/441
*/
#include <math.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "mpu6050_6Axis_MotionApps20.h"
#include <filters.h>
#include <Servo.h>
// ---------------------------------------------------------------------------
#define YAW 0
#define PITCH 1
#define ROLL 2
#define G (double)9.80665
// --------------------- mpu6050 variables ------------------------------------
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
mpu6050 mpu;
// mpu control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from mpu
uint8_t devstatus; // return status after each device operation (0 = success,!0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// Orientation/motion vars
Quaternion q; // [w,x,y,z] quaternion container
VectorFloat gravity; // [x,z] gravity vector
float ypr[3]; // [yaw,pitch,roll] yaw/pitch/roll container and gravity vector
int8_t logPitch[1400];
//int8_t logAZ[700];
int logIdx = 0;
unsigned long logTime;
// ---------------------------------------------------------------------------
#define LED_PIN 13
#define OFFSET (90)
/* The following values depends on the mpu6050 and must be defined for each chip.*/
// mpu moto
#define XACCEL_OFFSET 1921
#define YACCEL_OFFSET 1713
#define ZACCEL_OFFSET 1117
#define XGYRO_OFFSET 102
#define YGYRO_OFFSET -166
#define ZGYRO_OFFSET -200
/*/
// mpu DFRobot
#define XACCEL_OFFSET -1331
#define YACCEL_OFFSET 535
#define ZACCEL_OFFSET 883
#define XGYRO_OFFSET 45
#define YGYRO_OFFSET 29
#define ZGYRO_OFFSET 89
/*/
bool blinkState = false;
Servo servo;
int16_t ms;
int8_t global_fifo_count = 0; //made global so can monitor from outside GetIMUheadingDeg() fcn
int8_t rollSide = 1,blinkDiv;
float angle,ay,az;
const float cutoff_freq = 7.0; //Cutoff frequency in Hz
const float sampling_time = 0.005; //Sampling time in seconds.
IIR::ORDER order = IIR::ORDER::OD2; // Order (OD1 to OD4)
Filter f_az(cutoff_freq,sampling_time,order);
Filter f_angle(cutoff_freq,order);
/**
Setup configuration
*/
void setup() {
// Serial.begin(115200);
// while (!Serial);
//memset(logPitch,sizeof(logPitch));
logIdx = 0;
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if cpu is 8MHz)
pinMode(LED_PIN,OUTPUT); // LED
mpu.initialize();
// Load and configure the DMP
//Serial.println(F("Initializing DMP..."));
devstatus = mpu.dmpInitialize();
// Returns 0 if it worked
if (devstatus == 0) {
// Turn on the DMP,Now that it's ready
//Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// mpu calibration: set YOUR offsets here.
mpu.setXAccelOffset(XACCEL_OFFSET);
mpu.setYAccelOffset(YACCEL_OFFSET);
mpu.setZAccelOffset(ZACCEL_OFFSET);
mpu.setXGyroOffset(XGYRO_OFFSET);
mpu.setYGyroOffset(YGYRO_OFFSET);
mpu.setZGyroOffset(ZGYRO_OFFSET);
//mpu.setFullScaleGyroRange(mpu6050_GYRO_FS_250);
mpu.setFullScaleAccelRange(mpu6050_ACCEL_FS_8);
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load Failed
// 2 = DMP configuration updates Failed
// (if it's going to break,usually the code will be 1)
Serial.print(F("DMP Initialization Failed (code "));
Serial.print(devstatus);
Serial.println(F(")"));
}
servo.attach(9);
servo.write(OFFSET);
delay(1000);
digitalWrite(LED_PIN,1);
delay(10);
digitalWrite(LED_PIN,0);
delay(50);
digitalWrite(LED_PIN,0);
delay(2000);
logTime = millis() + 50;
//Serial.println();
}
int n = 10;
/**
Main program loop
*/
void loop() {
int8_t i;
if (Serial.available()) {
int i;
digitalWrite(LED_PIN,1);
delay(1000);
for (i = 0; i < sizeof(logPitch); i++) {
Serial.println(logPitch[i]);
//Serial.print("\t");
//Serial.println(logAZ[i]);
delay(1);
}
digitalWrite(LED_PIN,0);
delay(10000);
mpu.resetFIFO();
}
// if programming Failed,don't try to do anything
if (!dmpReady) {
Serial.println(F("DMP Initialization Failed "));
return;
}
static unsigned long mpuSampletime;
if ((millis() - mpuSampletime) >= 9) { // After FIFO read no new data will be there for at least 9 ms
mpuSampletime = millis();
if (mpu.dmpPacketAvailable())
{
//retreive the most current yaw value from IMU
angle = (f_angle.filterIn(GetIMUheadingDeg()) * 180.0) / M_PI;
//az = f_az.filterIn((float)mpu.getaccelerationZ() / 4096.0 * G);
/*
Serial.print(-90);
Serial.print("\t");
Serial.print(90);
Serial.print("\t");
//Serial.print(millis());
//Serial.print("\t");
Serial.print(ay);
Serial.print("\t");
Serial.print(az);
Serial.print("\t");
Serial.print((angle * 90.0) / M_PI);
Serial.println();
*/
if ((millis() > logTime ) && (logIdx < (sizeof(logPitch) -1))) {
logPitch[logIdx] = (int8_t)angle;
//logAZ[logIdx] = (int8_t)(az * 10.0);
logIdx++;
logTime = millis() + 50;
// blink LED to indicate activity
if (!((blinkDiv++) % 5)) {
blinkState = !blinkState;
digitalWrite(LED_PIN,blinkState);
}
}
//Serial.println(angle + OFFSET);
servo.write(angle + OFFSET);
}
}
}
float GetIMUheadingDeg()
{
// At least one data packet is available
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();// get current FIFO count
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & _BV(mpu6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
//Serial.println(F("FIFO overflow!"));
// otherwise,check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & _BV(mpu6050_INTERRUPT_DMP_INT_BIT))
{
// read all available packets from FIFO
while (fifoCount >= packetSize) // Lets catch up to Now,in case someone is using the dreaded delay()!
{
mpu.getFIFOBytes(fifoBuffer,packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
}
global_fifo_count = mpu.getFIFOCount(); //should be zero here
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q,fifoBuffer);
mpu.dmpGetGravity(&gravity,&q);
mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);
}
float pitch = -ypr[PITCH];
return pitch;
}
解决方法
暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！
如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。
小编邮箱:dio#foxmail.com (将#修改为@）