Arduino Forum

Community => Exhibition / Gallery => Topic started by: zhomeslice on Jun 23, 2019, 04:11 pm

Title: **Finally!** MPU6050 FIFO DMP Quaternion code for Arduino. --made easy--
Post by: zhomeslice on Jun 23, 2019, 04:11 pm
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 (https://github.com/ZHomeSlice/Simple_MPU6050) library.

Link: https://github.com/ZHomeSlice/Simple_MPU6050 (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:


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 (https://github.com/ZHomeSlice/Simple_MPU6050)

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
}