quadcopter cufe
/
MPU_6050_nadafa
mpu6050 nadafa
Diff: AngularDataAcquizition.h
- Revision:
- 0:8fe8d6dd7cd6
diff -r 000000000000 -r 8fe8d6dd7cd6 AngularDataAcquizition.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AngularDataAcquizition.h Tue Jun 23 14:47:52 2015 +0000 @@ -0,0 +1,181 @@ +#ifndef MBED_AngularDataAcquizition_H +#define MBED_AngularDataAcquizition_H + +#include "I2Cdev.h" +#include "mbed.h" +#include "MPU6050_6Axis_MotionApps20.h" // works +#include <math.h> + +#ifndef M_PI +#define M_PI 3.1415 +#endif +#define D_BAUDRATE 115200 + +class AngularDataAcquizition{ + public: + AngularDataAcquizition(PinName D_SDA, PinName D_SCL); + void BeginInterrupt(float InterruptDuration); + void StopInterrupt(); + void UpdateTheta(); + void MeanValue(float *FilteredVal, int num); + void FilterVariablesAquizition(float *variable,int number); + void callMeanFilteredReadings(); + + float Meanypr[3]; + bool dmpReady; // set true if DMP init was successful + + private: + I2C _i2c; + MPU6050 mpuLocal; + Ticker timer; + 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 + Serial pc; + // packet structure for InvenSense teapot demo + uint8_t teapotPacket[15]; + + }; + + + + + + + +AngularDataAcquizition::AngularDataAcquizition(PinName D_SDA, PinName D_SCL): _i2c(D_SDA, D_SCL),pc(USBTX, USBRX) { + dmpReady = false; + teapotPacket[0] = '$',teapotPacket[1] = 0x02,teapotPacket[2] = 0,teapotPacket[3] = 0,teapotPacket[4] = 0,teapotPacket[5] = 0,teapotPacket[6] = 0, + teapotPacket[7] = 0,teapotPacket[8] = 0,teapotPacket[9] = 0,teapotPacket[10] = 0x00,teapotPacket[11] = 0x00,teapotPacket[12] = '\r',teapotPacket[13] = '\n',teapotPacket[14] = 0 ; + //Serial pc(USBTX, USBRX); // tx, rx + + pc.baud(D_BAUDRATE); + // initialize device + pc.printf("Initializing I2C devices...\n"); + mpuLocal.initialize(); + // verify connection + pc.printf("Testing device connections...\n"); + + bool mpu6050TestResult = mpuLocal.testConnection(); + if(mpu6050TestResult){ + pc.printf("mpuLocalLocalLocal6050 test passed \n"); + } else{ + pc.printf("mpuLocalLocalLocal6050 test failed \n"); + } + + // wait for ready + pc.printf("\nSend any character to begin DMP programming and demo: "); + + //while(!pc.readable()); + // pc.getc(); + pc.printf("\n"); + + // load and configure the DMP + pc.printf("Initializing DMP...\n"); + devStatus = mpuLocal.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpuLocal.setXGyroOffset(-61); + mpuLocal.setYGyroOffset(-127); + mpuLocal.setZGyroOffset(19); + mpuLocal.setZAccelOffset(16282); // 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 + pc.printf("Enabling DMP...\n"); + mpuLocal.setDMPEnabled(true); + + // enable Arduino interrupt detection + pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n"); +// attachInterrupt(0, dmpDataReady, RISING); + mpuIntStatus = mpuLocal.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + pc.printf("DMP ready! Waiting for first interrupt...\n"); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpuLocal.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) + pc.printf("DMP Initialization failed (code "); + pc.printf("%u",devStatus); + pc.printf(")\n"); + } + + +} + +void AngularDataAcquizition::UpdateTheta(){ + // reset interrupt flag and get INT_STATUS byte + mpuIntStatus = mpuLocal.getIntStatus(); + + // get current FIFO count + fifoCount = mpuLocal.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 + mpuLocal.resetFIFO(); + pc.printf("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 = mpuLocal.getFIFOCount(); + + // read a packet from FIFO + mpuLocal.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; + // display Euler angles in degrees + mpuLocal.dmpGetQuaternion(&q, fifoBuffer); + mpuLocal.dmpGetGravity(&gravity, &q); + mpuLocal.dmpGetYawPitchRoll(ypr, &q, &gravity); + } +} +void AngularDataAcquizition::MeanValue(float *FilteredVal, int num){ + int i,j; + for(j=0;j<3;j++) + FilteredVal[j]=0; + for (i=0;i<num;i++){ + UpdateTheta(); + for(j=0;j<3;j++) + FilteredVal[j]=FilteredVal[j]+ypr[j]; + } + for(j=0;j<3;j++) + FilteredVal[j]=FilteredVal[j]/num; +} +void AngularDataAcquizition::callMeanFilteredReadings(){ + MeanValue(Meanypr,20); + Meanypr[1]=Meanypr[1]-(1.19284-0.00716-0.7679)*3.14/180; + Meanypr[2]=Meanypr[2]-(-0.96533+0.021043+0.107681)*3.14/180; + +} +void AngularDataAcquizition::BeginInterrupt(float InterruptDuration){ + if(dmpReady) + timer.attach(this,&AngularDataAcquizition::callMeanFilteredReadings, InterruptDuration); + } +void AngularDataAcquizition::StopInterrupt(){ + if(dmpReady) + timer.detach(); + } + +#endif +