File content as of revision 0:4a55d0a21ea9:

#include <math.h>
#include "I2Cdev.h"
#include "mbed.h"
#include "MPU6050_6Axis_MotionApps20.h" 

#include "PID.h"

#include "RangeFinder.h"
#ifndef M_PI
#define M_PI 3.1415
#endif
#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_QUATERNION
#define OUTPUT_READABLE_ACCELGYRO

////// VARIABLES //////
///ultrasonic variable///
float d=0;
//////////////////


/// PID VARIABLES ///
float KPRoll=5;
float KIRoll=1;
float KDRoll=0.5;

float KPPitch=5;
float KIPitch=1.1;
float KDPitch=0.5;//

float KPRPITCH=2;
float KIRPITCH=0;
float KDRPITCH=0.05;

float KPRROLL=2;
float KIRROLL=0;
float KDRROLL=0.05;

float KPHIGHT=5;
float KIHIGHT=1;
float KDHIGHT=0.1;

float  SetpointROLL=0;
float  RateRequired=0;
float  SetpointPITCH=0;

float  InputPITCH, OutputPITCH=0,RATEXinput=0,RATEXoutput=0,SetpointPitchrate=0,SetpointRollrate=0;
float  InputROLL, OutputROLL=0,RATEYinput=0,RATEYoutput=0;
float Input_altitude,delta_trotle=0,Setpointaltitude=600;
/// ESC VARIABLES ///
float in_pulse=1200;      // ranging from 0 to 1 
/// MPU VARIABLES ///
bool blinkState = false;
bool dmpReady = false; // set true if DMP init was successful
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
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
uint8_t teapotPacket[15] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n',0 };
float mean_yaw=0 ;
//int16_t &ax, &ay, &az;
int16_t gx, gy, gz;

// CALIBRATION VARIABLES //
float AngleYaw,pAngleYaw,AnglePitch,pAnglePitch,AngleRoll,pAngleRoll;
float AngleOffset[3];
int16_t GyroOffset[3];
int16_t Rate_x,Rate_y,Rate_z,pgx,pgy,pgz;
int start_flag=0;
////// INITIAL SETUP //////
////// mbed setup ///////
Serial pc(p13 ,p14); //xbee 13/14
/// PID SETUP /// 

PID PITCHPID(&InputPITCH, &OutputPITCH, &SetpointPITCH,KPPitch,KIPitch,KDPitch);
PID PITCHPIDrate(&RATEYinput, &RATEYoutput,&SetpointPitchrate,KPRPITCH,KIRPITCH,KDRPITCH);
PID ROLLPID(&InputROLL, &OutputROLL, &SetpointROLL,KPRoll,KIRoll,KDRoll);
PID ROLLPIDrate(&RATEXinput, &RATEXoutput,&SetpointRollrate,KPRROLL,KIRROLL,KDRROLL);
PID altitudecontroller(&Input_altitude, &delta_trotle, &Setpointaltitude,KPHIGHT,KIHIGHT,KDHIGHT);
//PID altitudedampingcontroller(&Zvelocity, &trotle_damping,&Setpoint_Zdamping,KPDAMPING,KIDAMPING,KDDAMPING);  //dont forget to declare the variables
/// MPU SETUP ///
MPU6050 mpu;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
    }
/// Motors SETUP ///
PwmOut esc1(p21);
PwmOut esc2(p22);
PwmOut esc3(p23);
PwmOut esc4(p24);

///ultrasonic setup////
RangeFinder rf(p20, 10, 5800.0, 100000);
///////////////////////////    



////// FUNCTION DECLARATIONS //////


/*void mpu_code();
void send_data();
void calibration();*/
////// FUNCTIONS //////

