diff --git a/ArduCopter.pde b/ArduCopter.pde
index e5298886be..a651418535 100644
--- a/ArduCopter.pde
+++ b/ArduCopter.pde
@@ -401,7 +401,7 @@ void setup()
   APM_RC_QUAD.OutputCh(2,MIN_THROTTLE+15);
   APM_RC_QUAD.OutputCh(3,MIN_THROTTLE+15);
   
-  #if (MAGNETOMTER==1)
+  #if (MAGNETOMETER)
     APM_Compass.Init();  // I2C initialization
   #endif
  
@@ -508,11 +508,12 @@ void loop(){
     
     // IMU DCM Algorithm
     Read_adc_raw();
-    #if (MAGNETOMETER==1)
+    #if (MAGNETOMETER)
     if (counter > 8)  // Read compass data at 10Hz... (7 loop runs)
       {
       counter=0;
       APM_Compass.Read();     // Read magnetometer
+      APM_Compass.Calculate(roll,pitch);  // Calculate heading
       }
     #endif
     Matrix_update(); 
diff --git a/DCM.pde b/DCM.pde
index 993f814ff4..edd52a181e 100644
--- a/DCM.pde
+++ b/DCM.pde
@@ -78,8 +78,8 @@ void Drift_correction(void)
   
   //*****YAW***************
   // We make the gyro YAW drift correction based on compass magnetic heading 
-  #if (MAGNETOMETER==1)
-    errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_X) - (DCM_Matrix[1][0]*APM_Compass.Heading_Y);  //Calculating YAW error
+  #if (MAGNETOMETER)
+    errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X);  //Calculating YAW error
     Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
   
     Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
@@ -113,8 +113,6 @@ void Matrix_update(void)
   Accel_Vector[1]=Accel_Vector[1]*0.4 + (float)read_adc(4)*0.6; // acc y
   Accel_Vector[2]=Accel_Vector[2]*0.4 + (float)read_adc(5)*0.6; // acc z
   
-  Accel_Vector[1]=0;
-  
   Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
   Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional