Hello All, In my flying career I saw analog instruments  phased out and digital instruments replace them. I wanted to try and understand how MEMS technology worked, not just how to use it. I purchased the Adafriut 9 axis sensor used in an instruction  video series and cut and pasted my way to the end, lesson 24.

I was surprised and disappointed in the complexity of calibrating the compass by waving it around and how quickly in fell out of the set parameters it captured during the calibration stage. Within minutes in had wandered several degrees. The breakout board claims to be tilt compensating, but the more I tilted it the faster the heading wandered. I found the same to be true with the 6 DOF MPU 6050

I did build a diy-arduino-gimbal-self-stabilizing-platform/#Arduino_Code   I never did get the visual for the computer up and running but the code running on a MPU 6050 did run the servos with the same problem as before, wandering heading. The instructor in the video said that this breakout board was not as accurate as it could be.  During this period of time I was reading and came across shields for Arduino. I saw an Arduino 9 axis motion shield and so I bought one to see how it preformed. It was night and day. No messing around, plug it in and move it a fraction of an inch and it is reading, accurately, no deviation in heading after 12 hours.

It was very interesting, both claim to use the BNO 005 and yet the Arduino shield is much better. More like what I saw in the cockpit.  Fast and reliable.

I think a truly superior system would be taking instructions from the Arduino 9 axis motion shield and using them to run the code in; DIY Gimbal - MPU6050 Arduino

 Code based on the MPU6050_DMP6 example from the i2cdevlib library by Jeff Rowberg

I watched a video on combining sketch's , but this looks like it would take time and is much more complicated than the video example. I have no idea if it is possible with a variety of "if" or conflicts ect.

If you have a minute please read through both sets of code and tell me if it is worth the effort or if something jumps out that you think makes it impossible. Thanks, Quince

#include <Wire.h>
#include "NAxisMoton.h"
//Object that for the sensor
NAxisMotion mySensor;

//Flag to indicate if an interrupt was detected
bool intDetected = false;

//At a Range of 4g, the threshold
//is set at 39.05mg or 0.3830m/s2.
//This Range is the default for NDOF Mode
int threshold = 5;

//At a filter Bandwidth of 62.5Hz,
//the duration is 8ms.
//This Bandwidth is the default for NDOF Mode
int duration = 1;

//To know which interrupt was triggered
bool anyMotion = true;

//This code is executed once
void setup() {
  //Peripheral Initialization

  //Initialize the Serial Port to view information on the Serial Monitor
  Serial.begin(115200);

  //Initialize I2C communication to the let the library communicate with the sensor.
  I2C.begin();

  //Sensor Initialization
  Serial.println("Please wait. Initialization in process.");

  //The I2C Address can be changed here inside this function in the library
  mySensor.initSensor();
  mySensor.setOperationMode(OPERATION_MODE_NDOF);

  //Can be configured to other operation modes as desired
  mySensor.setUpdateMode(MANUAL);
  //The default is AUTO. Changing to manual requires
  //calling the relevant update functions prior to calling the read functions

  //Setting to MANUAL  requires lesser reads to the sensor

  //Attach the interrupt to the Interrupt Service Routine
  //for a Rising Edge. Change the interrupt pin depending on the board
  attachInterrupt(INT_PIN, motionISR, RISING);

  //Setup the initial interrupt to trigger at No Motion
  mySensor.resetInterrupt();
  mySensor.enableSlowNoMotion(threshold, duration, NO_MOTION);
  anyMotion = false;

  //Accelerometer interrupts can be triggered from all 3 axes
  mySensor.accelInterrupts(ENABLE, ENABLE, ENABLE);

  Serial.println("This is a game to test how steady you can move an object with one hand. \n
                 Keep the device on a table and mark 2 points.");

  Serial.println("Move the Device from one place to another
                 without triggering the Any Motion Interrupt.\n\n");

  delay(1000); //Delay for the player(s) to read
  Serial.println("Move the device around and then place it at one position.\n
                 Change the threshold and duration to increase the difficulty level.");

  Serial.println("Have fun!\n\n");
}

void loop() //This code is looped forever
{
  if (intDetected)
  {
    if (anyMotion)
    {
      Serial.println("You moved!! Try again. Keep the Device at one place.\n");
      intDetected = false;

      //Reset the interrupt line
      mySensor.resetInterrupt();

      //Disable the Any motion interrupt
      mySensor.disableAnyMotion();

      //Enable the No motion interrupt
      //(can also use the Slow motion instead)
      mySensor.enableSlowNoMotion(threshold, duration, NO_MOTION);
      anyMotion = false;
    }
    else
    {
      Serial.println("Device is not moving. You may start again.\n\n\n");
      intDetected = false;

      //Reset the interrupt line
      mySensor.resetInterrupt();

      //Disable the Slow or No motion interrupt
      mySensor.disableSlowNoMotion();

      //Enable the Any motion interrupt
      mySensor.enableAnyMotion(threshold, duration);
      anyMotion = true;
    }
  }
}

//Interrupt Service Routine
//when the sensor triggers an Interrupt
void motionISR() {
  intDetected = true;
}

*/
// 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 "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#include <Servo.h>
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

// Define the 3 servo motors
Servo servo0;
Servo servo1;
Servo servo2;
float correct;
int j = 0;

#define OUTPUT_READABLE_YAWPITCHROLL

#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards

bool blinkState = false;

// 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 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

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

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

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  // initialize serial communication
  // (115200 chosen because it is required for Teapot Demo output, but it's
  // really up to you depending on your project)
  Serial.begin(38400);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  // initialize device
  //Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(17);
  mpu.setYGyroOffset(-69);
  mpu.setZGyroOffset(27);
  mpu.setZAccelOffset(1551); // 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
    // Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    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(")"));
  }

  // Define the pins to which the 3 servo motors are connected
  servo0.attach(10);
  servo1.attach(9);
  servo2.attach(8);
}
// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) {
    if (mpuInterrupt && fifoCount < packetSize) {
      // try to get out of the infinite loop
      fifoCount = mpu.getFIFOCount();
    }
  }

  // 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 & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    fifoCount = mpu.getFIFOCount();
    Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
    // 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;

    // Get Yaw, Pitch and Roll values
#ifdef OUTPUT_READABLE_YAWPITCHROLL
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    // Yaw, Pitch, Roll values - Radians to degrees
    ypr[0] = ypr[0] * 180 / M_PI;
    ypr[1] = ypr[1] * 180 / M_PI;
    ypr[2] = ypr[2] * 180 / M_PI;
    
    // Skip 300 readings (self-calibration process)
    if (j <= 300) {
      correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
      j++;
    }
    // After 300 readings
    else {
      ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract  the last random Yaw value from the currrent value to make the Yaw 0 degrees
      // Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
      int servo0Value = map(ypr[0], -90, 90, 0, 180);
      int servo1Value = map(ypr[1], -90, 90, 0, 180);
      int servo2Value = map(ypr[2], -90, 90, 180, 0);
      
      // Control the servos according to the MPU6050 orientation
      servo0.write(servo0Value);
      servo1.write(servo1Value);
      servo2.write(servo2Value);
    }
#endif
  }
}