void mpu_code(){
    
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();
        pc.printf("FIFO overflow!");
    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_QUATERNION
            mpu.dmpGetQuaternion(&q, fifoBuffer);
          /*pc.printf("%f",q.w);
            pc.printf(",");
            pc.printf("%f",q.x);
            pc.printf(",");
            pc.printf("%f",q.y);
            pc.printf(",");
            pc.printf("%f\n",q.z);*/
#endif

#ifdef OUTPUT_READABLE_EULER
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
         /* pc.printf("euler\t");
            pc.printf("%f",euler[0] * 180/M_PI);
            pc.printf("\t");
            pc.printf("%f",euler[1] * 180/M_PI);
            pc.printf("\t");
            pc.printf("%f\n",euler[2] * 180/M_PI);*/
#endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
#endif

#ifdef OUTPUT_READABLE_ACCELGYRO
          mpu.getRotation(&gx, &gy, &gz);
           // mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
            /*pc.printf("a/g:\t");
            pc.printf(gx); pc.printf("\t");
            pc.printf(gy); pc.printf("\t");
            pc.printf(gz); pc.printf("\t");
            pc.printf(gz);*/
#endif
    
#ifdef OUTPUT_READABLE_REALACCEL
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
#endif

#ifdef OUTPUT_READABLE_WORLDACCEL
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
#endif
#ifdef OUTPUT_TEAPOT
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            
            for(int i=0;i<14;i++)
            {
            pc.putc(teapotPacket[i]);
            }
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
#endif
    
        }
}



void send_data(){
    /////////// Pitch Final output calibration ///////////////
    
    int Final_output_x1 = 1100+OutputPITCH+delta_trotle-RATEYoutput;  
    int Final_output_x2 = 1100-OutputPITCH+delta_trotle+RATEYoutput;  
    float min=1050;
    if (Final_output_x1 < min){
        Final_output_x1=min;
    }
    else if (Final_output_x2 <min){
        Final_output_x2=min;
    }
    /////////// Roll Final output calibration ///////////////
    
    int Final_output_y1 = 1100+OutputROLL+delta_trotle+RATEXoutput;  
    int Final_output_y2 = 1100-OutputROLL+delta_trotle-RATEXoutput;  
    if (Final_output_y1 < min){
        Final_output_y1=min;
    }
    else if (Final_output_y2 <min){
        Final_output_y2=min;
    }
    
   ////////////////  Altitude final output calibration/////////
   
   
   
   
   //////////////// FINALIZATION /////////////////
    esc1.pulsewidth_us(Final_output_y2);
    esc2.pulsewidth_us(Final_output_x1);
    esc3.pulsewidth_us(Final_output_y1);
    esc4.pulsewidth_us(Final_output_x2);
    
}


void calibration(){
       if(pc.readable()) {
        switch (pc.getc()) {
            case 'c':
                AngleOffset[0]=ypr[0];
                AngleOffset[1]=ypr[1];
                AngleOffset[2]=ypr[2];
                GyroOffset[0]=gx;
                GyroOffset[1]=gy;
                GyroOffset[2]=gz;
                break;
            case 'x':
                start_flag=0;
            case 's':
                start_flag=1;
            }
        }
}
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////

 int main()
{
    
    esc1.period_ms(20);
    esc2.period_ms(20);
    esc3.period_ms(20);
    esc4.period_ms(20);  
#define D_SDA                  p9
#define D_SCL                  p10
I2C i2c(D_SDA, D_SCL);
    DigitalOut myled1(LED1);
    DigitalOut myled2(LED2);
    DigitalOut myled3(LED3);
    DigitalOut heartbeatLED(LED4);
    #define D_BAUDRATE            115200
    pc.baud(D_BAUDRATE);
    pc.printf("Initializing I2C devices...\n");
    mpu.initialize();
    pc.printf("Testing device connections...\n");
    bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult){
        pc.printf("MPU6050 test passed \n");
    } else{
        pc.printf("MPU6050 test failed \n");
    }  
    pc.printf("\nSend any character to begin DMP programming and demo: ");
    while(!pc.readable());
            pc.getc();
    pc.printf("\n");
    pc.printf("Initializing DMP...\n");
    devStatus = mpu.dmpInitialize();
    mpu.setXGyroOffset(-61);
    mpu.setYGyroOffset(-127);
    mpu.setZGyroOffset(19);
    mpu.setZAccelOffset(16282);
    if (devStatus == 0) {
        pc.printf("Enabling DMP...\n");
        mpu.setDMPEnabled(true);
        pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n");
        mpuIntStatus = mpu.getIntStatus();
        pc.printf("DMP ready! Waiting for first interrupt...\n");
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        pc.printf("DMP Initialization failed (code ");
        pc.printf("%u",devStatus);
        pc.printf(")\n");
    }
    
    esc1.pulsewidth_us(in_pulse);
    esc2.pulsewidth_us(in_pulse);
    esc3.pulsewidth_us(in_pulse);
    esc4.pulsewidth_us(in_pulse);
    
    
