MPU6050 angle filtering

i'm using imu's (mpu6050's) onboard dmp angle data. i'm using jeff rowbergs library this. example code working https://github.com/jrowberg/i2cdevlib/blob/master/arduino/mpu6050/examples/mpu6050_dmp6/mpu6050_dmp6.ino

code: [select]

#include "i2cdev.h"

#include "mpu6050_6axis_motionapps20.h"

#if i2cdev_implementation == i2cdev_arduino_wire
    #include "wire.h"
#endif


mpu6050 mpu;


#define led_pin 13 // (arduino 13, teensy 11, teensy++ 6)
bool blinkstate = false;

// mpu control/status vars
bool dmpready = false;  // set true if dmp init successful
uint8_t mpuintstatus;   // holds actual interrupt status byte mpu
uint8_t devstatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetsize;    // expected dmp packet size (default 42 bytes)
uint16_t fifocount;     // count of bytes 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 , gravity vector

// packet structure 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 automatically)
    #if i2cdev_implementation == i2cdev_arduino_wire
        wire.begin();
      //  twbr = 24; // 400khz i2c clock (200khz if cpu 8mhz)
    #elif i2cdev_implementation == i2cdev_builtin_fastwire
        fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (115200 chosen because required teapot demo output, it's
    // depending on project)
    serial.begin(115200);
    while (!serial); // wait leonardo enumeration, others continue immediately

    // note: 8mhz or slower host processors, teensy @ 3.3v or ardunio
    // pro mini running @ 3.3v, cannot handle baud rate reliably due to
    // baud timing being misaligned processor ticks. must use
    // 38400 or slower in these cases, or use kind of external separate
    // crystal solution uart timer.

    // initialize device
    serial.println(f("initializing i2c devices..."));
    mpu.initialize();

    // verify connection
    serial.println(f("testing device connections..."));
    serial.println(mpu.testconnection() ? f("mpu6050 connection successful") : f("mpu6050 connection failed"));

    // wait ready
    serial.println(f("\nsend character begin dmp programming , demo: "));
    while (serial.available() && serial.read()); // empty buffer
    while (!serial.available());                 // wait data
    while (serial.available() && serial.read()); // empty buffer again

    // load , configure dmp
    serial.println(f("initializing dmp..."));
    devstatus = mpu.dmpinitialize();

    // supply own gyro offsets here, scaled min sensitivity
    mpu.setxgyrooffset(33);
    mpu.setygyrooffset(-19);
    mpu.setzgyrooffset(33);
    mpu.setxacceloffset(-2443);
    mpu.setyacceloffset(-55);
    mpu.setzacceloffset(1286); // 1688 factory default test chip

    // make sure worked (returns 0 if so)
    if (devstatus == 0) {
        // turn on dmp, it's ready
        serial.println(f("enabling dmp..."));
        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 main loop() function knows it's okay use it
        serial.println(f("dmp ready! waiting first interrupt..."));
        dmpready = true;

        // expected dmp packet size later comparison
        packetsize = mpu.dmpgetfifopacketsize();
    } else {
        // error!
        // 1 = initial memory load failed
        // 2 = dmp configuration updates failed
        // (if it's going break, code 1)
        serial.print(f("dmp initialization failed (code "));
        serial.print(devstatus);
        serial.println(f(")"));
    }

    // configure led output
    pinmode(led_pin, output);
}



// ================================================================
// ===                    main program loop                     ===
// ================================================================

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

    // wait mpu interrupt or packet(s) available
    while (!mpuinterrupt && fifocount < packetsize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if paranoid can test in between other
        // stuff see if mpuinterrupt true, , if so, "break;" the
        // while() loop process mpu data
        // .
        // .
        // .
    }

    // reset interrupt flag , int_status byte
    mpuinterrupt = false;
    mpuintstatus = mpu.getintstatus();

    // current fifo count
    fifocount = mpu.getfifocount();

    // check overflow (this should never happen unless our code inefficient)
    if ((mpuintstatus & 0x10) || fifocount == 1024) {
        // reset can continue cleanly
        mpu.resetfifo();
        serial.println(f("fifo overflow!"));

    // otherwise, check dmp data ready interrupt (this should happen frequently)
    } else if (mpuintstatus & 0x02) {
        // wait correct available data length, should short wait
        while (fifocount < packetsize) fifocount = mpu.getfifocount();

        // read packet fifo
        mpu.getfifobytes(fifobuffer, packetsize);
       
        // track fifo count here in case there > 1 packet available
        // (this lets read more without waiting interrupt)
        fifocount -= packetsize;
        // display euler angles in degrees
            mpu.dmpgetquaternion(&q, fifobuffer);
            mpu.dmpgetgravity(&gravity, &q);
            mpu.dmpgetyawpitchroll(ypr, &q, &gravity);
            serial.print("ypr\t");
            serial.print(ypr[0] * 180/m_pi);
            serial.print("\t");
            serial.print(ypr[1] * 180/m_pi);
            serial.print("\t");
            serial.println(ypr[2] * 180/m_pi);
     

   


        // blink led indicate activity
        blinkstate = !blinkstate;
        digitalwrite(led_pin, blinkstate);
    }
}
i removed unnecessary parts me , removed line "//  twbr = 24; // 400khz i2c clock (200khz if cpu 8mhz)" work arduino due.
i plan on using angle detect inclination self balancing robot. problem data noisy. using nema-17 class stepper motors has step angle of 0.35 degrees @ full step , goes 1/32 steps , speeds around 1000 steps per second @ most. wanted know sort of opinions of have on sort of filtering technique should use, or whether purposes reading dmp (digital motion processing) necessary? (could read raw data , use complementary filter or kalman filter).

Arduino Forum > Using Arduino > Sensors > MPU6050 angle filtering

arduino