MPU6050 FIFO溢出和凍結問題

Question

我使用傳感器 MPU6050 來控制我的機器人 arm 的運動。 當它是一個獨立的程序時，代碼可以正常工作，但是當代碼被編譯到主程序中時，我一直遇到“FIFO溢出”。 這是我正在使用的代碼。

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;

RF24 radio(9, 8);  // CE, CSN

const byte address[6] = "00001";

const int AccReadings = 10;
//Wrist Roll
int DataX[AccReadings];
int WRIndex = 0;
int WRtotal = 0;
int WRaverage = 0;
//Wrist Pitch
int DataY[AccReadings];
int WPIndex = 0;
int WPtotal = 0;
int WPaverage = 0;
//Shoulder Lift
int DataY2[AccReadings];
int SLIndex = 0;
int SLtotal = 0;
int SLaverage = 0;
//Elbow Lift
int ELaverage = 0;
//Arm Rotation
int ARaverage = 0;
float correct;

#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
bool blinkState = false;

// 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
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

struct Sensor_Data
{
  int WristRoll;
  int WristPitch;
  int ShoulderLift;
  int ElbowLift;
  int ArmRotation;
};
Sensor_Data data;

//Interrupt Detection
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
  mpuInterrupt = true;
}

void setup()
{
  radio.begin();
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();

  //Zero-fill Arrays
  for (int i = 0; i < AccReadings; i++)
  {
   DataX[i] = 0;
  }
  for (int j = 0; j < AccReadings; j++)
  {
   DataY[j] = 0;
  }
  for (int k = 0; k < AccReadings; k++)
  {
   DataY2[k] = 0;
  }

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
    Wire.setClock(400000);
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
  #endif

  Serial.begin(115200);
  while (!Serial);

  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  mpu.setXGyroOffset(49);
  mpu.setYGyroOffset(-18);
  mpu.setZGyroOffset(9);
  mpu.setZAccelOffset(4427);

  if (devStatus == 0) 
  {
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    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(")"));
  }
}

void loop()
{
  /*smoothWR();
  movementWR();
  smoothWP();
  movementWP();
  smoothSL();
  movementSL();*/
  ElbowMovement();
  radio.write(&data, sizeof(Sensor_Data));
}

void smoothWR()
{
  WRtotal = WRtotal - DataX[WRIndex];
  DataX[WRIndex] = analogRead(A0);
  WRtotal = WRtotal + DataX[WRIndex];
  WRIndex = WRIndex + 1;

  if (WRIndex >= AccReadings)
  {
    WRIndex = 0;
  }

  WRaverage = WRtotal / AccReadings;
  //Serial.println(WRaverage);
}

void movementWR()
{            
  WRaverage = map(WRaverage, 278, 419, 0, 180);
  data.WristRoll = constrain(WRaverage, 0, 180);                                      
  //Serial.println(data.WristRoll);
}

void smoothWP()
{
  WPtotal = WPtotal - DataY[WPIndex];
  DataY[WPIndex] = analogRead(A1);
  WPtotal = WPtotal + DataY[WPIndex];
  WPIndex = WPIndex + 1;

  if (WPIndex >= AccReadings)
  {
    WPIndex = 0;
  }

  WPaverage = WPtotal / AccReadings;
  //Serial.println(WPaverage);
}

void movementWP()
{            
  WPaverage = map(WPaverage, 280, 421, 0 , 135);
  data.WristPitch = constrain(WPaverage, 0, 135);                                      
  //Serial.println(data.WristPitch);
}

void smoothSL()
{
  SLtotal = SLtotal - DataY2[SLIndex];
  DataY2[SLIndex] = analogRead(A2);
  SLtotal = SLtotal + DataY2[SLIndex];
  SLIndex = SLIndex + 1;

  if (SLIndex >= AccReadings)
  {
    SLIndex = 0;
  }

  SLaverage = SLtotal / AccReadings;
  //Serial.println(SLaverage);
}

void movementSL()
{                       
  SLaverage = map(SLaverage, 410, 270, 0 , 180);
  data.ShoulderLift = constrain(SLaverage, 35, 180);                                   
  //Serial.println(data.ShoulderLift);
}

void ElbowMovement()
{
  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) 
  {
    if (mpuInterrupt && fifoCount < packetSize) 
    {
      // try to get out of the infinite loop
      fifoCount = mpu.getFIFOCount();
    }
  }

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // 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();
    fifoCount = mpu.getFIFOCount();
    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)) 
  {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    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;

    // Get Yaw, Pitch and Roll values
#ifdef OUTPUT_READABLE_YAWPITCHROLL
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    // Yaw, Pitch, Roll values - Radians to degrees
    ypr[0] = ypr[0] * 180 / M_PI;
    ypr[1] = ypr[1] * 180 / M_PI;
    ypr[2] = ypr[2] * 180 / M_PI;
    
    // Skip 300 readings (self-calibration process)
    if (int l = 0; l <= 300) {
      correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
      l++;
    }
    // After 300 readings
    else {
      ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract  the last random Yaw value from the currrent value to make the Yaw 0 degrees
      // Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
      ELaverage = map(ypr[0], -90, 90, 0, 180);
      data.ElbowLift = constrain(ELaverage, 30, 110);
      ARaverage = map(ypr[1], -90, 90, 0, 180);
      data.ArmRotation = constrain(ARaverage, 0, 180);     
      //Serial.println(data.ElbowLift);
      Serial.println(ypr[1]);
    }
#endif
  }
}

有什么辦法可以擺脫 FIFO 溢出？ 此外，當我嘗試使用 Jeff Rowberg 的示例代碼 MPU6050_DMP6 時，程序將在幾秒鍾后凍結。 有什么解決辦法嗎？ 這些是我正在使用的示例代碼。

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
float correct;
int j = 0;

#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
bool blinkState = false;

// 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
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//Interrupt Detection
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}

void setup()
{
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000);
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
  #endif
  Serial.begin(38400);
  while (!Serial);

  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  mpu.setXGyroOffset(17);
  mpu.setYGyroOffset(-69);
  mpu.setZGyroOffset(27);
  mpu.setZAccelOffset(1551);

  if (devStatus == 0) 
  {
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    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(")"));
  }
}

void loop()
{
  if (!dmpReady) return;
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
  {
    #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            if (j <= 300) 
            {
              correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
              j++;
            }
            else
            {
              ypr[0] = ypr[0] - correct;
              Serial.print("ypr\t");
              Serial.print(ypr[0] * 180/M_PI);
              Serial.print("\t");
              Serial.print(ypr[1] * 180/M_PI);
              Serial.print("\t");
              Serial.println(ypr[2] * 180/M_PI);
            }
    #endif
  }
}

Answer 1

您正在使用 DMP（數字運動處理器）並在 MPU 本身上運行平均計算，這可以提供更精確和更少的 CPU 消耗，但您需要經常更新 FIFO 或軌道出錯。

遇到“FIFO 溢出”意味着您的循環代碼太慢，您應該提高循環代碼中另一個任務的速度。 或者只是使用其他不使用 DMP 的代碼。