//pc.printf("i am entering the loop");

///////////////////////////////////////////
// PROGRAM LOOP //
///////////////////////////////////////////

while(1)
{
   
   
   // setting an old value for the readings to use it in the filter 
    
    pAngleYaw = AngleYaw;
    pAnglePitch = AnglePitch;
    pAngleRoll= AngleRoll;
    pgx = Rate_x;
    pgy = Rate_y;
    pgz = Rate_z;
    mpu_code();

    calibration();
//pc.printf("g");
    AngleYaw = ypr[0] - AngleOffset[0];        // calibrating the readings
    AnglePitch=ypr[1] - AngleOffset[1];
    AngleRoll=ypr[2] - AngleOffset[2]; 
    Rate_x =gx - GyroOffset[0];
    Rate_y =gy - GyroOffset[1];
    Rate_z =gz - GyroOffset[2];
    AngleYaw = 0.7 * pAngleYaw + 0.3 * AngleYaw;               // low pass filter
    AnglePitch = 0.7 * pAnglePitch + 0.3 * AnglePitch;
    AngleRoll = 0.7 * pAngleRoll + 0.3 * AngleRoll;
    Rate_x = 0.6 * pgx + 0.4 * Rate_x;
    Rate_y = 0.6 * pgy + 0.4 * Rate_y;
    Rate_z = 0.6 * pgz + 0.4 * Rate_z;
   
    InputPITCH=AnglePitch * 180/M_PI;
    InputROLL=AngleRoll * 180/M_PI;

 RATEXinput=Rate_x;
    RATEYinput=Rate_y;
    
    
  
   ///// measure distance ultrasonic////////
 /* d = rf.read_m();
        if (d == -1.0)  {
            printf("Timeout Error.\n");   
        } else if (d > 5.0) {  
            // Seeed's sensor has a maximum range of 4m, it returns
            // something like 7m if the ultrasound pulse isn't reflected. 
            pc.printf("No object within detection range.\n");
        } else  {
            pc.printf("p:%f r:%f \n\r",AnglePitch*180/M_PI,AngleRoll*180/M_PI);
        }*/
   //////////////////////////
   
   
    if (start_flag==1){
        //PITCHPID.Compute();
        //ROLLPID.Compute();
        PITCHPIDrate.Compute();
        ROLLPIDrate.Compute();
        //altitudecontroller.Compute();
        send_data();
    } else{   
        esc1.pulsewidth_us(1000);
        esc2.pulsewidth_us(1000);
        esc3.pulsewidth_us(1000);
        esc4.pulsewidth_us(1000);
        start_flag=0;
    }
    
    
   //pc.printf("pitch:%f roll:%f\r\n",AnglePitch*180/M_PI,AngleRoll*180/M_PI);  
  pc.printf("rate y:%d rate x:%d\r\n",Rate_y,Rate_x);
    blinkState = !blinkState;
    myled1 = blinkState;

 
}
}