aaa
Dependencies: ArduinoSerial I2Cdev2
Dependents: AutoFlight2017_now2 AutoFlight2018_Control sbus_test_2018 Autoflight2018_sbusread ... more
Diff: MPU6050_DMP6.cpp
- Revision:
- 0:c3af3416e383
- Child:
- 2:ff025b9c6c7c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050_DMP6.cpp Tue Aug 28 07:09:21 2018 +0000 @@ -0,0 +1,342 @@ +#include "MPU6050_DMP6.h" + +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project + +//#include "MPU6050.h" // not necessary if using MotionApps include file + + + +/* ========================================================================= +NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch +depends on the MPU-6050's INT pin being connected to the Arduino's +external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is +digital I/O pin 2. +* ========================================================================= */ + +// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual +// quaternion components in a [w, x, y, z] format (not best for parsing +// on a remote host such as Processing or something though) +//#define OUTPUT_READABLE_QUATERNION + +// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles +// (in degrees) calculated from the quaternions coming from the FIFO. +// Note that Euler angles suffer from gimbal lock (for more info, see +// http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_EULER + +// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ +// pitch/roll angles (in degrees) calculated from the quaternions coming +// from the FIFO. Note this also requires gravity vector calculations. +// Also note that yaw/pitch/roll angles suffer from gimbal lock (for +// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) +#define OUTPUT_READABLE_YAWPITCHROLL + +// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration +// components with gravity removed. This acceleration reference frame is +// not compensated for orientation, so +X is always +X according to the +// sensor, just without the effects of gravity. If you want acceleration +// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. +//#define OUTPUT_READABLE_REALACCEL + +// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration +// components with gravity removed and adjusted for the world frame of +// reference (yaw is relative to initial orientation, since no magnetometer +// is present in this case). Could be quite handy in some cases. +//#define OUTPUT_READABLE_WORLDACCEL + +// uncomment "OUTPUT_TEAPOT" if you want output that matches the +// format used for the InvenSense teapot demo +//#define OUTPUT_TEAPOT + +#ifndef M_PI +#define M_PI 3.14159265358979 +#endif + +MPU6050DMP6::MPU6050DMP6(PinName intpin, RawSerial* serial_p) + : + checkpin_p(new InterruptIn(intpin)), + checkpin(*checkpin_p), + pc_p(serial_p) +{ + initializeValue(); +} + +MPU6050DMP6::~MPU6050DMP6(){} + +void MPU6050DMP6::dmpDataReady() +{ + mpuInterrupt = true; +} + +void MPU6050DMP6::transformeCoordinate_Skipper(Quaternion* q1, Quaternion* q2){ + q2->w = q1->w; + q2->x = q1->y; + q2->y = - q1->x; + q2->z = q1->z; +} + + +int MPU6050DMP6::loopstarting(){ + // if programming failed, don't try to do anything + if (!dmpReady) return -1; + + // wait for MPU interrupt or extra packet(s) available + while (!mpuInterrupt && fifoCount < packetSize) { + // other program behavior stuff here + // . + // . + // . + // if you are really paranoid you can frequently test in between other + // stuff to see if mpuInterrupt is true, and if so, "break;" from the + // while() loop to immediately process the MPU data + // . + // . + // . + } + + // 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 & 0x10) || fifoCount == 1024) { + // reset so we can continue cleanly + mpu.resetFIFO(); + //arduinoSerial.println(F("FIFO overflow!")); + + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mpuIntStatus & 0x02) { + // 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; + +} + + +} +// ================================================================ +// === INITIAL SETUP === +// ================================================================ + +int MPU6050DMP6::setup() +{ + // initialize arduinoSerial communication + // (115200 chosen because it is required for Teapot Demo output, but it's + // really up to you depending on your project) + // initialize device + pc_p->printf("Initializing I2C devices...\r\n"); + mpu.initialize(); + + // verify connection + pc_p->printf("Testing device connections...\r\n"); + + if(mpu.testConnection()) pc_p->printf("MPU6050 connection successful\r\n"); + else pc_p->printf("MPU6050 connection failed\r\n"); + + //arduinoSerial.println(F("Testing device connections...")); + //arduinoSerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); + + // wait for ready + // arduinoSerial.println(F("\nSend any character to begin DMP programming and demo: ")); + // while (arduinoSerial.available() && arduinoSerial.read()); // empty buffer + // while (!arduinoSerial.available()); // wait for data + // while (arduinoSerial.available() && arduinoSerial.read()); // empty buffer again + + // load and configure the DMP + pc_p->printf("Initializing DMP...\r\n"); + //arduinoSerial.println(F("Initializing DMP...")); + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(220); + mpu.setYGyroOffset(76); + mpu.setZGyroOffset(-85); + mpu.setZAccelOffset(1788); // 1688 factory default for my test chip + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // turn on the DMP, now that it's ready + pc_p->printf("Enabling DMP...\r\n"); + + //arduinoSerial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); + + // enable Arduino interrupt detection + pc_p->printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n"); + //arduinoSerial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); + checkpin.rise(this, &MPU6050DMP6::dmpDataReady); + mpuIntStatus = mpu.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + pc_p->printf("DMP ready! Waiting for first interrupt...\r\n"); + //arduinoSerial.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) + + pc_p->printf("DMP Initialization failed (code %d)\r\n",devStatus); + + + //arduinoSerial.print(F("DMP Initialization failed (code ")); + //arduinoSerial.print(devStatus); + //arduinoSerial.println(F(")")); + return -1; + } + + return 0; +} + + + +// ================================================================ +// === MAIN PROGRAM LOOP === +// ================================================================ + +void MPU6050DMP6::loop() +{ + loopstarting(); + +#ifdef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); + /* + arduinoSerial.print("quat\t"); + arduinoSerial.print(q.w); + arduinoSerial.print("\t"); + arduinoSerial.print(q.x); + arduinoSerial.print("\t"); + arduinoSerial.print(q.y); + arduinoSerial.print("\t"); + arduinoSerial.println(q.z); + */ +#endif + +#ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + /* + arduinoSerial.print("euler\t"); + arduinoSerial.print(euler[0] * 180/M_PI); + arduinoSerial.print("\t"); + arduinoSerial.print(euler[1] * 180/M_PI); + arduinoSerial.print("\t"); + arduinoSerial.println(euler[2] * 180/M_PI); + */ +#endif + +#ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + /* + arduinoSerial.print("ypr\t"); + arduinoSerial.print(ypr[0] * 180/M_PI); + arduinoSerial.print("\t"); + arduinoSerial.print(ypr[1] * 180/M_PI); + arduinoSerial.print("\t"); + arduinoSerial.println(ypr[2] * 180/M_PI); + */ +#endif + +#ifdef OUTPUT_READABLE_REALACCEL + // display real acceleration, adjusted to remove gravity + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + /* + arduinoSerial.print("areal\t"); + arduinoSerial.print(aaReal.x); + arduinoSerial.print("\t"); + arduinoSerial.print(aaReal.y); + arduinoSerial.print("\t"); + arduinoSerial.println(aaReal.z); + */ +#endif + +#ifdef OUTPUT_READABLE_WORLDACCEL + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + /* + arduinoSerial.print("aworld\t"); + arduinoSerial.print(aaWorld.x); + arduinoSerial.print("\t"); + arduinoSerial.print(aaWorld.y); + arduinoSerial.print("\t"); + arduinoSerial.println(aaWorld.z); + */ +#endif + +#ifdef OUTPUT_TEAPOT + // display quaternion values in InvenSense Teapot demo format: + teapotPacket[2] = fifoBuffer[0]; + teapotPacket[3] = fifoBuffer[1]; + teapotPacket[4] = fifoBuffer[4]; + teapotPacket[5] = fifoBuffer[5]; + teapotPacket[6] = fifoBuffer[8]; + teapotPacket[7] = fifoBuffer[9]; + teapotPacket[8] = fifoBuffer[12]; + teapotPacket[9] = fifoBuffer[13]; + //arduinoSerial.write(teapotPacket, 14); + teapotPacket[11]++; // packetCount, loops at 0xFF on purpose +#endif + +} + + +void MPU6050DMP6::getRollPitchYaw_Skipper(float rpy[3]){ + float ypr[3]; + loopstarting(); + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + transformeCoordinate_Skipper(&q, &q2); + mpu.dmpGetGravity(&gravity, &q2); + mpu.dmpGetYawPitchRoll(ypr, &q2, &gravity); + rpy[0] = ypr[2]; + rpy[1] = ypr[1]; + rpy[2] = ypr[0]; +} + +void MPU6050DMP6::initializeValue(){ + dmpReady = false; + mpuInterrupt = false; + + teapotPacket[0] = '$'; + teapotPacket[1] = 0x20; + teapotPacket[2] = 0; + teapotPacket[3] = 0; + teapotPacket[4] = 0; + teapotPacket[5] = 0; + teapotPacket[6] = 0; + teapotPacket[7] = 0; + teapotPacket[8] = 0; + teapotPacket[9] = 0; + teapotPacket[10] = 0x00; + teapotPacket[11] = 0x00; + teapotPacket[12] = '\r'; + teapotPacket[13] = '\n'; + +} \ No newline at end of file