/*
MPU6050 DMP6
Digital Motion Processor or DMP performs complex motion processing tasks.
- Fuses the data from the accel, gyro, and external magnetometer if applied, compensating individual sensor noise and errors.
- Detect specific types of motion without the need to continuously monitor raw sensor data with a microcontroller.
- Reduce workload on the microprocessor.
- Output processed data such as quaternions, Euler angles, and gravity vectors.
The code includes an auto-calibration and offsets generator tasks. Different output formats available.
Find the full MPU6050 library documentation here:
https://github.com/ElectronicCats/mpu6050/wiki
*/
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050_6Axis_MotionApps612.h"
/* MPU6050 default I2C address is 0x68*/
MPU6050 mpu;
/* OUTPUT FORMAT DEFINITION-------------------------------------------------------------------------------------------
- Use "OUTPUT_READABLE_EULER" for Euler angles (in degrees) output, calculated from the quaternions coming
from the FIFO. EULER ANGLES SUFFER FROM GIMBAL LOCK PROBLEM.
- Use "OUTPUT_READABLE_YAWPITCHROLL" for yaw/pitch/roll angles (in degrees) calculated from the quaternions
coming from the FIFO. THIS REQUIRES GRAVITY VECTOR CALCULATION.
YAW/PITCH/ROLL ANGLES SUFFER FROM GIMBAL LOCK PROBLEM.
----------------------------------------------------------------------------------------------------------------------*/
#define OUTPUT_READABLE_YAWPITCHROLL
//#define OUTPUT_READABLE_EULER
int const INTERRUPT_PIN = 2; // Define the interruption #0 pin
/*---MPU6050 Control/Status Variables---*/
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)
uint8_t FIFOBuffer[64]; // FIFO storage buffer
/*---Orientation/Motion Variables---*/
Quaternion q; // [w, x, y, z] Quaternion container (quaternion - mathematical concept that extends the concept of complex numbers into higher dimensions. In simple terms, it is a four-dimensional number that consists of a scalar part and a vector part)
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 routine------*/
volatile bool MPUInterrupt = false; // Indicates whether MPU6050 interrupt pin has gone high
void DMPDataReady() {
MPUInterrupt = true;
}
void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment on this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(57600); // встановлюємо високу швидкість, щоб дані встигали передаватись; така сама швидкість має бути виставлена у моніторі порту
while (!Serial);
/*Initialize device*/
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
/*Verify connection*/
Serial.println(F("Testing MPU6050 connection..."));
if(mpu.testConnection() == false){
Serial.println("MPU6050 connection failed");
while(true);
}
else {
Serial.println("MPU6050 connection successful");
}
/*Wait for Serial input*/
Serial.println(F("\nSend any character to begin: "));
while (Serial.available() && Serial.read()); // Empty buffer
while (!Serial.available()); // Wait for data
while (Serial.available() && Serial.read()); // Empty buffer again
/* Initializate and configure the DMP*/
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
/* Supply your gyro offsets here, scaled for min sensitivity */
mpu.setXGyroOffset(0);
mpu.setYGyroOffset(0);
mpu.setZGyroOffset(0);
mpu.setXAccelOffset(0);
mpu.setYAccelOffset(0);
mpu.setZAccelOffset(0);
/* Making sure it worked (returns 0 if so) */
if (devStatus == 0) {
mpu.CalibrateAccel(6); // Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateGyro(6);
Serial.println("These are the Active offsets: ");
mpu.PrintActiveOffsets();
Serial.println(F("Enabling DMP...")); //Turning ON DMP
mpu.setDMPEnabled(true);
/*Enable Arduino interrupt detection*/
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), DMPDataReady, RISING);
MPUIntStatus = mpu.getIntStatus();
/* Set the DMP Ready flag so the main loop() function knows it is okay to use it */
Serial.println(F("DMP ready! Waiting for first interrupt..."));
DMPReady = true;
packetSize = mpu.dmpGetFIFOPacketSize(); //Get expected DMP packet size for later comparison
}
else {
Serial.print(F("DMP Initialization failed (code ")); //Print the error code
Serial.print(devStatus);
Serial.println(F(")"));
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
}
}
void loop() {
if (!DMPReady) return; // Stop the program if DMP programming fails.
/* Read a packet from FIFO */
if (mpu.dmpGetCurrentFIFOPacket(FIFOBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
/* Display Euler angles in degrees */
mpu.dmpGetQuaternion(&q, FIFOBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.println("Yaw\tPitch\tRoll");
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
#ifdef OUTPUT_READABLE_EULER
/* Display Euler angles in degrees */
mpu.dmpGetQuaternion(&q, FIFOBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.println("psi\ttheta\tphi");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180/M_PI);
#endif
}
delay(1000);
}