Prosper Van / DCM_AHRS_GY80

AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5 built for sensor gy_80

Dependents:   quadCommand2

Fork of DCM_AHRS by Michael Shimniok

Files at this revision

API Documentation at this revision

Revision 14:6cb3c2204b9b, committed 2013-10-16

Comitter:
oprospero
Date:
Wed Oct 16 07:49:34 2013 +0000
Parent:
13:6134f21cad37
Commit message:
Fixed vector copy;

Changed in this revision

DCM.cpp Show annotated file Show diff for this revision Revisions of this file
DCM.h Show annotated file Show diff for this revision Revisions of this file 
--- 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

--- a/DCM.h	Tue Oct 15 08:24:34 2013 +0000
+++ b/DCM.h	Wed Oct 16 07:49:34 2013 +0000
@@ -35,6 +35,7 @@
          * than gyro/accelerometer-based update
          */
         void Update_step(float dt, float AccelV[], float GyroV[], float MagnV[]);
+        void reset_sensor_fusion(float AccelV[], float GyroV[], float MagnV[]);
 
         float dcm[3][3];        // The Direction Cosine Matrix
     private:
@@ -61,7 +62,6 @@
         void Euler_angles(void);
         void Update_mag(void);
         void Compass_Heading(void);
-        void reset_sensor_fusion(void);
         void pv(float a[3]);
         void pm(float a[3][3]);