aaa
Dependencies: ArduinoSerial I2Cdev2
Dependents: AutoFlight2017_now2 AutoFlight2018_Control sbus_test_2018 Autoflight2018_sbusread ... more
MPU6050_DMP6.cpp
- Committer:
- taknokolat
- Date:
- 2018-09-18
- Revision:
- 2:ff025b9c6c7c
- Parent:
- 0:c3af3416e383
File content as of revision 2:ff025b9c6c7c:
#include "MPU6050_DMP6.h" #include "MODSERIAL.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, MODSERIAL* 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'; }