Diff: DCM.cpp

Revision:

14:6cb3c2204b9b

Parent:

13:6134f21cad37

diff -r 6134f21cad37 -r 6cb3c2204b9b DCM.cpp
--- a/DCM.cpp	Tue Oct 15 08:24:34 2013 +0000
+++ b/DCM.cpp	Wed Oct 16 07:49:34 2013 +0000
@@ -7,7 +7,7 @@
 #define GRAVITY 255
 
 #include "mbed.h"
-Serial p(USBTX, USBRX); // tx, rx
+//Serial p(USBTX, USBRX); // tx, rx
 //
 //void DCM::pv(float a[3])
 //{
@@ -37,15 +37,14 @@
         for (int n=0; n < 3; n++) {
             dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
         }
-    reset_sensor_fusion();
     }
 }
 
 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];
+    Accel_Vector[1] = AccelV[1];
+    Accel_Vector[2] = AccelV[2];
     Gyro_Vector[0] = GyroV[0];
     Gyro_Vector[1] = GyroV[1];
     Gyro_Vector[2] = GyroV[2];
@@ -223,13 +222,21 @@
     MAG_Heading = atan2(-mag_y, mag_x);
 }
 
-void DCM::reset_sensor_fusion()
+void DCM::reset_sensor_fusion(float AccelV[], float GyroV[], float MagnV[])
 {
+    Accel_Vector[0] = AccelV[0];
+    Accel_Vector[1] = AccelV[1];
+    Accel_Vector[2] = AccelV[2];
+    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];
     float temp1[3];
     float temp2[3];
     float xAxis[] = {1.0f, 0.0f, 0.0f};
         
-    //  timestamp = timer.read_ms();
     
     // GET PITCH
     // Using y-z-plane-component/x-component of gravity vector