Diff: DCM.cpp

Revision:

6:49cbf2acc4e6

Parent:

5:d5ecdb82c17b

Child:

7:d22702f7ce89

--- a/DCM.cpp	Fri Jul 26 04:02:53 2013 +0000
+++ b/DCM.cpp	Fri Jul 26 18:44:33 2013 +0000
@@ -6,16 +6,21 @@
 
 #define MAG_SKIP 1
 
-float DCM::constrain(float x, float a, float b) 
+
+#include "mbed.h"
+Serial p(USBTX, USBRX); // tx, rx
+
+void DCM::pv(float a[3])
 {
-    float result = x;
-    
-    if (x < a) result = a;
-    if (x > b) result = b;
-
-    return result;  
+    p.printf("%g,\t%g,\t%g \n\r",a[0],a[1],a[2]);
 }
 
+void DCM::pm(float a[3][3])
+{
+    p.printf("\t%g,\t%g,\t%g \n\r",a[0][0],a[0][1],a[0][2]);
+    p.printf("\t%g,\t%g,\t%g \n\r",a[1][0],a[1][1],a[1][2]);
+    p.printf("\t%g,\t%g,\t%g \n\r",a[2][0],a[2][1],a[2][2]);
+}
 
 DCM::DCM(void):
     G_Dt(0.02), update_count(MAG_SKIP)
@@ -35,37 +40,36 @@
         for (int n=0; n < 3; n++) {
             dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
         }
-    
+    reset_sensor_fusion();
     }
-    
 }
 
 
 /**************************************************/
 void DCM::Normalize(void)
 {
-  float error=0;
-  float temporary[3][3];
-  float renorm=0;
-  
+    float error=0;
+    float temporary[3][3];
+    float renorm=0;
+    
     error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19
-
-  Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
-  Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
+    
+    Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
+    Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
     
-  Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
-  Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
-  
-  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
-  
-  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
-  Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
-  
-  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
-  Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
-  
-  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
-  Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
+    Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
+    Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
+    
+    Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+    
+    renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
+    Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
+    
+    renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
+    Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
+    
+    renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
+    Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
 }
 
 /**************************************************/
@@ -115,10 +119,10 @@
 /**************************************************/
 void DCM::Matrix_update(void)
 {
-
+//    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
-
+//    p.printf("Omega_Vector: ");pv(Omega_Vector);
     #if OUTPUTMODE==1         
     Update_Matrix[0][0]=0;
     Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
@@ -129,6 +133,7 @@
     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;
+//    p.printf("Update: \n\r");pm(Update_Matrix);
     #else                    // Uncorrected data (no drift correction)
     Update_Matrix[0][0]=0;
     Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
@@ -141,6 +146,8 @@
     Update_Matrix[2][2]=0;
     #endif
     
+ //   p.printf("Temp: \n\r");pm(Temporary_Matrix);
+//    p.printf("Update: \n\r");pm(Update_Matrix);
     Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b
     // ???  Matrix_Add(dcm, dcm, Temporary_Matrix); ???
     for(int x=0; x<3; x++) { //Matrix Addition (update)
@@ -200,3 +207,44 @@
     MAG_Heading = atan2(-mag_y, mag_x);
 }
 
+void DCM::reset_sensor_fusion()
+{
+    float temp1[3];
+    float temp2[3];
+    float xAxis[] = {1.0f, 0.0f, 0.0f};
+    
+    Read_Accel(Accel_Vector);         
+    Read_Gyro(Gyro_Vector);
+    
+    //  timestamp = timer.read_ms();
+    
+    // GET PITCH
+    // Using y-z-plane-component/x-component of gravity vector
+    pitch = -atan2(Accel_Vector[0], sqrt(Accel_Vector[1] * Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]));
+    
+    // GET ROLL
+    // Compensate pitch of gravity vector 
+    Vector_Cross_Product(temp1, Accel_Vector, xAxis);
+    Vector_Cross_Product(temp2, xAxis, temp1);
+    // Normally using x-z-plane-component/y-component of compensated gravity vector
+    // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
+    // Since we compensated for pitch, x-z-plane-component equals z-component:
+    roll = atan2(temp2[1], temp2[2]);
+    
+    // GET YAW
+    Update_mag();
+    yaw = MAG_Heading;
+    
+    // Init rotation matrix
+    init_rotation_matrix(dcm, yaw, pitch, roll);
+}
+
+float DCM::constrain(float x, float a, float b) 
+{
+    float result = x;
+    
+    if (x < a) result = a;
+    if (x > b) result = b;
+
+    return result;  
+}