Diff: DCM.cpp

Revision:

13:6134f21cad37

Parent:

12:e85008094132

Child:

14:6cb3c2204b9b

--- a/DCM.cpp	Mon Oct 14 02:44:11 2013 +0000
+++ b/DCM.cpp	Tue Oct 15 08:24:34 2013 +0000
@@ -1,11 +1,10 @@
 #include "DCM.h"
 #include "Matrix.h"
 #include "math.h"
-#include "Sensors.h"
 
 
 #define MAG_SKIP 1
-
+#define GRAVITY 255
 
 #include "mbed.h"
 Serial p(USBTX, USBRX); // tx, rx
@@ -25,12 +24,10 @@
 DCM::DCM(void):
     G_Dt(0.02), update_count(MAG_SKIP)
 {
-    Accel_Init();
-    Gyro_Init();
-    Magn_Init();
     for (int m=0; m < 3; m++) {
         Accel_Vector[m] = 0;
         Gyro_Vector[m] = 0;
+        Magn_Vector[m] = 0;
         Omega_Vector[m] = 0;
         Omega_P[m] = 0;
         Omega_I[m] = 0;
@@ -44,6 +41,27 @@
     }
 }
 
+void DCM::Update_step(float dt, float AccelV[], float GyroV[], float MagnV[])
+{
+    Accel_Vector[0] = AccelV[0];
+    Accel_Vector[1] = AccelV[0];
+    Accel_Vector[2] = AccelV[0];
+    Gyro_Vector[0] = GyroV[0];
+    Gyro_Vector[1] = GyroV[1];
+    Gyro_Vector[2] = GyroV[2];
+    Magn_Vector[0] = MagnV[0];
+    Magn_Vector[1] = MagnV[1];
+    Magn_Vector[2] = MagnV[2];
+    G_Dt = dt;   
+    if (--update_count == 0) {
+        Update_mag();
+        update_count = MAG_SKIP;
+    }
+    Matrix_update();
+    Normalize();
+    Drift_correction();
+    Euler_angles();
+}
 
 /**************************************************/
 void DCM::Normalize(void)
@@ -76,7 +94,7 @@
 }
 
 /**************************************************/
-void DCM::Drift_correction(void)
+void DCM::Drift_correction()
 {
 //    p.printf("DRIFT CORRECTION:----\n\r");
   float mag_heading_x;
@@ -125,7 +143,7 @@
 
 
 /**************************************************/
-void DCM::Matrix_update(void)
+void DCM::Matrix_update()
 {
 //    p.printf("MATRIX UPDATE:----\n\r");
 //    p.printf("Omega: ");pv(Omega);
@@ -177,27 +195,9 @@
     yaw = RAD2DEG(atan2(dcm[1][0],dcm[0][0]));
 }
 
-void DCM::Update_step(float dt)
-{
-    G_Dt = dt;
-    Read_Accel(Accel_Vector);  
-    p.printf("X: %d   \t",Accel_Vector[0]);
-    p.printf("Y: %d   \t",Accel_Vector[1]);
-    p.printf("Z: %d   \n\r\n",Accel_Vector[2]);       
-    Read_Gyro(Gyro_Vector);
-    if (--update_count == 0) {
-        Update_mag();
-        update_count = MAG_SKIP;
-    }
-    Matrix_update();
-    Normalize();
-    Drift_correction();
-    Euler_angles();
-}
 
 void DCM::Update_mag()
 {
-    Read_Magn(Magn_Vector);
     Compass_Heading();
 }
 
@@ -228,10 +228,7 @@
     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