Go Down

Topic: **Finally!** MPU6050 FIFO DMP Quaternion code for Arduino. --made easy-- (Read 195 times) previous topic - next topic

zhomeslice

The Simple_MPU6050 library is here!
I got sick of struggling with code that wasn't clear and any configuration change messed with other things. So I started simplifying it and came up with this Simple_MPU6050 library.

Link: https://github.com/ZHomeSlice/Simple_MPU6050

This is a library that quickly and easily gives you full access to the DMP FIFO feature of the MPU6050 with  Accell, Gyro, and Quaternion values that are just handed to you when they appear 100% ready for your usage.
Features:
  • It Uses the Latest Embedded MotionDriver 6.12
  • DMP Functioning with auto calibration on (8 seconds of no motion)
  • A simple Callback function triggered when new data appears in the FIFO buffer.
  • Offset generation/creation code provided with the library for easy startup integration.
  • * FIFO overflow protection so you can use the dreaded delay() function without having to reset the FIFO buffer.
  • Smallest footprint full DMP fits into 27% or 8824 bytes on a UNO! leaving you room for your code.
  • The startup is super fast with MPU DMP and FIFO Setup completed in just 288 Milliseconds!
  • Preconfigured firmware image so you can just load and go.
  • One function for loading default configuration attributes. Once loaded you are ready to get data.
  • Every known MPU register has a specific Macro to easily know what you are changing.
  • Macro names follow the naming pattern found in MPU-6000/MPU-6050 Register Map and Descriptions  


If you need to access additional registers I've created 2 macro files MPU_ReadMacros.h MPU_WriteMacros.h that break down each of the bit, bits and byte combinations to provide easy access to these settings.
snip from MPU_WriteMacros.h
Code: [Select]
#define USER_CTRL_WRITE_DMP_EN(Data)                    MPUi2cWrite(0x6A, 1, 7, Data)  //   1  Enable DMP operation mode.
#define USER_CTRL_WRITE_FIFO_EN(Data)                   MPUi2cWrite(0x6A, 1, 6, Data)  //   1  Enable FIFO operation mode. 0  Disable FIFO access from serial interface.  To disable FIFO writes by dma, use FIFO_EN register. To disable possible FIFO writes from DMP, disable the DMP.

 Usage Esample:

Code: [Select]
USER_CTRL_WRITE_DMP_EN(1) // DMP is enabled! you didn't need to know that you changed bit 7 ONLY in register 0x6A to 1 leaving the other bits 0~6 as they were

I'm glad I can share it.

Z

Attached: Minimum needed to get going. The above-mentioned macro Files are available on my ZHomeSlice Github page

Example Sketch:
Code: [Select]
#include "Simple_MPU6050.h"
#define MPU6050_ADDRESS_AD0_LOW     0x68
#define MPU6050_ADDRESS_AD0_HIGH    0x69
#define MPU6050_DEFAULT_ADDRESS     MPU6050_ADDRESS_AD0_LOW

Simple_MPU6050 mpu;
ENABLE_MPU_OVERFLOW_PROTECTION();
/*             _________________________________________________________*/
//               X Accel  Y Accel  Z Accel   X Gyro   Y Gyro   Z Gyro
#define OFFSETS      0,       0,       0,       0,       0,       0  // it will auto calibrate for your MPU put your offsets here for faster boot
//#define CalibrationLoops 2 //(Default is 8 for good measure) 1 - 2 Calibration loops are great for a quick tuneup (at 100 readings each) , 6+ Loops will completely find the correct calibration offsets!
/*             _________________________________________________________*/

//***************************************************************************************
//******************                Print Funcitons                **********************
//***************************************************************************************
// My Print assist Macros:
#define spamtimer(t) for (static uint32_t SpamTimer; (uint32_t)(millis() - SpamTimer) >= (t); SpamTimer = millis()) // (BLACK BOX) Ya, don't complain that I used "for(;;){}" instead of "if(){}" for my Blink Without Delay Timer macro. It works nicely!!!
// printfloatx() is a helper Macro used with the Serial class to simplify my code and provide enhanced viewing of Float and interger values:
#define printfloatx(Name,Variable,Spaces,Precision,EndTxt) print(Name); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(EndTxt);//Name,Variable,Spaces,Precision,EndTxt

//Gyro, Accel and Quaternion
int PrintAllValues(int16_t *gyro, int16_t *accel, int32_t *quat, uint16_t SpamDelay = 100) {
  Quaternion q;
  VectorFloat gravity;
  float ypr[3] = { 0, 0, 0 };
  float xyz[3] = { 0, 0, 0 };
  spamtimer(SpamDelay) {// non blocking delay before printing again. This skips the following code when delay time (ms) hasn't been met
    mpu.GetQuaternion(&q, quat);
    mpu.GetGravity(&gravity, &q);
    mpu.GetYawPitchRoll(ypr, &q, &gravity);
    mpu.ConvertToDegrees(ypr, xyz);
    Serial.printfloatx(F("Yaw")  , xyz[0], 9, 4, F(",   ")); //printfloatx is a Helper Macro that works with Serial.print that I created (See #define above)
    Serial.printfloatx(F("Pitch"), xyz[1], 9, 4, F(",   "));
    Serial.printfloatx(F("Roll") , xyz[2], 9, 4, F(",   "));
    Serial.printfloatx(F("ax")   , accel[0], 5, 0, F(",   "));
    Serial.printfloatx(F("ay")   , accel[1], 5, 0, F(",   "));
    Serial.printfloatx(F("az")   , accel[2], 5, 0, F(",   "));
    Serial.printfloatx(F("gx")   , gyro[0],  5, 0, F(",   "));
    Serial.printfloatx(F("gy")   , gyro[1],  5, 0, F(",   "));
    Serial.printfloatx(F("gz")   , gyro[2],  5, 0, F("\n"));
  }
}
//***************************************************************************************
//******************              Callback Funciton                **********************
//***************************************************************************************

// This callback is triggered automatically when new data became available and was retrieved
// from the fifo buffer.
void print_Values (int16_t *gyro, int16_t *accel, int32_t *quat, uint32_t *timestamp) {
  uint8_t Spam_Delay = 100; // Built in Blink without delay timer preventing Serial.print SPAM
  PrintAllValues(gyro, accel, quat, Spam_Delay);
}

//***************************************************************************************
//******************                Setup and Loop                 **********************
//***************************************************************************************
void setup() {
  uint8_t val;
  // initialize serial communication
  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately
  //  Wire.begin();
  Serial.println("Start: ");
  // Lets test for connection on any address
  mpu.SetAddress(MPU6050_ADDRESS_AD0_LOW).TestConnection(1);
  mpu.load_DMP_Image(OFFSETS); // Does it all for you
  Serial.print(F("Setup Complete in "));
  Serial.print(millis());
  Serial.println(F(" Miliseconds"));
  Serial.println(F("If this is your first time running this program with this specific MPU6050,\n"
  " Start by having the MPU6050 placed  stationary on a flat surface to get a proper accellerometer calibration\n"
  " Lets get started here are the Starting and Ending Offsets\n"
  " Place the new offsets on the #define OFFSETS... line above for quick startup"));

  mpu.PrintActiveOffsets();
  mpu.CalibrateGyro(CalibrationLoops);
  mpu.CalibrateAccel(CalibrationLoops);
  mpu.PrintActiveOffsets();
  mpu.on_FIFO(print_Values);
  Serial.print(F("Full calibration Complete in "));
  Serial.print(millis());
  Serial.println(F(" Miliseconds"));
}

void loop() {
  mpu.dmp_read_fifo();// Must be in loop
}
HC

Go Up