Diff: DCM.cpp

Revision:

1:115cf0a84a9d

Parent:

0:62284d27d75e

Child:

2:486018bc6acd

--- a/DCM.cpp	Tue Jan 24 17:40:40 2012 +0000
+++ b/DCM.cpp	Wed Jul 24 01:58:33 2013 +0000
@@ -1,43 +1,12 @@
-/*
-MinIMU-9-mbed-AHRS
-Pololu MinIMU-9 + mbed AHRS (Attitude and Heading Reference System)
-
-Modified and ported to mbed environment by Michael Shimniok
-http://www.bot-thoughts.com/
-
-Basedd on MinIMU-9-Arduino-AHRS
-Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
-
-Copyright (c) 2011 Pololu Corporation.
-http://www.pololu.com/
-
-MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
-http://code.google.com/p/sf9domahrs/
-
-sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
-Julio and Doug Weibel:
-http://code.google.com/p/ardu-imu/
-
-MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
-under the terms of the GNU Lesser General Public License as published by the
-Free Software Foundation, either version 3 of the License, or (at your option)
-any later version.
-
-MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
-WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
-more details.
-
-You should have received a copy of the GNU Lesser General Public License along
-with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
-
-*/
 #include "DCM.h"
 #include "Matrix.h"
 #include "math.h"
 #include "Sensors.h"
 
-#define MAG_SKIP 2
+#include "mbed.h"
+//Serial pc3(USBTX, USBRX);
+
+#define MAG_SKIP 1
 
 float DCM::constrain(float x, float a, float b) 
 {
@@ -51,8 +20,11 @@
 
 
 DCM::DCM(void):
-    G_Dt(0.02), update_count(MAG_SKIP)
+    G_Dt(0.25), 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;
@@ -77,11 +49,11 @@
   float temporary[3][3];
   float renorm=0;
   
-  error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19
+    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_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
   
@@ -118,134 +90,78 @@
   // 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);  //  
-
+    
   Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
   Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
   
   Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
-  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);  
   
   //*****YAW***************
   // We make the gyro YAW drift correction based on compass magnetic heading
+    mag_heading_x = cos(MAG_Heading);
+    mag_heading_y = sin(MAG_Heading);
+    errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x);  //Calculating YAW error
+    Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+    
+    Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
+    Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
+    
+    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
 
-  /* http://tinyurl.com/7bul438
-   * William Premerlani:
-   * 1. If you are treating the magnetometer as a tilt compensated compass, it will not work for pitch values near 90 degrees.
-   *    A better way to do it is to use the magnetometer measurement as a reference vector instead. Use the direction cosine
-   *    matrix to transform the magnetometer vector from body frame to earth frame, which works in any orientation, even with
-   *    90 degree pitch. Then, extract the horizontal component of the magnetometer earth frame vector, and take the cross
-   *    product of it with the known horizontal component of the earth's magnetic field. The result is a rotation error vector
-   *    that you transform back into the body frame, and use it to compensate for gyro drift. That is technique we are using in
-   *    MatrixPilot, it works for any orientation. A combination of two reference vectors (magnetometer and accelerometer) will
-   *     provide a 3 axis lock.
-   * 2. If you are using Euler angles to represent orientation, they do not work for 90 degree pitch. There is an effect known 
-   *    as "gimbal lock" that throws off the roll. It is better to use either an angle and rotation axis representation, or
-   *    quaternions.
-   *
-   * ummm... we're no actually calculating MAG_Heading anywhere... so it's zero...
-   * mag_earth[3x1] = mag[3x1] dot dcm[3x3]
-   * earth_rotation_error_vector = mag_earth[x and y] cross known_earth_mag[??]
-   * gyro drift error aka body_rotation_error_vector = earth_rotation_error_Vector times dcm[3x3]
-  float mag_earth[3], mag_sensor[3];
-  mag_sensor[0] = magnetom_x;
-  mag_sensor[1] = magnetom_y;
-  mag_sensor[2] = magnetom_z;
-  mag_earth[0] = VectorDotProduct( &dcm[0] , mag_sensor ) << 1;
-  mag_earth[1] = VectorDotProduct( &dcm[1] , mag_sensor ) << 1;
-  mag_earth[2] = VectorDotProduct( &dcm[2] , mag_sensor ) << 1;
-  mag_error = 100 * VectorDotProduct( 2 , mag_earth , declinationVector ) ; // Dotgain = 1/2
-  VectorScale( 3 , errorYawplane , &rmat[6] , mag_error ) ; // Scalegain = 1/2
-   */
- 
-  mag_heading_x = cos(MAG_Heading);
-  mag_heading_y = sin(MAG_Heading);
-  errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x);  //Calculating YAW error
-  Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
-  
-  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
-  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
-  
-  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
 }
 
