Advertisement
kennyho-rass

imu to ros

Oct 6th, 2021
855
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
  2. // for both classes must be in the include path of your project
  3. #include "I2Cdev.h"
  4.  
  5. #include "MPU6050_6Axis_MotionApps20.h"
  6. //#include "MPU6050.h" // not necessary if using MotionApps include file
  7.  
  8. // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
  9. // is used in I2Cdev.h
  10. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  11.     #include "Wire.h"
  12. #endif
  13.  
  14. // class default I2C address is 0x68
  15. // specific I2C addresses may be passed as a parameter here
  16. // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
  17. // AD0 high = 0x69
  18. MPU6050 mpu;
  19. //MPU6050 mpu(0x69); // <-- use for AD0 high
  20.  
  21. /* =========================================================================
  22.    NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
  23.    depends on the MPU-6050's INT pin being connected to the Arduino's
  24.    external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
  25.    digital I/O pin 2.
  26.  * ========================================================================= */
  27.  
  28. /* =========================================================================
  29.    NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
  30.    when using Serial.write(buf, len). The Teapot output uses this method.
  31.    The solution requires a modification to the Arduino USBAPI.h file, which
  32.    is fortunately simple, but annoying. This will be fixed in the next IDE
  33.    release. For more info, see these links:
  34.    http://arduino.cc/forum/index.php/topic,109987.0.html
  35.    http://code.google.com/p/arduino/issues/detail?id=958
  36.  * ========================================================================= */
  37.  
  38.  
  39. #define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
  40. #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
  41. bool blinkState = false;
  42.  
  43. // MPU control/status vars
  44. bool dmpReady = false;  // set true if DMP init was successful
  45. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
  46. uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
  47. uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
  48. uint16_t fifoCount;     // count of all bytes currently in FIFO
  49. uint8_t fifoBuffer[64]; // FIFO storage buffer
  50.  
  51. // orientation/motion vars
  52. Quaternion q;           // [w, x, y, z]         quaternion container
  53. VectorInt16 aa;         // [x, y, z]            accel sensor measurements
  54. VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
  55. VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
  56. VectorFloat gravity;    // [x, y, z]            gravity vector
  57. float euler[3];         // [psi, theta, phi]    Euler angle container
  58. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  59.  
  60. // packet structure for InvenSense teapot demo
  61. uint8_t teapotPacket[28] = { '$', 0x03, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
  62.  
  63.  
  64.  
  65. // ================================================================
  66. // ===               INTERRUPT DETECTION ROUTINE                ===
  67. // ================================================================
  68.  
  69. volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
  70. void dmpDataReady() {
  71.     mpuInterrupt = true;
  72. }
  73.  
  74.  
  75.  
  76. // ================================================================
  77. // ===                      INITIAL SETUP                       ===
  78. // ================================================================
  79.  
  80. void setup() {
  81.     // join I2C bus (I2Cdev library doesn't do this automatically)
  82.     #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  83.         Wire.begin();
  84.         Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  85.     #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  86.         Fastwire::setup(400, true);
  87.     #endif
  88.  
  89.     // initialize serial communication
  90.     // (115200 chosen because it is required for Teapot Demo output, but it's
  91.     // really up to you depending on your project)
  92.     Serial.begin(115200);
  93.     while (!Serial); // wait for Leonardo enumeration, others continue immediately
  94.  
  95.     // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
  96.     // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
  97.     // the baud timing being too misaligned with processor ticks. You must use
  98.     // 38400 or slower in these cases, or use some kind of external separate
  99.     // crystal solution for the UART timer.
  100.  
  101.     // initialize device
  102.     Serial.println(F("Initializing I2C devices..."));
  103.     mpu.initialize();
  104.     pinMode(INTERRUPT_PIN, INPUT);
  105.  
  106.     // verify connection
  107.     Serial.println(F("Testing device connections..."));
  108.     Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  109.  
  110.     // load and configure the DMP
  111.     Serial.println(F("Initializing DMP..."));
  112.     devStatus = mpu.dmpInitialize();
  113.  
  114.     // supply your own gyro offsets here, scaled for min sensitivity
  115.     mpu.setXAccelOffset(-1169);
  116.     mpu.setYAccelOffset(744);
  117.     mpu.setZAccelOffset(1620);
  118.     mpu.setXGyroOffset(48);
  119.     mpu.setYGyroOffset(47);
  120.     mpu.setZGyroOffset(-8);
  121.  
  122.     // make sure it worked (returns 0 if so)
  123.     if (devStatus == 0) {
  124.         // turn on the DMP, now that it's ready
  125.         Serial.println(F("Enabling DMP..."));
  126.         mpu.setDMPEnabled(true);
  127.  
  128.         // enable Arduino interrupt detection
  129.         Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
  130.         attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
  131.         mpuIntStatus = mpu.getIntStatus();
  132.  
  133.         // set our DMP Ready flag so the main loop() function knows it's okay to use it
  134.         Serial.println(F("DMP ready! Waiting for first interrupt..."));
  135.         dmpReady = true;
  136.  
  137.         // get expected DMP packet size for later comparison
  138.         packetSize = mpu.dmpGetFIFOPacketSize();
  139.     } else {
  140.         // ERROR!
  141.         // 1 = initial memory load failed
  142.         // 2 = DMP configuration updates failed
  143.         // (if it's going to break, usually the code will be 1)
  144.         Serial.print(F("DMP Initialization failed (code "));
  145.         Serial.print(devStatus);
  146.         Serial.println(F(")"));
  147.     }
  148.  
  149.     // configure LED for output
  150.     pinMode(LED_PIN, OUTPUT);
  151. }
  152.  
  153.  
  154.  
  155. // ================================================================
  156. // ===                    MAIN PROGRAM LOOP                     ===
  157. // ================================================================
  158.  
  159. void loop() {
  160.     // if programming failed, don't try to do anything
  161.     if (!dmpReady) return;
  162.  
  163.     // wait for MPU interrupt or extra packet(s) available
  164.     while (!mpuInterrupt && fifoCount < packetSize) {
  165.         // other program behavior stuff here
  166.         // .
  167.         // .
  168.         // .
  169.         // if you are really paranoid you can frequently test in between other
  170.         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
  171.         // while() loop to immediately process the MPU data
  172.         // .
  173.         // .
  174.         // .
  175.     }
  176.  
  177.     // reset interrupt flag and get INT_STATUS byte
  178.     mpuInterrupt = false;
  179.     mpuIntStatus = mpu.getIntStatus();
  180.  
  181.     // get current FIFO count
  182.     fifoCount = mpu.getFIFOCount();
  183.  
  184.     // check for overflow (this should never happen unless our code is too inefficient)
  185.     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
  186.         // reset so we can continue cleanly
  187.         mpu.resetFIFO();
  188.         Serial.println(F("FIFO overflow!"));
  189.  
  190.     // otherwise, check for DMP data ready interrupt (this should happen frequently)
  191.     } else if (mpuIntStatus & 0x02) {
  192.         // wait for correct available data length, should be a VERY short wait
  193.         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  194.  
  195.         // read a packet from FIFO
  196.         mpu.getFIFOBytes(fifoBuffer, packetSize);
  197.  
  198.         // track FIFO count here in case there is > 1 packet available
  199.         // (this lets us immediately read more without waiting for an interrupt)
  200.         fifoCount -= packetSize;
  201.  
  202.         // display quaternion values in InvenSense Teapot demo format:
  203.         teapotPacket[2] = fifoBuffer[0];
  204.         teapotPacket[3] = fifoBuffer[1];
  205.         teapotPacket[4] = fifoBuffer[4];
  206.         teapotPacket[5] = fifoBuffer[5];
  207.         teapotPacket[6] = fifoBuffer[8];
  208.         teapotPacket[7] = fifoBuffer[9];
  209.         teapotPacket[8] = fifoBuffer[12];
  210.         teapotPacket[9] = fifoBuffer[13];
  211.         // gyro values
  212.         teapotPacket[10] = fifoBuffer[16];
  213.         teapotPacket[11] = fifoBuffer[17];
  214.         teapotPacket[12] = fifoBuffer[20];
  215.         teapotPacket[13] = fifoBuffer[21];
  216.         teapotPacket[14] = fifoBuffer[24];
  217.         teapotPacket[15] = fifoBuffer[25];
  218.         // accelerometer values
  219.         teapotPacket[16] = fifoBuffer[28];
  220.         teapotPacket[17] = fifoBuffer[29];
  221.         teapotPacket[18] = fifoBuffer[32];
  222.         teapotPacket[19] = fifoBuffer[33];
  223.         teapotPacket[20] = fifoBuffer[36];
  224.         teapotPacket[21] = fifoBuffer[37];
  225.         //temperature
  226.         int16_t temperature = mpu.getTemperature();
  227.         teapotPacket[22] = temperature >> 8;
  228.         teapotPacket[23] = temperature & 0xFF;
  229.         Serial.write(teapotPacket, 28);
  230.         teapotPacket[25]++; // packetCount, loops at 0xFF on purpose
  231.  
  232.         // blink LED to indicate activity
  233.         blinkState = !blinkState;
  234.         digitalWrite(LED_PIN, blinkState);
  235.     }
  236. }
Advertisement
RAW Paste Data Copied
Advertisement