File content as of revision 1:e08a4f517989:

/*
                         F                  +X
                         |                   | 
                         |                   |
                 L ------|------R    -Y------|------+Y
                         |                   |
                         |                   |
                         B                  -X
*/
#include "mbed.h"
#include "rtos.h"
#include "GlobalDefines.h"
#include "MPU6050.h"
#include "BMP085.h"
//#include "DCM.h" Drifts a lot, needs tuning 
#include "PID.h"
#include "Motors.h"

#define Debugging

typedef enum{Armed=0,
UnArmed
}State;

State MotorState = Armed;
bool print = false;
bool P_readingReady=false;
bool T_readingReady=false;
char c ;

uint8 count=0;
int16 Temperature=0;
int32 Pressure=0;


PIDClass PitchPID(4,5,2.5,0.1); // This is a nested PID loop
                               // Kp,Ki,Kp,Kd,iLim..... best till now 4,0,2.5,0.1
MPU6050 Sensor;
BMP085 BMP;
//DCM IMU;

void FlightControl(void const *args){

        //Get Sensor Data
        Sensor.GyroRate();
        Sensor.AccAngle();
        
        ///////////////////////BMP085 Contorol switch statement(Work in Progress!!!)//////////////////////////////////////
        switch(count){
        
        case 0: 
        BMP.readUT_Flag();
        count+=1; 
        break;
        
        
        case 1: 
        Temperature=BMP.read_Temperature();
        T_readingReady=true;
        BMP.readUP_Flag();
        count+=1;
        break;
        
        
        case 7: 
        Pressure = BMP.read_Pressure();
        P_readingReady=true;
        count=0;
        break;
        
        default: count+=1; break;
        }
    ///////////////////////////////////////////////////////////////////////////////////////////
    
        //Gotta Keep Time
        deltaTime = Global.read();
        Global.reset();
     
         pitch = (0.99)*(pitch+Sensor.Scaled_GyroX*deltaTime) + (0.01)*(Sensor.accangle[0]);
         roll  = (0.99)*(roll+ Sensor.Scaled_GyroY*deltaTime) + (0.01)*(Sensor.accangle[1]);
        

         //IMU.Update_DCM(deltaTime,ToRad(Sensor.Scaled_GyroX),ToRad(Sensor.Scaled_GyroY),ToRad(Sensor.Scaled_GyroZ),Sensor.Scaled_AccX,Sensor.Scaled_AccY,Sensor.Scaled_AccZ);

        //Update PID 
        if(Throttle>=1200){
        PitchCorrection= PitchPID.update(PitchSetpoint,pitch,Sensor.Scaled_GyroX,deltaTime);
        }
        
        //Motor Commands
        if(MotorState==Armed){
        updateMotors(PitchCorrection,0);
        }
    
        print = true;
}


int main(){
    
    #ifdef Debugging
    pc.baud(115200);
    #endif
    
    //Start-Up functions
    __disable_irq();
     CalibrateESC(); // Throttle Sweep on startup
     two=1;        // check sensor connection
     while(!Sensor.CheckConnection()); //check sensor connection
     two=0;        // sensor okay 
     BMP.Calibrate(); // Calibrations sequence for BMP... TODO: store values in LocalFileSystem
     Sensor.MPU_Setup(); // Setup MPU6050
     Sensor.CalibrateGyro(); // Calibrate Gyros  (averaging 100 readings)
     Sensor.CalibrateAcc(); //  Calibrate ACC    (averaging 100 readings)
     one=1;   // System okay to start 
     __enable_irq();    
     
     RtosTimer Fly(FlightControl,osTimerPeriodic,NULL); // Initialize RTOS Timer  
     Global.start(); // Start timer
     Fly.start(5);  // RTOS Timer (200Hz Control Loop)
 
 while(true){
           
          if(pc.readable()){
          c = pc.getc();
          if(c == 'a'||c=='A') MotorState = Armed;
            else if(c == 's'||c=='S') MotorState = UnArmed;
                else if(c == 'u'||c=='U') Throttle+=50 ;
                    else if(c == 'i'||c=='I') Throttle-=50;
                        else if(c=='m'||c=='M') PitchSetpoint+=10;
                            else if(c=='n'||c=='N') PitchSetpoint-=10;

          //print=true;
          }
          
          #ifdef Debugging
          if(print){
          //pc.printf("%d\n",Temperature);
          pc.printf("%6.2f\n",pitch);
          //T_readingReady=false;
          print=false;
          }
          #endif
          
          Thread::wait(20);         
}
}