121 lines
4.3 KiB
C
121 lines
4.3 KiB
C
#include "MPU6050_6Axis_MotionApps612.h"
|
|
//#include "MPU6050.h" // not necessary if using MotionApps include file
|
|
|
|
|
|
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
|
#include "Wire.h"
|
|
#endif
|
|
|
|
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
|
|
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
|
VectorInt16 gy; // [x, y, z] gyro sensor measurements
|
|
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
|
VectorInt16 aaRealLast; // [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
|
|
|
|
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
|
|
void ICACHE_RAM_ATTR dmpDataReady() {
|
|
mpuInterrupt = true;
|
|
}
|
|
|
|
#define INTERRUPT_PIN D7
|
|
|
|
void initGyro(){
|
|
// initialize device
|
|
Serial.println(F("Initializing I2C devices..."));
|
|
mpu.initialize();
|
|
pinMode(INTERRUPT_PIN, INPUT);
|
|
|
|
// verify connection
|
|
Serial.println(F("Testing device connections..."));
|
|
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
|
|
|
// wait for ready
|
|
// Serial.println(F("\nSend any character to begin DMP programming and demo: "));
|
|
// while (Serial.available() && Serial.read()); // empty buffer
|
|
// while (!Serial.available()); // wait for data
|
|
// while (Serial.available() && Serial.read()); // empty buffer again
|
|
|
|
// load and configure the DMP
|
|
Serial.println(F("Initializing DMP..."));
|
|
devStatus = mpu.dmpInitialize();
|
|
|
|
// supply your own gyro offsets here, scaled for min sensitivity
|
|
|
|
mpu.setXAccelOffset(-6009);
|
|
mpu.setYAccelOffset(-5577);
|
|
mpu.setZAccelOffset(7961);
|
|
mpu.setXGyroOffset(98);
|
|
mpu.setYGyroOffset(85);
|
|
mpu.setZGyroOffset(81);
|
|
// make sure it worked (returns 0 if so)
|
|
if (devStatus == 0) {
|
|
// Calibration Time: generate offsets and calibrate our MPU6050
|
|
// mpu.CalibrateAccel(6);
|
|
// mpu.CalibrateGyro(6);
|
|
Serial.println();
|
|
mpu.PrintActiveOffsets();
|
|
// turn on the DMP, now that it's ready
|
|
Serial.println(F("Enabling 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 our DMP Ready flag so the main loop() function knows it's okay to use it
|
|
Serial.println(F("DMP ready! Waiting for first interrupt..."));
|
|
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(")"));
|
|
}
|
|
|
|
delay(5000);
|
|
}
|
|
|
|
bool updateGyro(){
|
|
if (!dmpReady) return false;
|
|
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
|
|
// Xf = 1/4*(aaRealLast.x + aaReal.x)t^2 + Vot + Xo
|
|
// aaRealLast = aaReal;
|
|
|
|
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
|
mpu.dmpGetAccel(&aa, fifoBuffer);
|
|
mpu.dmpGetGyro(&gy, fifoBuffer);
|
|
mpu.dmpGetGravity(&gravity, &q);
|
|
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
|
mpu.dmpGetEuler(euler, &q);
|
|
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
|
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
|
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|