quadcopter cufe / Mbed 2 deprecated MPU_6050_nadafa

mpu6050 nadafa

Dependencies:   mbed

AngularDataAcquizition.h@0:8fe8d6dd7cd6, 2015-06-23 (annotated)

Committer:
khaledelmadawi
Date:
Tue Jun 23 14:47:52 2015 +0000
Revision:
0:8fe8d6dd7cd6
mpu nadafa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:8fe8d6dd7cd6 1 #ifndef MBED_AngularDataAcquizition_H
khaledelmadawi 0:8fe8d6dd7cd6 2 #define MBED_AngularDataAcquizition_H
khaledelmadawi 0:8fe8d6dd7cd6 3
khaledelmadawi 0:8fe8d6dd7cd6 4 #include "I2Cdev.h"
khaledelmadawi 0:8fe8d6dd7cd6 5 #include "mbed.h"
khaledelmadawi 0:8fe8d6dd7cd6 6 #include "MPU6050_6Axis_MotionApps20.h" // works
khaledelmadawi 0:8fe8d6dd7cd6 7 #include <math.h>
khaledelmadawi 0:8fe8d6dd7cd6 8
khaledelmadawi 0:8fe8d6dd7cd6 9 #ifndef M_PI
khaledelmadawi 0:8fe8d6dd7cd6 10 #define M_PI 3.1415
khaledelmadawi 0:8fe8d6dd7cd6 11 #endif
khaledelmadawi 0:8fe8d6dd7cd6 12 #define D_BAUDRATE 115200
khaledelmadawi 0:8fe8d6dd7cd6 13
khaledelmadawi 0:8fe8d6dd7cd6 14 class AngularDataAcquizition{
khaledelmadawi 0:8fe8d6dd7cd6 15 public:
khaledelmadawi 0:8fe8d6dd7cd6 16 AngularDataAcquizition(PinName D_SDA, PinName D_SCL);
khaledelmadawi 0:8fe8d6dd7cd6 17 void BeginInterrupt(float InterruptDuration);
khaledelmadawi 0:8fe8d6dd7cd6 18 void StopInterrupt();
khaledelmadawi 0:8fe8d6dd7cd6 19 void UpdateTheta();
khaledelmadawi 0:8fe8d6dd7cd6 20 void MeanValue(float *FilteredVal, int num);
khaledelmadawi 0:8fe8d6dd7cd6 21 void FilterVariablesAquizition(float *variable,int number);
khaledelmadawi 0:8fe8d6dd7cd6 22 void callMeanFilteredReadings();
khaledelmadawi 0:8fe8d6dd7cd6 23
khaledelmadawi 0:8fe8d6dd7cd6 24 float Meanypr[3];
khaledelmadawi 0:8fe8d6dd7cd6 25 bool dmpReady; // set true if DMP init was successful
khaledelmadawi 0:8fe8d6dd7cd6 26
khaledelmadawi 0:8fe8d6dd7cd6 27 private:
khaledelmadawi 0:8fe8d6dd7cd6 28 I2C _i2c;
khaledelmadawi 0:8fe8d6dd7cd6 29 MPU6050 mpuLocal;
khaledelmadawi 0:8fe8d6dd7cd6 30 Ticker timer;
khaledelmadawi 0:8fe8d6dd7cd6 31 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
khaledelmadawi 0:8fe8d6dd7cd6 32 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
khaledelmadawi 0:8fe8d6dd7cd6 33 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
khaledelmadawi 0:8fe8d6dd7cd6 34 uint16_t fifoCount; // count of all bytes currently in FIFO
khaledelmadawi 0:8fe8d6dd7cd6 35 uint8_t fifoBuffer[64]; // FIFO storage buffer
khaledelmadawi 0:8fe8d6dd7cd6 36 // orientation/motion vars
khaledelmadawi 0:8fe8d6dd7cd6 37 Quaternion q; // [w, x, y, z] quaternion container
khaledelmadawi 0:8fe8d6dd7cd6 38 VectorInt16 aa; // [x, y, z] accel sensor measurements
khaledelmadawi 0:8fe8d6dd7cd6 39 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
khaledelmadawi 0:8fe8d6dd7cd6 40 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
khaledelmadawi 0:8fe8d6dd7cd6 41 VectorFloat gravity; // [x, y, z] gravity vector
khaledelmadawi 0:8fe8d6dd7cd6 42 float euler[3]; // [psi, theta, phi] Euler angle container
khaledelmadawi 0:8fe8d6dd7cd6 43 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
khaledelmadawi 0:8fe8d6dd7cd6 44 Serial pc;
khaledelmadawi 0:8fe8d6dd7cd6 45 // packet structure for InvenSense teapot demo
khaledelmadawi 0:8fe8d6dd7cd6 46 uint8_t teapotPacket[15];
khaledelmadawi 0:8fe8d6dd7cd6 47
khaledelmadawi 0:8fe8d6dd7cd6 48 };
khaledelmadawi 0:8fe8d6dd7cd6 49
khaledelmadawi 0:8fe8d6dd7cd6 50
khaledelmadawi 0:8fe8d6dd7cd6 51
khaledelmadawi 0:8fe8d6dd7cd6 52
khaledelmadawi 0:8fe8d6dd7cd6 53
khaledelmadawi 0:8fe8d6dd7cd6 54
khaledelmadawi 0:8fe8d6dd7cd6 55
khaledelmadawi 0:8fe8d6dd7cd6 56 AngularDataAcquizition::AngularDataAcquizition(PinName D_SDA, PinName D_SCL): _i2c(D_SDA, D_SCL),pc(USBTX, USBRX) {
khaledelmadawi 0:8fe8d6dd7cd6 57 dmpReady = false;
khaledelmadawi 0:8fe8d6dd7cd6 58 teapotPacket[0] = '$',teapotPacket[1] = 0x02,teapotPacket[2] = 0,teapotPacket[3] = 0,teapotPacket[4] = 0,teapotPacket[5] = 0,teapotPacket[6] = 0,
khaledelmadawi 0:8fe8d6dd7cd6 59 teapotPacket[7] = 0,teapotPacket[8] = 0,teapotPacket[9] = 0,teapotPacket[10] = 0x00,teapotPacket[11] = 0x00,teapotPacket[12] = '\r',teapotPacket[13] = '\n',teapotPacket[14] = 0 ;
khaledelmadawi 0:8fe8d6dd7cd6 60 //Serial pc(USBTX, USBRX); // tx, rx
khaledelmadawi 0:8fe8d6dd7cd6 61
khaledelmadawi 0:8fe8d6dd7cd6 62 pc.baud(D_BAUDRATE);
khaledelmadawi 0:8fe8d6dd7cd6 63 // initialize device
khaledelmadawi 0:8fe8d6dd7cd6 64 pc.printf("Initializing I2C devices...\n");
khaledelmadawi 0:8fe8d6dd7cd6 65 mpuLocal.initialize();
khaledelmadawi 0:8fe8d6dd7cd6 66 // verify connection
khaledelmadawi 0:8fe8d6dd7cd6 67 pc.printf("Testing device connections...\n");
khaledelmadawi 0:8fe8d6dd7cd6 68
khaledelmadawi 0:8fe8d6dd7cd6 69 bool mpu6050TestResult = mpuLocal.testConnection();
khaledelmadawi 0:8fe8d6dd7cd6 70 if(mpu6050TestResult){
khaledelmadawi 0:8fe8d6dd7cd6 71 pc.printf("mpuLocalLocalLocal6050 test passed \n");
khaledelmadawi 0:8fe8d6dd7cd6 72 } else{
khaledelmadawi 0:8fe8d6dd7cd6 73 pc.printf("mpuLocalLocalLocal6050 test failed \n");
khaledelmadawi 0:8fe8d6dd7cd6 74 }
khaledelmadawi 0:8fe8d6dd7cd6 75
khaledelmadawi 0:8fe8d6dd7cd6 76 // wait for ready
khaledelmadawi 0:8fe8d6dd7cd6 77 pc.printf("\nSend any character to begin DMP programming and demo: ");
khaledelmadawi 0:8fe8d6dd7cd6 78
khaledelmadawi 0:8fe8d6dd7cd6 79 //while(!pc.readable());
khaledelmadawi 0:8fe8d6dd7cd6 80 // pc.getc();
khaledelmadawi 0:8fe8d6dd7cd6 81 pc.printf("\n");
khaledelmadawi 0:8fe8d6dd7cd6 82
khaledelmadawi 0:8fe8d6dd7cd6 83 // load and configure the DMP
khaledelmadawi 0:8fe8d6dd7cd6 84 pc.printf("Initializing DMP...\n");
khaledelmadawi 0:8fe8d6dd7cd6 85 devStatus = mpuLocal.dmpInitialize();
khaledelmadawi 0:8fe8d6dd7cd6 86
khaledelmadawi 0:8fe8d6dd7cd6 87 // supply your own gyro offsets here, scaled for min sensitivity
khaledelmadawi 0:8fe8d6dd7cd6 88 mpuLocal.setXGyroOffset(-61);
khaledelmadawi 0:8fe8d6dd7cd6 89 mpuLocal.setYGyroOffset(-127);
khaledelmadawi 0:8fe8d6dd7cd6 90 mpuLocal.setZGyroOffset(19);
khaledelmadawi 0:8fe8d6dd7cd6 91 mpuLocal.setZAccelOffset(16282); // 1688 factory default for my test chip
khaledelmadawi 0:8fe8d6dd7cd6 92
khaledelmadawi 0:8fe8d6dd7cd6 93 // make sure it worked (returns 0 if so)
khaledelmadawi 0:8fe8d6dd7cd6 94 if (devStatus == 0) {
khaledelmadawi 0:8fe8d6dd7cd6 95 // turn on the DMP, now that it's ready
khaledelmadawi 0:8fe8d6dd7cd6 96 pc.printf("Enabling DMP...\n");
khaledelmadawi 0:8fe8d6dd7cd6 97 mpuLocal.setDMPEnabled(true);
khaledelmadawi 0:8fe8d6dd7cd6 98
khaledelmadawi 0:8fe8d6dd7cd6 99 // enable Arduino interrupt detection
khaledelmadawi 0:8fe8d6dd7cd6 100 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n");
khaledelmadawi 0:8fe8d6dd7cd6 101 // attachInterrupt(0, dmpDataReady, RISING);
khaledelmadawi 0:8fe8d6dd7cd6 102 mpuIntStatus = mpuLocal.getIntStatus();
khaledelmadawi 0:8fe8d6dd7cd6 103
khaledelmadawi 0:8fe8d6dd7cd6 104 // set our DMP Ready flag so the main loop() function knows it's okay to use it
khaledelmadawi 0:8fe8d6dd7cd6 105 pc.printf("DMP ready! Waiting for first interrupt...\n");
khaledelmadawi 0:8fe8d6dd7cd6 106 dmpReady = true;
khaledelmadawi 0:8fe8d6dd7cd6 107
khaledelmadawi 0:8fe8d6dd7cd6 108 // get expected DMP packet size for later comparison
khaledelmadawi 0:8fe8d6dd7cd6 109 packetSize = mpuLocal.dmpGetFIFOPacketSize();
khaledelmadawi 0:8fe8d6dd7cd6 110 } else {
khaledelmadawi 0:8fe8d6dd7cd6 111 // ERROR!
khaledelmadawi 0:8fe8d6dd7cd6 112 // 1 = initial memory load failed
khaledelmadawi 0:8fe8d6dd7cd6 113 // 2 = DMP configuration updates failed
khaledelmadawi 0:8fe8d6dd7cd6 114 // (if it's going to break, usually the code will be 1)
khaledelmadawi 0:8fe8d6dd7cd6 115 pc.printf("DMP Initialization failed (code ");
khaledelmadawi 0:8fe8d6dd7cd6 116 pc.printf("%u",devStatus);
khaledelmadawi 0:8fe8d6dd7cd6 117 pc.printf(")\n");
khaledelmadawi 0:8fe8d6dd7cd6 118 }
khaledelmadawi 0:8fe8d6dd7cd6 119
khaledelmadawi 0:8fe8d6dd7cd6 120
khaledelmadawi 0:8fe8d6dd7cd6 121 }
khaledelmadawi 0:8fe8d6dd7cd6 122
khaledelmadawi 0:8fe8d6dd7cd6 123 void AngularDataAcquizition::UpdateTheta(){
khaledelmadawi 0:8fe8d6dd7cd6 124 // reset interrupt flag and get INT_STATUS byte
khaledelmadawi 0:8fe8d6dd7cd6 125 mpuIntStatus = mpuLocal.getIntStatus();
khaledelmadawi 0:8fe8d6dd7cd6 126
khaledelmadawi 0:8fe8d6dd7cd6 127 // get current FIFO count
khaledelmadawi 0:8fe8d6dd7cd6 128 fifoCount = mpuLocal.getFIFOCount();
khaledelmadawi 0:8fe8d6dd7cd6 129
khaledelmadawi 0:8fe8d6dd7cd6 130 // check for overflow (this should never happen unless our code is too inefficient)
khaledelmadawi 0:8fe8d6dd7cd6 131 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
khaledelmadawi 0:8fe8d6dd7cd6 132 // reset so we can continue cleanly
khaledelmadawi 0:8fe8d6dd7cd6 133 mpuLocal.resetFIFO();
khaledelmadawi 0:8fe8d6dd7cd6 134 pc.printf("FIFO overflow!");
khaledelmadawi 0:8fe8d6dd7cd6 135
khaledelmadawi 0:8fe8d6dd7cd6 136 // otherwise, check for DMP data ready interrupt (this should happen frequently)
khaledelmadawi 0:8fe8d6dd7cd6 137 } else if (mpuIntStatus & 0x02) {
khaledelmadawi 0:8fe8d6dd7cd6 138 // wait for correct available data length, should be a VERY short wait
khaledelmadawi 0:8fe8d6dd7cd6 139 while (fifoCount < packetSize) fifoCount = mpuLocal.getFIFOCount();
khaledelmadawi 0:8fe8d6dd7cd6 140
khaledelmadawi 0:8fe8d6dd7cd6 141 // read a packet from FIFO
khaledelmadawi 0:8fe8d6dd7cd6 142 mpuLocal.getFIFOBytes(fifoBuffer, packetSize);
khaledelmadawi 0:8fe8d6dd7cd6 143
khaledelmadawi 0:8fe8d6dd7cd6 144 // track FIFO count here in case there is > 1 packet available
khaledelmadawi 0:8fe8d6dd7cd6 145 // (this lets us immediately read more without waiting for an interrupt)
khaledelmadawi 0:8fe8d6dd7cd6 146 fifoCount -= packetSize;
khaledelmadawi 0:8fe8d6dd7cd6 147 // display Euler angles in degrees
khaledelmadawi 0:8fe8d6dd7cd6 148 mpuLocal.dmpGetQuaternion(&q, fifoBuffer);
khaledelmadawi 0:8fe8d6dd7cd6 149 mpuLocal.dmpGetGravity(&gravity, &q);
khaledelmadawi 0:8fe8d6dd7cd6 150 mpuLocal.dmpGetYawPitchRoll(ypr, &q, &gravity);
khaledelmadawi 0:8fe8d6dd7cd6 151 }
khaledelmadawi 0:8fe8d6dd7cd6 152 }
khaledelmadawi 0:8fe8d6dd7cd6 153 void AngularDataAcquizition::MeanValue(float *FilteredVal, int num){
khaledelmadawi 0:8fe8d6dd7cd6 154 int i,j;
khaledelmadawi 0:8fe8d6dd7cd6 155 for(j=0;j<3;j++)
khaledelmadawi 0:8fe8d6dd7cd6 156 FilteredVal[j]=0;
khaledelmadawi 0:8fe8d6dd7cd6 157 for (i=0;i<num;i++){
khaledelmadawi 0:8fe8d6dd7cd6 158 UpdateTheta();
khaledelmadawi 0:8fe8d6dd7cd6 159 for(j=0;j<3;j++)
khaledelmadawi 0:8fe8d6dd7cd6 160 FilteredVal[j]=FilteredVal[j]+ypr[j];
khaledelmadawi 0:8fe8d6dd7cd6 161 }
khaledelmadawi 0:8fe8d6dd7cd6 162 for(j=0;j<3;j++)
khaledelmadawi 0:8fe8d6dd7cd6 163 FilteredVal[j]=FilteredVal[j]/num;
khaledelmadawi 0:8fe8d6dd7cd6 164 }
khaledelmadawi 0:8fe8d6dd7cd6 165 void AngularDataAcquizition::callMeanFilteredReadings(){
khaledelmadawi 0:8fe8d6dd7cd6 166 MeanValue(Meanypr,20);
khaledelmadawi 0:8fe8d6dd7cd6 167 Meanypr[1]=Meanypr[1]-(1.19284-0.00716-0.7679)*3.14/180;
khaledelmadawi 0:8fe8d6dd7cd6 168 Meanypr[2]=Meanypr[2]-(-0.96533+0.021043+0.107681)*3.14/180;
khaledelmadawi 0:8fe8d6dd7cd6 169
khaledelmadawi 0:8fe8d6dd7cd6 170 }
khaledelmadawi 0:8fe8d6dd7cd6 171 void AngularDataAcquizition::BeginInterrupt(float InterruptDuration){
khaledelmadawi 0:8fe8d6dd7cd6 172 if(dmpReady)
khaledelmadawi 0:8fe8d6dd7cd6 173 timer.attach(this,&AngularDataAcquizition::callMeanFilteredReadings, InterruptDuration);
khaledelmadawi 0:8fe8d6dd7cd6 174 }
khaledelmadawi 0:8fe8d6dd7cd6 175 void AngularDataAcquizition::StopInterrupt(){
khaledelmadawi 0:8fe8d6dd7cd6 176 if(dmpReady)
khaledelmadawi 0:8fe8d6dd7cd6 177 timer.detach();
khaledelmadawi 0:8fe8d6dd7cd6 178 }
khaledelmadawi 0:8fe8d6dd7cd6 179
khaledelmadawi 0:8fe8d6dd7cd6 180 #endif
khaledelmadawi 0:8fe8d6dd7cd6 181