Revision 1:e08a4f517989, committed 2013-08-27

Comitter:

KarimAzzouz

Date:

Tue Aug 27 09:38:49 2013 +0000

Parent:

0:54b67cd15a5b

Commit message:

initial commit, achieved single axis stability

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085.cpp	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,129 @@
+#include "BMP085.h"
+
+BMP085::BMP085(){}
+
+void BMP085::write(char reg,char data){
+    
+    char tx[2]={reg,data};
+
+    I2C0.blockwrite((BMP_ADDRESS << 1)&0xFE, tx, 2);
+
+ }
+
+int16 BMP085::read (char reg){
+
+    char tx = reg;
+    char rx[2];
+
+    I2C0.blockwrite((BMP_ADDRESS << 1)&0xFE, &tx, 1);
+
+    I2C0.blockread((BMP_ADDRESS << 1)|0x01, rx, 2);
+
+    int16 output = ((int16) rx[0] << 8) | ((int16) rx[1]);
+    return output;
+}
+
+uint8 BMP085::readInt (char reg){
+
+    char tx = reg;
+    char rx;
+
+    I2C0.blockwrite((BMP_ADDRESS << 1)&0xFE, &tx, 1);
+
+    I2C0.blockread((BMP_ADDRESS << 1)|0x01, &rx, 1);
+    
+    return rx;
+}
+
+// Get Calibration Values
+void BMP085::Calibrate(void){
+
+  AC1 = read(0xAA);
+  AC2 = read(0xAC);
+  AC3 = read(0xAE);
+  AC4 = read(0xB0);
+  AC5 = read(0xB2);
+  AC6 = read(0xB4);
+  B1  = read(0xB6);
+  B2  = read(0xB8);
+  MB  = read(0xBA);
+  MC  = read(0xBC);
+  MD  = read(0xBE);
+ 
+}
+
+//Uncompensated temperature takes 4.5ms to read, so we'll just send a read flag and read the data the next cycle since our control loop repeats every 5 ms 
+
+void BMP085::readUT_Flag(void){
+
+  write(0xF4,0x2E);
+}
+
+// reading uncompensated Pressure depends on our resolution setting, we'll select ultra high resolution setting so the delay will be 25.5ms (5 iterations);
+
+void BMP085::readUP_Flag(void){
+
+  write(0xF4,(0x34 + (OSS<<6)));
+}
+
+uint16 BMP085::readUT(void){
+  
+  uint16 ut = (uint16)read(0xF6);
+  
+  return ut;
+  
+}
+uint32 BMP085::readUP(void){
+  
+  uint32 up = (((uint32) readInt(0xF6) << 16) | ((uint32) readInt(0xF7) << 8) | (uint32) readInt(0xF8)) >> (8-OSS);
+  
+  return up;
+}
+
+int16 BMP085::read_Temperature(void){
+
+ int32 X1,X2;
+ 
+ X1 = (((int32)readUT() - (int32)AC6)*(int32)AC5) >> 15;
+ X2 = ((int32)MC << 11)/(X1 + MD);
+ 
+ B5 = X1+X2;
+ 
+ return ((B5 + 8)>>4); 
+}
+
+int32 BMP085::read_Pressure(void){
+  
+  int32 X1, X2, X3, B3, B6, p;
+  uint32 B4, B7;
+  
+  B6 = B5 - 4000;
+  // Calculate B3
+  X1 = (B2 * (B6 * B6)>>12)>>11;
+  X2 = (AC2 * B6)>>11;
+  X3 = X1 + X2;
+  B3 = (((((int32)AC1)*4 + X3)<<OSS) + 2)>>2;
+  
+  // Calculate B4
+  X1 = (AC3 * B6)>>13;
+  X2 = (B1 * ((B6 * B6)>>12))>>16;
+  X3 = ((X1 + X2) + 2)>>2;
+  B4 = (AC4 * (uint32)(X3 + 32768))>>15;
+  
+  B7 = ((uint32)(readUP() - B3) * (50000>>OSS));
+  
+  if (B7 < (uint32)0x80000000)
+  {
+    p = (B7<<1)/B4;
+    }
+  else{
+    p = (B7/B4)<<1;
+    }
+    
+  X1 = (p>>8) * (p>>8);
+  X1 = (X1 * 3038)>>16;
+  X2 = (-7357 * p)>>16;
+  p += (X1 + X2 + 3791)>>4;
+  
+  return p;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085.h	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,37 @@
+#ifndef BMP085_H
+#define BMP085_H
+
+#include "Std_Types.h"
+#include "MyI2C.h"
+
+#define BMP_ADDRESS 0x77
+#define OSS 3
+
+extern MyI2C I2C0;
+
+class BMP085{
+
+ public:
+ BMP085();
+ void Calibrate(void);
+ void readUT_Flag(void);
+ void readUP_Flag(void);
+ uint16 readUT(void);
+ uint32 readUP(void);
+ int16 read_Temperature(void);
+ int32 read_Pressure(void);
+ private:
+ 
+ int16 AC1,AC2,AC3,AC4,AC5,AC6;
+ int16 B1,B2;
+ int16 MB,MC,MD;
+ int32 B5;
+ 
+ void write(char reg,char data);
+
+ int16 read (char reg);
+ 
+ uint8 readInt (char reg);
+ 
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM/DCM.cpp	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,197 @@
+#include"DCM.h"
+#include "HelperMath.h"
+
+float DCM::constrain(float x, float a, float b) 
+{
+    float result = x;
+    
+    if (x < a) result = a;
+    if (x > b) result = b;
+ 
+    return result;  
+}
+
+DCM::DCM(){
+
+  G_Dt=0.005;
+  
+  for(int i=0;i<3;i++){
+  Accel_Vector[i]=0;
+  Gyro_Vector[i]= 0;
+  Omega_Vector[i]=0;
+  Omega_P[i]=0;
+  Omega_I[i]=0;
+  Omega[i]= 0;
+  errorRollPitch[i]=0;
+  }
+  
+  DCM_Matrix[0][0]=1;
+  DCM_Matrix[0][1]=0;
+  DCM_Matrix[0][2]=0;
+  DCM_Matrix[1][0]=0;
+  DCM_Matrix[1][1]=1;
+  DCM_Matrix[1][2]=0;
+  DCM_Matrix[2][0]=0;
+  DCM_Matrix[2][1]=0;
+  DCM_Matrix[2][2]=1;
+  
+  Temporary_Matrix[0][0]=0;
+  Temporary_Matrix[0][1]=0;
+  Temporary_Matrix[0][2]=0;
+  Temporary_Matrix[1][0]=0;
+  Temporary_Matrix[1][1]=0;
+  Temporary_Matrix[1][2]=0;
+  Temporary_Matrix[2][0]=0;
+  Temporary_Matrix[2][1]=0;
+  Temporary_Matrix[2][2]=0;
+  
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=1;
+  Update_Matrix[0][2]=2;
+  Update_Matrix[1][0]=3;
+  Update_Matrix[1][1]=4;
+  Update_Matrix[1][2]=5;
+  Update_Matrix[2][0]=6;
+  Update_Matrix[2][1]=7;
+  Update_Matrix[2][2]=8;
+}
+/**************************************************/
+void DCM::Normalize(void)
+{
+  float error=0;
+  float temporary[3][3];
+  float renorm=0;
+  bool problem=false;
+  
+  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
+
+  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+  
+  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
+  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
+  
+  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+  
+  renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); 
+  if (renorm < 1.5625f && renorm > 0.64f) {
+    renorm= .5 * (3-renorm);                                                 //eq.21
+  } else if (renorm < 100.0f && renorm > 0.01f) {
+    renorm= 1. / sqrt(renorm);
+  }
+      Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+  
+  renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); 
+  if (renorm < 1.5625f && renorm > 0.64f) {
+    renorm= .5 * (3-renorm);                                                 //eq.21
+  } else if (renorm < 100.0f && renorm > 0.01f) {
+    renorm= 1. / sqrt(renorm);  
+  } else {
+    problem = true;
+  }
+  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+  
+  renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); 
+  if (renorm < 1.5625f && renorm > 0.64f) {
+    renorm= .5 * (3-renorm);                                                 //eq.21
+  } else if (renorm < 100.0f && renorm > 0.01f) {
+    renorm= 1. / sqrt(renorm);   
+
+  } else {
+    problem = true;  
+  }
+  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+  
+  if (problem) {                // Our solution is blowing up and we will force back to initial condition.  Hope we are not upside down!
+      DCM_Matrix[0][0]= 1.0f;
+      DCM_Matrix[0][1]= 0.0f;
+      DCM_Matrix[0][2]= 0.0f;
+      DCM_Matrix[1][0]= 0.0f;
+      DCM_Matrix[1][1]= 1.0f;
+      DCM_Matrix[1][2]= 0.0f;
+      DCM_Matrix[2][0]= 0.0f;
+      DCM_Matrix[2][1]= 0.0f;
+      DCM_Matrix[2][2]= 1.0f;
+      problem = false;  
+  }
+}
+
+/**************************************************/
+void DCM::Drift_correction(void)
+{
+  //Compensation the Roll, Pitch and Yaw drift. 
+  static float Scaled_Omega_I[3];
+  float Accel_magnitude;
+  float Accel_weight;
+  
+  //*****Roll and Pitch***************
+
+  // Calculate the magnitude of the accelerometer vector
+  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+  Accel_magnitude /=GRAVITY; // Scale to gravity.
+  // Dynamic weighting of accelerometer info (reliability filter)
+  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
+  
+  
+  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
+  
+}
+
+void DCM::Matrix_update(float GyroX,float GyroY,float GyroZ, float AccX, float AccY, float AccZ)
+{
+  Gyro_Vector[0]=GyroX; //gyro x roll
+  Gyro_Vector[1]=GyroY; //gyro y pitch
+  Gyro_Vector[2]=GyroZ; //gyro Z yaw
+  
+  Accel_Vector[0]=AccX*GRAVITY; // acc x
+  Accel_Vector[1]=AccY*GRAVITY; // acc y
+  Accel_Vector[2]=AccZ*GRAVITY; // acc z
+  
+  Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
+  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+       
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+  Update_Matrix[2][2]=0;
+
+  Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+  for(int x=0; x<3; x++) //Matrix Addition (update)
+  {
+    for(int y=0; y<3; y++)
+    {
+      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
+    } 
+  }
+}
+
+void DCM::Euler_angles(void)
+{
+    pitch = -asin(DCM_Matrix[2][0]);
+    roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+    yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+}
+
+void DCM::Update_DCM(float dt,float a, float b, float c, float d, float e, float f){
+         
+         G_Dt = dt;
+         
+         Matrix_update(a,b,c,d,e,f); 
+
+         Normalize();
+
+         Drift_correction();
+   
+         Euler_angles();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM/DCM.h	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,56 @@
+#ifndef DCM_H
+#define DCM_H
+
+#include "mbed.h"
+
+#define GRAVITY 4096
+#define Kp_ROLLPITCH 1.515/GRAVITY //1.515
+#define Ki_ROLLPITCH 0.00101/GRAVITY //0.00101
+
+
+class DCM{
+
+public:
+   
+ DCM(void);
+   
+float G_Dt;
+
+float roll;
+
+float pitch;
+
+float yaw;
+
+void Update_DCM(float dt,float a, float b, float c, float d, float e, float f);
+
+float DCM_Matrix[3][3]; 
+
+float Update_Matrix[3][3];
+
+float Temporary_Matrix[3][3];
+
+private:
+
+float Accel_Vector[3]; //Store the acceleration in a vector
+float Gyro_Vector[3];//Store the gyros rutn rate in a vector
+float Omega_Vector[3]; //Corrected Gyro_Vector data
+float Omega_P[3];//Omega Proportional correction
+float Omega_I[3];//Omega Integrator
+float Omega[3];
+float errorRollPitch[3]; 
+
+
+float constrain(float x, float a, float b);
+
+void Normalize(void);
+
+void Drift_correction(void);
+
+void Matrix_update(float GyroX,float GyroY,float GyroZ, float AccX, float AccY, float AccZ);
+
+void Euler_angles(void);
+
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM/HelperMath.cpp	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,59 @@
+#include "HelperMath.h"
+/**************************************************/
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+  float op[3]; 
+  for(int x=0; x<3; x++)
+  {
+    for(int y=0; y<3; y++)
+    {
+      for(int w=0; w<3; w++)
+      {
+       op[w]=a[x][w]*b[w][y];
+      } 
+      mat[x][y]=0;
+      mat[x][y]=op[0]+op[1]+op[2];
+      
+    }
+  }
+}
+
+
+//Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3],float vector2[3])
+{
+  float op=0;
+  
+  for(int c=0; c<3; c++)
+  {
+  op+=vector1[c]*vector2[c];
+  }
+  
+  return op; 
+}
+
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+{
+  vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+  vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+  vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar. 
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+  for(int c=0; c<3; c++)
+  {
+   vectorOut[c]=vectorIn[c]*scale2; 
+  }
+}
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+  for(int c=0; c<3; c++)
+  {
+     vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+  }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM/HelperMath.h	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,19 @@
+#ifndef HELPERMATH_H
+#define HELPERMATH_H
+/**************************************************/
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]);
+
+//Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3],float vector2[3]);
+
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]);
+
+//Multiply the vector by a scalar. 
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2);
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]);
+
+
+#endif
\ No newline at end of file