-/**************************************************/
-void DCM::Accel_adjust(void)
-{
- Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]);  // Centrifugal force on Acc_y = GPS_speed*GyroZ
- Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY
- // Add linear (x-axis) acceleration correction here
- 
-// from MatrixPilot
-// total (3D) airspeed in cm/sec is used to adjust for acceleration
-//gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , air_speed_3DGPS ) ;
-//gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , air_speed_3DGPS ) ;
-//gplane[1]=gplane[1]+ ((unsigned int)(ACCELSCALE))*forward_acceleration 
-}
 
 /**************************************************/
 void DCM::Matrix_update(void)
 {
-  // TODO: Hardware-specific routines to convert gyro to units; this (probably) should be handled elsewhere
 
-  Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
-  Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
-  Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
-  
-  // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
-  Accel_Vector[0]=accel_x;
-  Accel_Vector[1]=accel_y;
-  Accel_Vector[2]=accel_z;
-    
-  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
+    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
 
-  // Remove centrifugal & linear acceleration.   
-  Accel_adjust();    
-  
- #if OUTPUTMODE==1         
-  Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
-  Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
-  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;
- #else                    // Uncorrected data (no drift correction)
-  Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
-  Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
-  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][2]=0;
- #endif
-
-  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)
-    for(int y=0; y<3; y++) {
-      dcm[x][y] += Temporary_Matrix[x][y];
-    } 
-  }
+    #if OUTPUTMODE==1         
+    Update_Matrix[0][0]=0;
+    Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+    Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+    Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+    Update_Matrix[1][1]=0;
+    Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+    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;
+    #else                    // Uncorrected data (no drift correction)
+    Update_Matrix[0][0]=0;
+    Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
+    Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
+    Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
+    Update_Matrix[1][1]=0;
+    Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
+    Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
+    Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
+    Update_Matrix[2][2]=0;
+    #endif
+    
+    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)
+        for(int y=0; y<3; y++) {
+            dcm[x][y] += Temporary_Matrix[x][y];
+        } 
+    }
 }
 
 void DCM::Euler_angles(void)
 {
-  pitch = -asin(dcm[2][0]);
-  roll = atan2(dcm[2][1],dcm[2][2]);
-  yaw = atan2(dcm[1][0],dcm[0][0]);
+  pitch = RAD2DEG(-asin(dcm[2][0]));
+  roll = RAD2DEG(atan2(dcm[2][1],dcm[2][2]));
+  yaw = RAD2DEG(atan2(dcm[1][0],dcm[0][0]));
 }
 
 void DCM::Update_step()
-{         
+{
+    Read_Accel();         
     Read_Gyro();
-    Read_Accel();
     if (--update_count == 0) {
         Update_mag();
         update_count = MAG_SKIP;
@@ -253,12 +169,34 @@
     Matrix_update();
     Normalize();
     Drift_correction();
-    //Accel_adjust();
     Euler_angles();
 }
 
 void DCM::Update_mag()
 {
-    Read_Compass();
+    Read_Magn();
     Compass_Heading();
 }
+
+void DCM::Compass_Heading()
+{
+    float mag_x;
+    float mag_y;
+    float cos_roll;
+    float sin_roll;
+    float cos_pitch;
+    float sin_pitch;
+  
+    cos_roll = cos(roll);
+    sin_roll = sin(roll);
+    cos_pitch = cos(pitch);
+    sin_pitch = sin(pitch);
+  
+    // Tilt compensated magnetic field X
+    mag_x = Magn_Vector[0]*cos_pitch + Magn_Vector[1]*sin_roll*sin_pitch + Magn_Vector[2]*cos_roll*sin_pitch;
+    // Tilt compensated magnetic field Y
+    mag_y = Magn_Vector[1]*cos_roll - Magn_Vector[2]*sin_roll;
+    // Magnetic Heading
+    MAG_Heading = atan2(-mag_y, mag_x);
+}
+