rotation.ino

// Thanks to https://forum.arduino.cc/index.php?topic=408851.msg2813515#msg2813515

#ifndef JOYSTICKSHIELD
  MPU6050 mpu;

  struct MPU_INFO {
    int xa;
    int ya;
    int za;
    int xg;
    int yg;
    int zg;
  };
  const struct MPU_INFO offsets = { .xa = 2192, .ya = -506, .za = 595, .xg = 114, .yg = 23, .zg = -5 };

  // ================================================================
  // ===               INTERRUPT DETECTION ROUTINE                ===
  // ================================================================
  volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
  void dmpDataReady() {
    mpuInterrupt = true;
  }

  // ================================================================
  // ===                      MPU DMP SETUP                       ===
  // ================================================================
  int FifoAlive = 0; // tests if the interrupt is triggering
  int IsAlive = -20;     // counts interrupt start at -20 to get 20+ good values before assuming connected
  // MPU control/status vars
  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 aaReal;     // [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
  //float Yaw; // in degrees



  void MPU6050Connect() {
    static int MPUInitCntr = 0;
    // initialize device
    mpu.initialize(); // same
    // load and configure the DMP
    devStatus = mpu.dmpInitialize();// same

    if (devStatus != 0) {
      // ERROR!
      // 1 = initial memory load failed
      // 2 = DMP configuration updates failed
      // (if it's going to break, usually the code will be 1)

      MPUInitCntr++;

      if (MPUInitCntr >= 10) return; //only try 10 times
      delay(1000);
      MPU6050Connect(); // Lets try again
      return;
    }

    mpu.setXAccelOffset(offsets.xa);
    mpu.setYAccelOffset(offsets.ya);
    mpu.setZAccelOffset(offsets.za);
    mpu.setXGyroOffset(offsets.xg);
    mpu.setYGyroOffset(offsets.yg);
    mpu.setZGyroOffset(offsets.zg);

    mpu.setDMPEnabled(true);

    attachInterrupt(digitalPinToInterrupt(pinInterruptMPU), dmpDataReady, FALLING); // Enabling interrupt detection

    mpuIntStatus = mpu.getIntStatus(); // Same
    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
    delay(1000); // Let it Stabalize
    mpu.resetFIFO(); // Clear fifo buffer
    mpu.getIntStatus();
    mpuInterrupt = false; // wait for next interrupt
  }
  // ================================================================
  // ===                      i2c SETUP Items                     ===
  // ================================================================
  void i2cSetup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
    TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
  #endif
  }


  float getYaw() {
    updateRotationAngles();
    //return Yaw;
    return (float) ypr[0];
  }


  void updateRotationAngles() {
    if (!mpuInterrupt) { return; }

    mpuInterrupt = false;
    FifoAlive = 1;
    fifoCount = mpu.getFIFOCount();
    /*
    fifoCount is a 16-bit unsigned value. Indicates the number of bytes stored in the FIFO buffer.
    This number is in turn the number of bytes that can be read from the FIFO buffer and it is
    directly proportional to the number of samples available given the set of sensor data bound
    to be stored in the FIFO
    */

    // PacketSize = 42; refference in MPU6050_6Axis_MotionApps20.h Line 527
    // FIFO Buffer Size = 1024;
    uint16_t MaxPackets = 20;// 20*42=840 leaving us with  2 Packets (out of a total of 24 packets) left before we overflow.
    // If we overflow the entire FIFO buffer will be corrupt and we must discard it!

    // At this point in the code FIFO Packets should be at 1 99% of the time if not we need to look to see where we are skipping samples.
    if ((fifoCount % packetSize) || (fifoCount > (packetSize * MaxPackets)) || (fifoCount < packetSize)) { // we have failed Reset and wait till next time!
      updateRotationLed(); // lets turn off the blinking light so we can see we are failing.

      mpuIntStatus = mpu.getIntStatus(); // reads MPU6050_RA_INT_STATUS       0x3A
      // MPU6050_RA_INT_STATUS       0x3A
      //
      // Bit7, Bit6, Bit5, Bit4          , Bit3       , Bit2, Bit1, Bit0
      // ----, ----, ----, FIFO_OFLOW_INT, I2C_MST_INT, ----, ----, DATA_RDY_INT

      /*
      Bit4 FIFO_OFLOW_INT: This bit automatically sets to 1 when a FIFO buffer overflow interrupt has been generated.
      Bit3 I2C_MST_INT: This bit automatically sets to 1 when an I2C Master interrupt has been generated. For a list of I2C Master interrupts, please refer to Register 54.
      Bit1 DATA_RDY_INT This bit automatically sets to 1 when a Data Ready interrupt is generated.
      */

      //I2C_MST_STATUS
      //PASS_THROUGH, I2C_SLV4_DONE,I2C_LOST_ARB,I2C_SLV4_NACK,I2C_SLV3_NACK,I2C_SLV2_NACK,I2C_SLV1_NACK,I2C_SLV0_NACK,
      mpu.resetFIFO();// clear the buffer and start over
      mpu.getIntStatus(); // make sure status is cleared we will read it again.
    } else {
      while (fifoCount  >= packetSize) { // Get the packets until we have the latest!
        if (fifoCount < packetSize) break; // Something is left over and we don't want it!!!
        mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
        fifoCount -= packetSize;
      }
      calcAngles();
      blinkRotationLed(); // Blink the Light
      if (fifoCount > 0) mpu.resetFIFO(); // Should never happen! but lets start fresh if we need to.
    }
  }


  void calcAngles() {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    // Yaw = (ypr[0] * 180 / M_PI);
  }
#endif