--- a/FastPWM.lib	Sun Dec 23 23:50:21 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/Sissors/code/FastPWM/#3094d3806cfc

--- a/GlobalDefines.h	Sun Dec 23 23:50:21 2012 +0000
+++ b/GlobalDefines.h	Tue Aug 27 09:38:49 2013 +0000
@@ -1,23 +1,26 @@
-#include "mbed.h"
-#include "PulseIn.h"
-#include "FastPWM.h"
-
-#define ToRad(x) (x*0.01745329252)  // *pi/180
-#define ToDeg(x) (x*57.2957795131)  // *180/pi
+#include "Std_Types.h"
 
-//// Motor Pins /////
-FastPWM FrontMotor(p21); // Front Motor Pin
-FastPWM RearMotor(p22);  // Rear Motor Pin
-FastPWM LeftMotor(p23); // Left Motor Pin
-FastPWM RightMotor(p24);// Right Motor Pin
-/////////////////////
-
+////////////////Motors.h //////////////////
+PwmOut FrontMotor(p24); // Front Motor Pin
+PwmOut RearMotor(p21);  // Rear Motor Pin
+PwmOut LeftMotor(p23); // Left Motor Pin
+PwmOut RightMotor(p22);// Right Motor Pin
+uint16 Mspeed[4];
+uint16 Throttle= 1000;
+int16 PitchSetpoint=0,RollSetpoint=0,YawSetpoint=0;
+int16 PitchCorrection,RollCorrection,YawCorrection;
+///////////////////////////////////////////
+Serial pc(USBTX, USBRX);
 DigitalOut one(LED1);
