Diff: DCM.cpp

Revision:

8:195d53710158

Parent:

7:d22702f7ce89

Child:

11:8d46121bd255

--- a/DCM.cpp	Mon Jul 29 19:58:54 2013 +0000
+++ b/DCM.cpp	Wed Jul 31 18:58:32 2013 +0000
@@ -48,6 +48,7 @@
 /**************************************************/
 void DCM::Normalize(void)
 {
+//    p.printf("NORMLALIZE:----\n\r");
     float error=0;
     float temporary[3][3];
     float renorm=0;
@@ -70,11 +71,14 @@
     
     renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
     Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
+//    p.printf("dcm: \n\r");pm(dcm);
+    
 }
 
 /**************************************************/
 void DCM::Drift_correction(void)
 {
+//    p.printf("DRIFT CORRECTION:----\n\r");
   float mag_heading_x;
   float mag_heading_y;
   float errorCourse;
@@ -93,9 +97,12 @@
   // 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);  //  
-    
+  
+//  p.printf("Accel_W: %g", Accel_weight);
   Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
+//  p.printf("errorRollPitch: ");pv(errorRollPitch);
   Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+//  p.printf("Omega_P: ");pv(Omega_P);
   
   Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);  
@@ -113,12 +120,14 @@
     Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
     Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
 
+//    p.printf("dcm: \n\r");pm(dcm);
 }
 
 
 /**************************************************/
 void DCM::Matrix_update(void)
 {
+//    p.printf("MATRIX UPDATE:----\n\r");
 //    p.printf("Omega: ");pv(Omega);
     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
@@ -155,6 +164,7 @@
             dcm[x][y] += Temporary_Matrix[x][y];
         } 
     }
+//    p.printf("dcm: \n\r");pm(dcm);
 }
 
 void DCM::Euler_angles(void)