DCM.cpp

00001 /*
00002 MinIMU-9-mbed-AHRS
00003 Pololu MinIMU-9 + mbed AHRS (Attitude and Heading Reference System)
00004 
00005 Modified and ported to mbed environment by Michael Shimniok
00006 http://www.bot-thoughts.com/
00007 
00008 Basedd on MinIMU-9-Arduino-AHRS
00009 Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
00010 
00011 Copyright (c) 2011 Pololu Corporation.
00012 http://www.pololu.com/
00013 
00014 MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
00015 http://code.google.com/p/sf9domahrs/
00016 
00017 sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
00018 Julio and Doug Weibel:
00019 http://code.google.com/p/ardu-imu/
00020 
00021 MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
00022 under the terms of the GNU Lesser General Public License as published by the
00023 Free Software Foundation, either version 3 of the License, or (at your option)
00024 any later version.
00025 
00026 MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
00027 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00028 FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
00029 more details.
00030 
00031 You should have received a copy of the GNU Lesser General Public License along
00032 with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
00033 
00034 */
00035 #include "DCM.h"
00036 #include "Matrix.h"
00037 #include "math.h"
00038 #include "Sensors.h"
00039 
00040 #define MAG_SKIP 2
00041 
00042 float DCM::constrain(float x, float a, float b) 
00043 {
00044     float result = x;
00045     
00046     if (x < a) result = a;
00047     if (x > b) result = b;
00048 
00049     return result;  
00050 }
00051 
00052 
00053 DCM::DCM(void):
00054     G_Dt(0.02), update_count(MAG_SKIP)
00055 {
00056     for (int m=0; m < 3; m++) {
00057         Accel_Vector[m] = 0;
00058         Gyro_Vector[m] = 0;
00059         Omega_Vector[m] = 0;
00060         Omega_P[m] = 0;
00061         Omega_I[m] = 0;
00062         Omega[m] = 0;
00063         errorRollPitch[m] = 0; 
00064         errorYaw[m] = 0;
00065         for (int n=0; n < 3; n++) {
00066             dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
00067         }
00068     }
00069     
00070 }
00071 
00072 
00073 /**************************************************/
00074 void DCM::Normalize(void)
00075 {
00076   float error=0;
00077   float temporary[3][3];
00078   float renorm=0;
00079   
00080   error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19
00081 
00082   Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
00083   Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
00084   
00085   Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
00086   Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
00087   
00088   Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
00089   
00090   renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
00091   Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
00092   
00093   renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
00094   Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
00095   
00096   renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
00097   Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
00098 }
00099 
00100 /**************************************************/
00101 void DCM::Drift_correction(void)
00102 {
00103   float mag_heading_x;
00104   float mag_heading_y;
00105   float errorCourse;
00106   //Compensation the Roll, Pitch and Yaw drift. 
00107   static float Scaled_Omega_P[3];
00108   static float Scaled_Omega_I[3];
00109   float Accel_magnitude;
00110   float Accel_weight;
00111   
00112   
00113   //*****Roll and Pitch***************
00114 
00115   // Calculate the magnitude of the accelerometer vector
00116   Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
00117   Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
00118   // Dynamic weighting of accelerometer info (reliability filter)
00119   // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
00120   Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
00121 
00122   Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
00123   Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
00124   
00125   Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
00126   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
00127   
00128   //*****YAW***************
00129   // We make the gyro YAW drift correction based on compass magnetic heading
00130 
00131   /* http://tinyurl.com/7bul438
00132    * William Premerlani:
00133    * 1. If you are treating the magnetometer as a tilt compensated compass, it will not work for pitch values near 90 degrees.
00134    *    A better way to do it is to use the magnetometer measurement as a reference vector instead. Use the direction cosine
00135    *    matrix to transform the magnetometer vector from body frame to earth frame, which works in any orientation, even with
00136    *    90 degree pitch. Then, extract the horizontal component of the magnetometer earth frame vector, and take the cross
00137    *    product of it with the known horizontal component of the earth's magnetic field. The result is a rotation error vector
00138    *    that you transform back into the body frame, and use it to compensate for gyro drift. That is technique we are using in
00139    *    MatrixPilot, it works for any orientation. A combination of two reference vectors (magnetometer and accelerometer) will
00140    *     provide a 3 axis lock.
00141    * 2. If you are using Euler angles to represent orientation, they do not work for 90 degree pitch. There is an effect known 
00142    *    as "gimbal lock" that throws off the roll. It is better to use either an angle and rotation axis representation, or
00143    *    quaternions.
00144    *
00145    * ummm... we're no actually calculating MAG_Heading anywhere... so it's zero...
00146    * mag_earth[3x1] = mag[3x1] dot dcm[3x3]
00147    * earth_rotation_error_vector = mag_earth[x and y] cross known_earth_mag[??]
00148    * gyro drift error aka body_rotation_error_vector = earth_rotation_error_Vector times dcm[3x3]
00149   float mag_earth[3], mag_sensor[3];
00150   mag_sensor[0] = magnetom_x;
00151   mag_sensor[1] = magnetom_y;
00152   mag_sensor[2] = magnetom_z;
00153   mag_earth[0] = VectorDotProduct( &dcm[0] , mag_sensor ) << 1;
00154   mag_earth[1] = VectorDotProduct( &dcm[1] , mag_sensor ) << 1;
00155   mag_earth[2] = VectorDotProduct( &dcm[2] , mag_sensor ) << 1;
00156   mag_error = 100 * VectorDotProduct( 2 , mag_earth , declinationVector ) ; // Dotgain = 1/2
00157   VectorScale( 3 , errorYawplane , &rmat[6] , mag_error ) ; // Scalegain = 1/2
00158    */
00159  
00160   mag_heading_x = cos(MAG_Heading);
00161   mag_heading_y = sin(MAG_Heading);
00162   errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x);  //Calculating YAW error
00163   Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
00164   
00165   Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
00166   Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
00167   
00168   Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
00169   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
00170 }
00171 
00172 /**************************************************/
00173 void DCM::Accel_adjust(void)
00174 {
00175  Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]);  // Centrifugal force on Acc_y = GPS_speed*GyroZ
00176  Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY
00177  // Add linear (x-axis) acceleration correction here
00178  
00179 // from MatrixPilot
00180 // total (3D) airspeed in cm/sec is used to adjust for acceleration
00181 //gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , air_speed_3DGPS ) ;
00182 //gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , air_speed_3DGPS ) ;
00183 //gplane[1]=gplane[1]+ ((unsigned int)(ACCELSCALE))*forward_acceleration 
00184 }
00185 
00186 /**************************************************/
00187 void DCM::Matrix_update(void)
00188 {
00189   // TODO: Hardware-specific routines to convert gyro to units; this (probably) should be handled elsewhere
00190 
00191   Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
00192   Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
00193   Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
00194   
00195   // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
00196   Accel_Vector[0]=accel_x;
00197   Accel_Vector[1]=accel_y;
00198   Accel_Vector[2]=accel_z;
00199     
00200   Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
00201   Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
00202 
00203   // Remove centrifugal & linear acceleration.   
00204   Accel_adjust();    
00205   
00206  #if OUTPUTMODE==1         
00207   Update_Matrix[0][0]=0;
00208   Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
00209   Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
00210   Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
00211   Update_Matrix[1][1]=0;
00212   Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
00213   Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
00214   Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
00215   Update_Matrix[2][2]=0;
00216  #else                    // Uncorrected data (no drift correction)
00217   Update_Matrix[0][0]=0;
00218   Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
00219   Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
00220   Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
00221   Update_Matrix[1][1]=0;
00222   Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
00223   Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
00224   Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
00225   Update_Matrix[2][2]=0;
00226  #endif
00227 
00228   Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b
00229 
00230 // ???  Matrix_Add(dcm, dcm, Temporary_Matrix); ???
00231   for(int x=0; x<3; x++) { //Matrix Addition (update)
00232     for(int y=0; y<3; y++) {
00233       dcm[x][y] += Temporary_Matrix[x][y];
00234     } 
00235   }
00236 }
00237 
00238 void DCM::Euler_angles(void)
00239 {
00240   pitch = -asin(dcm[2][0]);
00241   roll = atan2(dcm[2][1],dcm[2][2]);
00242   yaw = atan2(dcm[1][0],dcm[0][0]);
00243 }
00244 
00245 void DCM::Update_step()
00246 {         
00247     Read_Gyro();
00248     Read_Accel();
00249     if (--update_count == 0) {
00250         Update_mag();
00251         update_count = MAG_SKIP;
00252     }
00253     Matrix_update();
00254     Normalize();
00255     Drift_correction();
00256     //Accel_adjust();
00257     Euler_angles();
00258 }
00259 
00260 void DCM::Update_mag()
00261 {
00262     Read_Compass();
00263     Compass_Heading();
00264 }