-PulseIn Throttle(p11);
-
+DigitalOut two(LED2);
 Timer Global;
+/*
+RC Aileron(p11);
+RC Elevator(p12);
+RC Throttle(p13);
+RC Rudder(p14);
+*/
+//float pitch,roll;
+float deltaTime;
+float pitch,roll;
 
-int16_t RX_Data[4];
-float pitch,roll,deltaTime;
-/////PID Variables/////
-float PID[3],error[3],I[3],D[3];
\ No newline at end of file

--- a/MPU.h	Sun Dec 23 23:50:21 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,173 +0,0 @@
-// Contains all Sensor related functions : initializing the sensor, calibrating sensors, getting data , accelerometer angles , and filtering gyro/acc data 
-
-#define MPU_ADDRESS 0x68
-#define GyroScale  16.4
-#define AccScale   4096
-
-I2C i2c(p9,p10);
-
-float filtered_AccX[4],filtered_AccY[4],filtered_AccZ[4],filtered_GyroX[4],filtered_GyroY[4],filtered_GyroZ[4]; 
-float Scaled_GyroX,Scaled_GyroY,Scaled_GyroZ;
-float Scaled_AccX,Scaled_AccY,Scaled_AccZ;
-float GyroOffset[3],AccOffset[3];
-float accangle[2];
-
-void write(char reg,char data){
-    
-    char tx[2]={reg,data};
-    
-    i2c.write((MPU_ADDRESS << 1)&0xFE, tx, 2);
-    
- }
-
-int read (char reg){
-
-    char tx = reg;
-    char rx[2];
-    
-    i2c.write((MPU_ADDRESS << 1)&0xFE , &tx,1);
-    
-    i2c.read((MPU_ADDRESS << 1)|0x01, rx, 2);
-    
-    
-    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
- 
-    return output;
-}
-
-void MPU_Setup(){
-       
-       
-       write(0x6B,0x80); // Restart 
-       wait_ms(5);
-       write(0x6B,0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 1 (PLL with Z Gyro reference) 
-       write(0x1B,0x18); //GYRO_CONFIG   -- FS_SEL = 2 : Scale = +/-2000 deg/sec
-       write(0x1C,0x10); //ACCEL_CONFIG  -- AFS_SEL=2  : Scale = +/-8G
-       write(0x1A,0x03); //DLPF Settings -- 44Hz Accelerometer Bandwidth 42Hz Gyroscope Bandwidth 
-       wait_ms(60);
-}
-
-void ScaledGyro(){
-   
-    Scaled_GyroX = -read(0x45)/GyroScale;
-    Scaled_GyroY =  read(0x43)/GyroScale;
-    Scaled_GyroZ =  read(0x47)/GyroScale;
-}
-
-void ScaledAcc(){
-   
-   Scaled_AccX = (float)read(0x3B)/AccScale;
-   Scaled_AccY = (float)read(0x3D)/AccScale;
-   Scaled_AccZ = (float)read(0x3F)/AccScale;
-}
-
- void CalibrateGyro(){
- 
-     for(int k=0;k<200;k++){
-     ScaledGyro();
-     GyroOffset[0]+=Scaled_GyroX;
-     GyroOffset[1]+=Scaled_GyroY;
-     GyroOffset[2]+=Scaled_GyroZ;
-     wait_ms(1);
-     }
-     GyroOffset[0]/=200;
-     GyroOffset[1]/=200;
-     GyroOffset[2]/=200;
- }
- 
-  void CalibrateAcc(){
- 
-     for(int k=0;k<200;k++){
-     ScaledAcc();
-     AccOffset[0]+=Scaled_AccX;
-     AccOffset[1]+=Scaled_AccY;
-     AccOffset[2]+=Scaled_AccZ;
-     wait_ms(1);
-     }
-     AccOffset[0]/=200;
-     AccOffset[1]/=200;
-     AccOffset[2]/=200;
-     AccOffset[2]-=1;
- }
- 
- void GyroRate(){
-    
-    ScaledGyro();
-    Scaled_GyroX-=GyroOffset[0];
-    Scaled_GyroY-=GyroOffset[1];
-    Scaled_GyroZ-=GyroOffset[2];
- }
- 
- void Acc(){
- 
-    ScaledAcc();
-    Scaled_AccX-=AccOffset[0];
-    Scaled_AccY-=AccOffset[1];
-    Scaled_AccZ-=AccOffset[2];
- }
-   void filterGyro(void){
-      
-       GyroRate();
-    
-    //////////////////////////////////////// Filter X Axis ////////////////////////////////////////////////////////////////
-    filtered_GyroX[0] = Scaled_GyroX;
-    Scaled_GyroX      = filtered_GyroX[0]*0.25 + filtered_GyroX[1]*0.25 + filtered_GyroX[2]*0.25 + filtered_GyroX[3]*0.25;
-    filtered_GyroX[3] = filtered_GyroX[2];
-    filtered_GyroX[2] = filtered_GyroX[1];
-    filtered_GyroX[1] = filtered_GyroX[0];
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    
-    /////////////////////////////////////// Filter Y Axis ////////////////////////////////////////////////////////////////
-    filtered_GyroY[0] = Scaled_GyroY;
-    Scaled_GyroY      = filtered_GyroY[0]*0.25 + filtered_GyroY[1]*0.25 + filtered_GyroY[2]*0.25 + filtered_GyroY[3]*0.25;
-    filtered_GyroY[3] = filtered_GyroY[2];
-    filtered_GyroY[2] = filtered_GyroY[1];
-    filtered_GyroY[1] = filtered_GyroY[0];
-    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    
-    /////////////////////////////////////// Filter Z Axis ////////////////////////////////////////////////////////////////
-    filtered_GyroZ[0] = Scaled_GyroZ;
-    Scaled_GyroZ      = filtered_GyroZ[0]*0.25 + filtered_GyroZ[1]*0.25 + filtered_GyroZ[2]*0.25 + filtered_GyroZ[3]*0.25;
-    filtered_GyroZ[3] = filtered_GyroZ[2];
-    filtered_GyroZ[2] = filtered_GyroZ[1];
-    filtered_GyroZ[1] = filtered_GyroZ[0];
-    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }
-   
-   
- void filterAcc(void){
-      
-       Acc();
-    
-    //////////////////////////////////////// Filter X Axis///////////////////////////////////////////////////////////
-    filtered_AccX[0] = Scaled_AccX;
-    Scaled_AccX      = filtered_AccX[0]*0.25 + filtered_AccX[1]*0.25 + filtered_AccX[2]*0.25 + filtered_AccX[3]*0.25;
-    filtered_AccX[3] = filtered_AccX[2];
-    filtered_AccX[2] = filtered_AccX[1];
-    filtered_AccX[1] = filtered_AccX[0];
-    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    
-    //////////////////////////////////////// Filter Y Axis //////////////////////////////////////////////////////////
-    filtered_AccY[0] = Scaled_AccY;
-    Scaled_AccY      = filtered_AccY[0]*0.25 + filtered_AccY[1]*0.25 + filtered_AccY[2]*0.25 + filtered_AccY[3]*0.25;
-    filtered_AccY[3] = filtered_AccY[2];
-    filtered_AccY[2] = filtered_AccY[1];
-    filtered_AccY[1] = filtered_AccY[0];
-    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    
-    //////////////////////////////////////// Filter Z Axis//////////////////////////////////////////////////////////
-    filtered_AccZ[0] = Scaled_AccZ;
-    Scaled_AccZ      = filtered_AccZ[0]*0.25 + filtered_AccZ[1]*0.25 + filtered_AccZ[2]*0.25 + filtered_AccZ[3]*0.25;
-    filtered_AccZ[3] = filtered_AccZ[2];
-    filtered_AccZ[2] = filtered_AccZ[1];
-    filtered_AccZ[1] = filtered_AccZ[0];
-    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }
- 
-  void AccAngle(){
-  
-   filterAcc();
-   
-   accangle[0] = ToDeg(atan2(Scaled_AccX,sqrt(Scaled_AccY*Scaled_AccY+Scaled_AccZ*Scaled_AccZ)));
-   accangle[1] = ToDeg(atan2(Scaled_AccY,sqrt(Scaled_AccX*Scaled_AccX+Scaled_AccZ*Scaled_AccZ)));
- }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU/MPU6050.cpp	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,167 @@
+#include "MPU6050.h"
+
+MPU6050::MPU6050(){
+   
+   Panic=false;
+}
+
+void MPU6050::write(char reg,char data){
+    
+    char tx[2]={reg,data};
+
+    I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, tx, 2);
+
+ }
+
+int MPU6050::read (char reg){
+
+    char tx = reg;
+    char rx[2];
+
+    I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, &tx, 1);
+
+    I2C0.blockread((MPU_ADDRESS << 1)|0x01, rx, 2);
+
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+    return output;
+}
+
+bool MPU6050::CheckConnection(void){
+     
+    char tx = 0x75; // Who Am I Register 
+    char rx;
+
+    I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, &tx, 1);
+
+    I2C0.blockread((MPU_ADDRESS << 1)|0x01, &rx, 1);
+    
+    if(rx==0x68) return true;
+        
+        else return false ;
+}
+
+void MPU6050::MPU_Setup(void){
+
+       write(0x6B,0x80); // Restart 
+       wait_ms(5);
+       write(0x6B,0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) 
+       write(0x1B,0x18); //GYRO_CONFIG   -- FS_SEL =3 : Scale = +/-2000 deg/sec
+       write(0x1C,0x10); //ACCEL_CONFIG  -- AFS_SEL= 2 : Scale = +/-8G
+       write(0x1A,0x03); //DLPF Settings -- 44Hz Accelerometer Bandwidth 42Hz Gyroscope Bandwidth 
+       wait_ms(60);
+}
+
+void MPU6050::ScaledGyro(void){
+   
+    Scaled_GyroX = -(float)read(0x45)/GyroScale;
+    Scaled_GyroY =  (float)read(0x43)/GyroScale;
+    Scaled_GyroZ =  (float)read(0x47)/GyroScale;
+}
+
+void MPU6050::ScaledAcc(void){
+   
+   Scaled_AccX = (float)read(0x3B)/AccScale;
+   Scaled_AccY = (float)read(0x3D)/AccScale;
+   Scaled_AccZ = (float)read(0x3F)/AccScale;
+}
+
+void MPU6050::RawAcc(void){
+   
+   Raw_AccX = read(0x3B);
+   Raw_AccY = read(0x3D);
+   Raw_AccZ = read(0x3F);
+}
+
+ void MPU6050::CalibrateGyro(void){
+ 
+     for(int k=0;k<100;k++){
+     ScaledGyro();
+     GyroOffset[0]+=Scaled_GyroX;
+     GyroOffset[1]+=Scaled_GyroY;
+     GyroOffset[2]+=Scaled_GyroZ;
+     wait_ms(5);
+     }
+     GyroOffset[0]/=100;
+     GyroOffset[1]/=100;
+     GyroOffset[2]/=100;
+
+ }
+ 
+  void MPU6050::CalibrateAcc(void){
+ 
+     for(int k=0;k<100;k++){
+     ScaledAcc();
+     AccOffset[0]+=Scaled_AccX;
+     AccOffset[1]+=Scaled_AccY;
+     AccOffset[2]+=Scaled_AccZ;
+     wait_ms(5);
+     }
+     AccOffset[0]/=100;
+     AccOffset[1]/=100;
+     AccOffset[2]/=100;
+     AccOffset[2]-=1;
+     
+ }
+ 
+ void MPU6050::GyroRate(void){
+    
+    ScaledGyro();
+    Scaled_GyroX-=GyroOffset[0];
+    Scaled_GyroY-=GyroOffset[1];
+    Scaled_GyroZ-=GyroOffset[2];
+ }
+ 
+ void MPU6050::Acc(void){
+ 
+    ScaledAcc();
+    Scaled_AccX-=AccOffset[0];
+    Scaled_AccY-=AccOffset[1];
+    Scaled_AccZ-=AccOffset[2];
+     
+ }
+ 
+   void MPU6050::filterGyro(void){
+      
+       GyroRate();
+    
+    // Filter X Axis //
+    filtered_Gyro[0] = filter_GyroX.update(Scaled_GyroX);
+    
+    //Filter Y Axis //
+    filtered_Gyro[1] = filter_GyroY.update(Scaled_GyroY);
+    
+    // Filter Z Axis //
+    filtered_Gyro[2] = filter_GyroZ.update(Scaled_GyroZ);
+ }
+   
+   
+ void MPU6050::filterAcc(void){
+      
+       Acc();
+    
+    // Filter X Axis//
+    filtered_Acc[0] = filter_AccX.update(Scaled_AccX);
+    
+    // Filter Y Axis //
+    filtered_Acc[1] = filter_AccY.update(Scaled_AccY);
+    
+    // Filter Z Axis//
+    filtered_Acc[2] = filter_AccZ.update(Scaled_AccZ);
+    
+    Acceleration_Magnitude = sqrt(filtered_Acc[0]*filtered_Acc[0] + filtered_Acc[1]*filtered_Acc[1] + filtered_Acc[2]*filtered_Acc[2]);
+    
+    if(Acceleration_Magnitude==0) Panic = true; // Free-Fall detection;
+    else Panic= false;
+ }
+ 
+  void MPU6050::AccAngle(void){
+  
+   //filterAcc();
+   Acc();
+   
+   //accangle[0] = ToDeg(atan2(filtered_Acc[0],sqrt(filtered_Acc[1]*filtered_Acc[1]+filtered_Acc[2]*filtered_Acc[2])));
+   accangle[0] = ToDeg(atan2(Scaled_AccX,sqrt(Scaled_AccY*Scaled_AccY+Scaled_AccZ*Scaled_AccZ)));
+   //accangle[1] = ToDeg(atan2(filtered_Acc[1],sqrt(filtered_Acc[0]*filtered_Acc[0]+filtered_Acc[2]*filtered_Acc[2])));
+   accangle[1] = ToDeg(atan2(Scaled_AccY,sqrt(Scaled_AccX*Scaled_AccX+Scaled_AccZ*Scaled_AccZ)));
+ }
+ 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU/MPU6050.h	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,68 @@
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include "mbed.h"
+#include "MyI2C.h"
+#include "MAF.h"
+
+#define MPU_ADDRESS 0x68
+#define GyroScale  16.4f
+#define AccScale   4096
+#define ToRad(x) (x*0.01745329252)  // *pi/180
+#define ToDeg(x) (x*57.2957795131)  // *180/pi
+
+extern MyI2C I2C0;
+
+class MPU6050
+{
+
+public:
+
+ MPU6050(void);
+float filtered_Gyro[3],filtered_Acc[3];
+float Scaled_GyroX,Scaled_GyroY,Scaled_GyroZ;
+float Scaled_AccX,Scaled_AccY,Scaled_AccZ;
+int Raw_AccX,Raw_AccY,Raw_AccZ;
+float GyroOffset[3],AccOffset[3];
+float accangle[2];
+float Acceleration_Magnitude;
+bool Panic;
+
+
+bool CheckConnection(void);
+
+void MPU_Setup(void);
+
+void ScaledGyro(void);
+
+void ScaledAcc(void);
+
+void RawAcc(void);
+
+void CalibrateGyro(void);
+
+void CalibrateAcc(void);
+
+void GyroRate(void);
+
+void Acc(void);
+
+void filterGyro(void);
+
+void filterAcc(void);
+
+void AccAngle(void);
+
+private:
+
+MAF filter_AccX,filter_AccY,filter_AccZ; // Moving Average Filter Initiallization 
+
+MAF filter_GyroX,filter_GyroY,filter_GyroZ;
+
+void write(char reg,char data);
+
+int read (char reg);
+
+};
+
+#endif
\ No newline at end of file

