quadcopter cufe
/
2006_Theta_Control_yaw
Quadcopter Attitude Control(Yaw-Pitch-Roll)
AngularDataAcquizition.h
- Committer:
- khaledelmadawi
- Date:
- 2015-07-03
- Revision:
- 0:e63996fd7d3e
File content as of revision 0:e63996fd7d3e:
#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