quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

AngularDataAcquizition.h@0:e63996fd7d3e, 2015-07-03 (annotated)

Committer:
khaledelmadawi
Date:
Fri Jul 03 11:16:02 2015 +0000
Revision:
0:e63996fd7d3e
Quadcopter Attitude Control(Yaw-Pitch-Roll)

Who changed what in which revision?

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