--- a/Motors.h	Sun Dec 23 23:50:21 2012 +0000
+++ b/Motors.h	Tue Aug 27 09:38:49 2013 +0000
@@ -1,57 +1,49 @@
-#define Kp 1
-#define Ki 1
-#define Kd 1
-
-#define PITCH 0
-#define ROLL 1
-#define YAW 2
-
-   int constrain(int a , int b, int c){
-   
+#define RefreshRate 2500 // in us :400Hz
+    
+   static uint16 Constrain(uint16 a , uint16 b, uint16 c){
    if(a<b) return b;
    else if(a>c)return c;
    else return a;
    }
-
-   void InitializeMotors(void){
-   
-    FrontMotor.period_ms(20);
-    RearMotor.period_ms(20);
-    LeftMotor.period_ms(20);
-    RightMotor.period_ms(20);
-   
+ 
+  void CalibrateESC(void){
+  
+    FrontMotor.period_us(RefreshRate); // setting one will by default set all other pins to the same period 
+    
+    /*FrontMotor.pulsewidth_us(2000);
+    RearMotor.pulsewidth_us(2000);
+    LeftMotor.pulsewidth_us(2000);
+    RightMotor.pulsewidth_us(2000);
+    
+    wait(2);  still in testing phase */ 
+    
     FrontMotor.pulsewidth_us(1000);
     RearMotor.pulsewidth_us(1000);
     LeftMotor.pulsewidth_us(1000);
     RightMotor.pulsewidth_us(1000);
     
-    wait(3); // let the hardware settle
- }
- 
-  void updatePID(void){ 
+    wait(2);
+  }
+   
+  void updateMotors(int16 PitchCommand,int16 RollCommand){
   
-    error[PITCH]= RX_Data[PITCH] - pitch;
-    error[ROLL]= RX_Data[ROLL] - roll ;
-    error[YAW]= RX_Data[YAW] - Scaled_GyroZ;
-    
-    I[PITCH]+= error[PITCH]*Ki;
-    I[ROLL]+= error[ROLL]*Ki;
-    I[YAW]+= error[YAW]*Ki;
+    if(Throttle>=1200){
+
+    Mspeed[0] = Constrain((Throttle+PitchCommand),1200,2000);
+    Mspeed[1] = Constrain((Throttle-PitchCommand),1200,2000);
+    Mspeed[2] = Constrain((Throttle+RollCommand),1200,2000);
+    Mspeed[3] = Constrain((Throttle-RollCommand),1200,2000);
+    }
     
-    D[PITCH] = Scaled_GyroX;
-    D[ROLL] = Scaled_GyroY;
-    D[YAW] = Scaled_GyroZ;
+    else{
+    Mspeed[0] = 1000;
+    Mspeed[1] = 1000;
+    Mspeed[2] = 1000;
+    Mspeed[3] = 1000;
+    }
     
-    PID[PITCH] = error[PITCH]*Kp + I[PITCH] + D[PITCH]*Kd ; // Pitch correction
-    PID[ROLL]  = error[ROLL]*Kp + I[ROLL] + D[ROLL]*Kd ;    // Roll correction
-    PID[YAW]   = error[YAW] ;                               // Yaw correction 
-  
-   }
-   
-  void updateMotors(void){
-  
-    FrontMotor.pulsewidth_us(constrain(RX_Data[3]+PID[PITCH],1000,2000));
-    RearMotor.pulsewidth_us(constrain(RX_Data[3]-PID[PITCH],1000,2000));
-    LeftMotor.pulsewidth_us(constrain(RX_Data[3]+PID[ROLL],1000,2000));
-    RightMotor.pulsewidth_us(constrain(RX_Data[3]-PID[ROLL],1000,2000));
+    FrontMotor.pulsewidth_us(Mspeed[0]);
+    RearMotor.pulsewidth_us(Mspeed[1]);
+    LeftMotor.pulsewidth_us(Mspeed[2]);
+    RightMotor.pulsewidth_us(Mspeed[3]);
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MovingAverageFilter.lib	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/KarimAzzouz/code/MovingAverageFilter/#fbc57eb4e61d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MyI2C.lib	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/KarimAzzouz/code/MyI2C/#1c722325e933

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/KarimAzzouz/code/PID/#2ac3074cfdb3

--- a/PulseIn.lib	Sun Dec 23 23:50:21 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-PulseIn#eaf70ff4df07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RC.lib	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/KarimAzzouz/code/RC/#c98b23d53e42

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Std_Types.h	Tue Aug 27 09:38:49 2013 +0000
@@ -0,0 +1,16 @@
+#ifndef STD_TYPES_H
+#define STD_DEFINES_H
+
+#include "mbed.h"
+
+#define TRUE (uint8)1
+#define FALSE(uint8)0
+
+typedef uint8_t boolean;
+typedef uint8_t uint8;
+typedef uint16_t uint16;
+typedef uint32_t uint32;
+typedef int8_t int8;
+typedef int16_t int16;
+typedef int32_t int32;
+#endif
\ No newline at end of file

--- a/main.cpp	Sun Dec 23 23:50:21 2012 +0000
+++ b/main.cpp	Tue Aug 27 09:38:49 2013 +0000
@@ -1,71 +1,147 @@
+/*
+                         F                  +X
+                         |                   | 
+                         |                   |
+                 L ------|------R    -Y------|------+Y
+                         |                   |
+                         |                   |
+                         B                  -X
+*/
+#include "mbed.h"
 #include "rtos.h"
 #include "GlobalDefines.h"
-#include "MPU.h"
+#include "MPU6050.h"
+#include "BMP085.h"
+//#include "DCM.h" Drifts a lot, needs tuning 
+#include "PID.h"
 #include "Motors.h"
 
-#define FlightControlUpdateRate 5
-#define RxUpdateRate 20
-#define Enable_Serial
+#define Debugging
+
+typedef enum{Armed=0,
+UnArmed
+}State;
+
+State MotorState = Armed;
+bool print = false;
+bool P_readingReady=false;
+bool T_readingReady=false;
+char c ;
 
-#ifdef Enable_Serial
-Serial pc(USBTX, USBRX);
-#endif
+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){
 
-void RX_thread(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();
      
-     while(true){
-     RX_Data[3] = Throttle.read();
-     #ifdef Enable_Serial
-     //pc.printf("%d\n",RX_Data[3]);
-     #endif
-     Thread::wait(RxUpdateRate);
-     }
+         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(){
-
-    //i2c.frequency(400000);
+    
+    #ifdef Debugging
+    pc.baud(115200);
+    #endif
     
-    #ifdef Enable_Serial
-        pc.baud(57600);
-    #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();    
      
-     one=1;
-     MPU_Setup();
-     CalibrateGyro();
-     CalibrateAcc();
-     InitializeMotors();
-     one=0;
- 
- Thread thread(RX_thread);
- 
- Global.start();
+     RtosTimer Fly(FlightControl,osTimerPeriodic,NULL); // Initialize RTOS Timer  
+     Global.start(); // Start timer
+     Fly.start(5);  // RTOS Timer (200Hz Control Loop)
  
  while(true){
-    
-    // Get Sensor Data
-    filterGyro();
-    AccAngle();
-    
-    // Gotta Keep Time
-    deltaTime = Global.read();
-    Global.reset();
-    
-    //Pitch and Roll Calculation using complimentary filter 
-    //pitch= (0.98)*(pitch+Scaled_GyroX*deltaTime) + (0.02)*(accangle[0]);
-    pitch+= Scaled_GyroX*deltaTime;
-    
-    // update PID and Motor Commands
-    //updatePID();
-    updateMotors();
-    
-    #ifdef Enable_Serial
-    pc.printf("%f,%f\n",pitch,accangle[0]);
-    #endif
-    
-    Thread::wait(FlightControlUpdateRate);
+           
+          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);         
 }
 }

--- a/mbed-rtos.lib	Sun Dec 23 23:50:21 2012 +0000
+++ b/mbed-rtos.lib	Tue Aug 27 09:38:49 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed-rtos/#88a1a9c26ae3
+http://mbed.org/users/mbed_official/code/mbed-rtos/#869ef732a8a2

--- a/mbed.bld	Sun Dec 23 23:50:21 2012 +0000
+++ b/mbed.bld	Tue Aug 27 09:38:49 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/63cdd78b2dc1
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/e3affc9e7238
\ No newline at end of file