DIY Flight Simulator: Artificial Horizon & Digital Compass Using MPU6050

   


                       
 Code for Arduino

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13
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();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #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(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately
    mpu.initialize();
    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) {
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, 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 {
       
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}

// ================================================================
// ===                    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) {
 
    }

    // 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();
        //Serial.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;

        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.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);
            //Serial.print("Phi: ");
            Serial.print(ypr[2] * 18/M_PI);
            //Serial.print("\t theta: ");
            Serial.print(" ");
            Serial.print(ypr[1] * 180/M_PI);
            //Serial.print("\t Psi: ");
            Serial.print(" ");
            Serial.println(ypr[0] * 180/M_PI);
            //delay(100);
        #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);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.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);
            Serial.print("aworld\t");
            Serial.print(aaWorld.x);
            Serial.print("\t");
            Serial.print(aaWorld.y);
            Serial.print("\t");
            Serial.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];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif

        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

Processing ide Code

import org.firmata.*; import cc.arduino.*; import processing.serial.*; import cc.arduino.*; float Pitch; float Bank; float Azimuth; float ArtificialHoizonMagnificationFactor=0.7; float CompassMagnificationFactor=0.85; float SpanAngle=120; int NumberOfScaleMajorDivisions; int NumberOfScaleMinorDivisions; PVector v1, v2; Serial port; float Phi; float Theta; float Psi; void setup() { size(1366, 768); rectMode(CENTER); smooth(); strokeCap(SQUARE);//Optional println(Serial.list()); port = new Serial(this, Serial.list()[0], 115200); port.bufferUntil('\n'); } void draw() { background(0); translate(1366/4, 768/2.1); MakeAnglesDependentOnMPU6050(); Horizon(); rotate(-Bank); PitchScale(); Axis(); rotate(Bank); Borders(); Plane(); ShowAngles(); Compass(); ShowAzimuth(); } void serialEvent(Serial port) { String input = port.readStringUntil('\n'); if(input != null){ input = trim(input); String[] values = split(input, " "); if(values.length == 3){ float phi = float(values[0]); float theta = float(values[1]); float psi = float(values[2]); print(phi); print(theta); println(psi); Phi = phi; Theta = theta; Psi = psi; } } } void MakeAnglesDependentOnMPU6050() { Bank =-Phi/5; Pitch=Theta*10; Azimuth=Psi; } void Horizon() { scale(ArtificialHoizonMagnificationFactor); noStroke(); fill(0, 180, 255); rect(0, -100, 900, 1000); fill(95, 55, 40); rotate(Bank); rect(0, 400+Pitch, 900, 800); rotate(-Bank); rotate(-PI-PI/6); SpanAngle=120; NumberOfScaleMajorDivisions=12; NumberOfScaleMinorDivisions=24; CircularScale(); rotate(PI+PI/6); rotate(-PI/6); CircularScale(); rotate(PI/6); } void ShowAzimuth() { fill(50); noStroke(); rect(20, 470, 440, 50); int Azimuth1=round(Azimuth); textAlign(CORNER); textSize(35); fill(255); text("Azimuth: "+Azimuth1+" Deg", 80, 477, 500, 70); textSize(40); fill(25,25,150); text("|TechPilot|", -335, -500, 500, 60); text("~Flight Instruments~", -425, -450, 500, 70); } void Compass() { translate(2*1366/3, 0); scale(CompassMagnificationFactor); noFill(); stroke(100); strokeWeight(80); ellipse(0, 0, 750, 750); strokeWeight(50); stroke(50); fill(0, 0, 40); ellipse(0, 0, 610, 610); for (int k=255;k>0;k=k-5) { noStroke(); fill(0, 0, 255-k); ellipse(0, 0, 2*k, 2*k); } strokeWeight(20); NumberOfScaleMajorDivisions=18; NumberOfScaleMinorDivisions=36; SpanAngle=180; CircularScale(); rotate(PI); SpanAngle=180; CircularScale(); rotate(-PI); fill(255); textSize(60); textAlign(CENTER); text("W", -375, 0, 100, 80); text("E", 370, 0, 100, 80); text("N", 0, -365, 100, 80); text("S", 0, 375, 100, 80); textSize(30); text("COMPASS", 0, -130, 500, 80); rotate(PI/4); textSize(40); text("NW", -370, 0, 100, 50); text("SE", 365, 0, 100, 50); text("NE", 0, -355, 100, 50); text("SW", 0, 365, 100, 50); rotate(-PI/4); CompassPointer(); } void CompassPointer() { rotate(PI+radians(Azimuth)); stroke(0); strokeWeight(4); fill(100, 255, 100); triangle(-20, -210, 20, -210, 0, 270); triangle(-15, 210, 15, 210, 0, 270); ellipse(0, 0, 45, 45); fill(0, 0, 50); noStroke(); ellipse(0, 0, 10, 10); triangle(-20, -213, 20, -213, 0, -190); triangle(-15, -215, 15, -215, 0, -200); rotate(-PI-radians(Azimuth)); } void Plane() { fill(0); strokeWeight(1); stroke(0, 255, 0); triangle(-20, 0, 20, 0, 0, 25); rect(110, 0, 140, 20); rect(-110, 0, 140, 20); } void CircularScale() { float GaugeWidth=800; textSize(GaugeWidth/30); float StrokeWidth=1; float an; float DivxPhasorCloser; float DivxPhasorDistal; float DivyPhasorCloser; float DivyPhasorDistal; strokeWeight(2*StrokeWidth); stroke(255); float DivCloserPhasorLenght=GaugeWidth/2-GaugeWidth/9-StrokeWidth; float DivDistalPhasorLenght=GaugeWidth/2-GaugeWidth/7.5-StrokeWidth; for (int Division=0;Division<NumberOfScaleMinorDivisions+1;Division++) { an=SpanAngle/2+Division*SpanAngle/NumberOfScaleMinorDivisions; DivxPhasorCloser=DivCloserPhasorLenght*cos(radians(an)); DivxPhasorDistal=DivDistalPhasorLenght*cos(radians(an)); DivyPhasorCloser=DivCloserPhasorLenght*sin(radians(an)); DivyPhasorDistal=DivDistalPhasorLenght*sin(radians(an)); line(DivxPhasorCloser, DivyPhasorCloser, DivxPhasorDistal, DivyPhasorDistal); } DivCloserPhasorLenght=GaugeWidth/2-GaugeWidth/10-StrokeWidth; DivDistalPhasorLenght=GaugeWidth/2-GaugeWidth/7.4-StrokeWidth; for (int Division=0;Division<NumberOfScaleMajorDivisions+1;Division++) { an=SpanAngle/2+Division*SpanAngle/NumberOfScaleMajorDivisions; DivxPhasorCloser=DivCloserPhasorLenght*cos(radians(an)); DivxPhasorDistal=DivDistalPhasorLenght*cos(radians(an)); DivyPhasorCloser=DivCloserPhasorLenght*sin(radians(an)); DivyPhasorDistal=DivDistalPhasorLenght*sin(radians(an)); if (Division==NumberOfScaleMajorDivisions/2|Division==0|Division==NumberOfScaleMajorDivisions) { strokeWeight(15); stroke(0); line(DivxPhasorCloser, DivyPhasorCloser, DivxPhasorDistal, DivyPhasorDistal); strokeWeight(8); stroke(100, 255, 100); line(DivxPhasorCloser, DivyPhasorCloser, DivxPhasorDistal, DivyPhasorDistal); } else { strokeWeight(3); stroke(255); line(DivxPhasorCloser, DivyPhasorCloser, DivxPhasorDistal, DivyPhasorDistal); } } } void Axis() { stroke(255, 0, 0); strokeWeight(3); line(-115, 0, 115, 0); line(0, 280, 0, -280); fill(100, 255, 100); stroke(0); triangle(0, -285, -10, -255, 10, -255); triangle(0, 285, -10, 255, 10, 255); } void ShowAngles() { textSize(30); fill(50); noStroke(); rect(-150, 400, 280, 40); rect(150, 400, 280, 40); fill(255); Pitch=Pitch/5; int Pitch1=round(Pitch); text("Pitch: "+Pitch1+" Deg", -20, 411, 500, 60); text("Bank: "+Bank*100+" Deg", 280, 411, 500, 60); } void Borders() { noFill(); stroke(0); strokeWeight(400); rect(0, 0, 1100, 1100); strokeWeight(200); ellipse(0, 0, 1000, 1000); fill(0); noStroke(); rect(4*1366/5, 0,1366, 2*768); rect(-4*1366/5, 0, 1366, 2*768); } void PitchScale() { stroke(255); fill(255); strokeWeight(3); textSize(24); textAlign(CENTER); for (int i=-4;i<5;i++) { if ((i==0)==false) { line(110, 50*i, -110, 50*i); } text(""+i*10, 140, 50*i, 100, 30); text(""+i*10, -140, 50*i, 100, 30); } textAlign(CORNER); strokeWeight(2); for (int i=-9;i<10;i++) { if ((i==0)==false) { line(25, 25*i, -25, 25*i); } } }

Post a Comment

Previous Post Next Post

Contact Form