diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.c b/sw/airborne/subsystems/ahrs/dcm/dcm.c
index 61dabf2025..26a038f46e 100644
--- a/sw/airborne/subsystems/ahrs/dcm/dcm.c
+++ b/sw/airborne/subsystems/ahrs/dcm/dcm.c
@@ -32,13 +32,11 @@ float speed_3d = 0;
 
 // Algebra
 
-//Computes the dot product of two vectors
 static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
 {
   return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2];
 }
 
-//Computes the cross product of two vectors
 static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
 {
   vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
@@ -46,7 +44,6 @@ static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2
   vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
 }
 
-//Multiply the vector by a scalar.
 static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
 {
   vectorOut[0]=vectorIn[0]*scale2;
@@ -101,117 +98,65 @@ void Normalize(void)
   float temporary[3][3];
   float renorm=0;
   boolean 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_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]);
+  
+  renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); 
   if (renorm < 1.5625f && renorm > 0.64f) {
-    renorm= .5 * (3-renorm);                                                 //eq.21
+    renorm= .5 * (3-renorm);                                          //eq.21
   } else if (renorm < 100.0f && renorm > 0.01f) {
     renorm= 1. / sqrt(renorm);
-#if PERFORMANCE_REPORTING == 1
+#if PERFORMANCE_REPORTING == 1  
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:1,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.print (",TOW:");
-    Serial.print (iTOW);
-    Serial.println("***");
 #endif
   } else {
     problem = TRUE;
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-	#if PRINT_DEBUG != 0
-    Serial.print("???PRB:1,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.print (",TOW:");
-    Serial.print (iTOW);
-    Serial.println("***");
 #endif
   }
       Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
-
-  renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
+  
+  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);
-#if PERFORMANCE_REPORTING == 1
+    renorm= 1. / sqrt(renorm);  
+#if PERFORMANCE_REPORTING == 1    
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:2,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.print (",TOW:");
-    Serial.print (iTOW);
-    Serial.println("***");
 #endif
   } else {
     problem = TRUE;
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???PRB:2,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.print (",TOW:");
-    Serial.print (iTOW);
-    Serial.println("***");
 #endif
   }
   Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
-
-  renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
+  
+  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);
-#if PERFORMANCE_REPORTING == 1
+    renorm= 1. / sqrt(renorm);   
+#if PERFORMANCE_REPORTING == 1 
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:3,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.print (",TOW:");
-    Serial.print (iTOW);
-    Serial.println("***");
 #endif
   } else {
-    problem = TRUE;
+    problem = TRUE;  
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???PRB:3,RNM:");
-    Serial.print (renorm);
-    Serial.print (",TOW:");
-    Serial.print (iTOW);
-    Serial.println("***");
 #endif
   }
   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;
@@ -222,7 +167,7 @@ void Normalize(void)
       DCM_Matrix[2][0]= 0.0f;
       DCM_Matrix[2][1]= 0.0f;
       DCM_Matrix[2][2]= 1.0f;
-      problem = FALSE;
+      problem = FALSE;  
   }
 }
 
@@ -239,7 +184,7 @@ float MAG_Heading;
 
 void Drift_correction(void)
 {
-  //Compensation the Roll, Pitch and Yaw drift.
+  //Compensation the Roll, Pitch and Yaw drift. 
   static float Scaled_Omega_P[3];
   static float Scaled_Omega_I[3];
   float Accel_magnitude;
@@ -267,34 +212,37 @@ void Drift_correction(void)
   Accel_magnitude = 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 = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1);  //
-
+  Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1);  //  
+  
   #if PERFORMANCE_REPORTING == 1
-    tempfloat = ((Accel_weight - 0.5) * 256.0f);    //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
+  {
+  
+    float tempfloat = ((Accel_weight - 0.5) * 256.0f);    //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
     imu_health += tempfloat;
     Bound(imu_health,129,65405);
+  }
   #endif
-
+  
   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);
-
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
+  
   //*****YAW***************
-
-  #if USE_MAGNETOMETER==1
+  
+  #if USE_MAGNETOMETER==1 
     // We make the gyro YAW drift correction based on compass magnetic heading
     mag_heading_x = cos(MAG_Heading);
     mag_heading_y = sin(MAG_Heading);
     errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_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);
     Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
-
+    
     Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
-    Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+    Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I   
   #else  // Use GPS Ground course to correct yaw gyro drift
     if(gps_mode==3 && ground_speed>= 0.5)  //hwarm
   {
@@ -303,29 +251,21 @@ void Drift_correction(void)
     COGY = sin(RadOfDeg(ground_course));
     errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX);  //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);
     Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
-
+  
     Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
-    Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+    Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I   
   }
   #endif
   //  Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
   Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
   if (Integrator_magnitude > DegOfRad(300)) {
     Vector_Scale(Omega_I,Omega_I,0.5f*DegOfRad(300)/Integrator_magnitude);
-#if PRINT_DEBUG != 0
-    Serial.print("!!!INT:1,MAG:");
-    Serial.print (ToDeg(Integrator_magnitude));
-
-    Serial.print (",TOW:");
-    Serial.print (iTOW);
-    Serial.println("***");
-#endif
   }
-
-
+  
+  
 }
 /**************************************************/
 
@@ -337,11 +277,11 @@ void Matrix_update(void)
   if (gps_mode==3)    //Remove centrifugal acceleration.
   {
     Accel_Vector[1] += speed_3d*Omega[2];  // Centrifugal force on Acc_y = GPS_speed*GyroZ
-    Accel_Vector[2] -= speed_3d*Omega[1];  // Centrifugal force on Acc_z = GPS_speed*GyroY
+    Accel_Vector[2] -= speed_3d*Omega[1];  // Centrifugal force on Acc_z = GPS_speed*GyroY 
   }
 
-
- #if OUTPUTMODE==1    // With corrected data (drift correction)
+  
+ #if OUTPUTMODE==1    // With corrected data (drift correction)     
   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
@@ -370,7 +310,7 @@ void Matrix_update(void)
     for(int y=0; y<3; y++)
     {
       DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
-    }
+    } 
   }
 }
 
@@ -381,9 +321,6 @@ void Euler_angles(void)
     euler.theta = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x)
     euler.psi = 0;
   #else
-    //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]);
     euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
     euler.theta = -asin(DCM_Matrix[2][0]);
     euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.h b/sw/airborne/subsystems/ahrs/dcm/dcm.h
index d5a92b781b..0cb1dda865 100644
--- a/sw/airborne/subsystems/ahrs/dcm/dcm.h
+++ b/sw/airborne/subsystems/ahrs/dcm/dcm.h
@@ -34,5 +34,15 @@ extern struct FloatEulers euler;
 // Mode 1 = DCM integration with Kp and Ki
 // Mode 2 = direct accelerometer -> euler
 
+#define MAGNETOMETER 1
+extern float MAG_Heading;
+
+#define PERFORMANCE_REPORTING 1
+#if PERFORMANCE_REPORTING == 1
+extern int renorm_sqrt_count;
+extern int renorm_blowup_count;
+extern float imu_health;
+#endif
+