• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/AP_DCM/AP_DCM.cpp

Go to the documentation of this file.
00001 /*
00002         APM_DCM_FW.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega
00003                 Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
00004 
00005         This library works with the ArduPilot Mega and "Oilpan"
00006         
00007         This library is free software; you can redistribute it and/or
00008     modify it under the terms of the GNU Lesser General Public
00009     License as published by the Free Software Foundation; either
00010     version 2.1 of the License, or (at your option) any later version.
00011 
00012         Methods:
00013                                 update_DCM(_G_Dt)       : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data
00014                                 get_gyro()                      : Returns gyro vector corrected for bias
00015                                 get_accel()             : Returns accelerometer vector
00016                                 get_dcm_matrix()        : Returns dcm matrix
00017 
00018 */
00019 #include <AP_DCM.h>
00020 
00021 #define OUTPUTMODE 1                            // This is just used for debugging, remove later
00022 #define ToRad(x) (x*0.01745329252)      // *pi/180
00023 #define ToDeg(x) (x*57.2957795131)      // *180/pi
00024 
00025 #define Kp_ROLLPITCH 0.05967            // .0014 * 418/9.81 Pitch&Roll Drift Correction Proportional Gain
00026 #define Ki_ROLLPITCH 0.00001278         // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain
00027 #define Kp_YAW 0.8                                      // Yaw Drift Correction Porportional Gain       
00028 #define Ki_YAW 0.00004                          // Yaw Drift CorrectionIntegrator Gain
00029 
00030 #define SPEEDFILT 300                           // centimeters/second
00031 #define ADC_CONSTRAINT 900
00032 
00033 
00034 void
00035 AP_DCM::set_compass(Compass *compass)
00036 {
00037         _compass = compass;
00038 }
00039 
00040 /**************************************************/
00041 void
00042 AP_DCM::update_DCM(float _G_Dt)
00043 {
00044         _gyro_vector    = _imu->get_gyro();                     // Get current values for IMU sensors
00045         _accel_vector   = _imu->get_accel();                    // Get current values for IMU sensors
00046         
00047         matrix_update(_G_Dt);   // Integrate the DCM matrix
00048         normalize();                    // Normalize the DCM matrix
00049         drift_correction();             // Perform drift correction
00050         
00051         euler_angles();                 // Calculate pitch, roll, yaw for stabilization and navigation
00052 }
00053 
00054 /**************************************************/
00055 
00056     //For Debugging
00057 /*
00058 void
00059 printm(const char *l, Matrix3f &m)
00060 {       Serial.println(" "); Serial.println(l);
00061         Serial.print(m.a.x, 12); Serial.print(" "); Serial.print(m.a.y, 12); Serial.print(" "); Serial.println(m.a.z, 12);
00062         Serial.print(m.b.x, 12); Serial.print(" "); Serial.print(m.b.y, 12); Serial.print(" "); Serial.println(m.b.z, 12);
00063         Serial.print(m.c.x, 12); Serial.print(" "); Serial.print(m.c.y, 12); Serial.print(" "); Serial.println(m.c.z, 12);  
00064         Serial.print(*(uint32_t *)&(m.a.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.a.y), HEX); Serial.print(" ");  Serial.println(*(uint32_t *)&(m.a.z), HEX);
00065         Serial.print(*(uint32_t *)&(m.b.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.b.y), HEX); Serial.print(" ");  Serial.println(*(uint32_t *)&(m.b.z), HEX);
00066         Serial.print(*(uint32_t *)&(m.c.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.c.y), HEX); Serial.print(" ");  Serial.println(*(uint32_t *)&(m.c.z), HEX);
00067 }
00068 */
00069  
00070 /**************************************************/
00071 void 
00072 AP_DCM::matrix_update(float _G_Dt)
00073 {
00074         Matrix3f        update_matrix;
00075         Matrix3f        temp_matrix;
00076         
00077         //Record when you saturate any of the gyros.
00078         if((fabs(_gyro_vector.x) >= radians(300)) || 
00079                 (fabs(_gyro_vector.y) >= radians(300)) || 
00080                 (fabs(_gyro_vector.z) >= radians(300))){
00081                 gyro_sat_count++;
00082         }
00083 
00084         _omega_integ_corr       = _gyro_vector          + _omega_I;             // Used for _centripetal correction (theoretically better than _omega)
00085         _omega                          = _omega_integ_corr + _omega_P;         // Equation 16, adding proportional and integral correction terms               
00086 
00087         if(_centripetal){
00088                 accel_adjust();                         // Remove _centripetal acceleration.
00089         }
00090         
00091  #if OUTPUTMODE == 1                             
00092         update_matrix.a.x = 0;
00093         update_matrix.a.y = -_G_Dt * _omega.z;          // -delta Theta z
00094         update_matrix.a.z =  _G_Dt * _omega.y;          // delta Theta y
00095         update_matrix.b.x =  _G_Dt * _omega.z;          // delta Theta z
00096         update_matrix.b.y = 0;
00097         update_matrix.b.z = -_G_Dt * _omega.x;          // -delta Theta x
00098         update_matrix.c.x = -_G_Dt * _omega.y;          // -delta Theta y
00099         update_matrix.c.y =  _G_Dt * _omega.x;          // delta Theta x
00100         update_matrix.c.z = 0;
00101  #else                                                                          // Uncorrected data (no drift correction)                                
00102         update_matrix.a.x = 0;
00103         update_matrix.a.y = -_G_Dt * _gyro_vector.z;            
00104         update_matrix.a.z =  _G_Dt * _gyro_vector.y;
00105         update_matrix.b.x =  _G_Dt * _gyro_vector.z;            
00106         update_matrix.b.y = 0;
00107         update_matrix.b.z = -_G_Dt * _gyro_vector.x;            
00108         update_matrix.c.x = -_G_Dt * _gyro_vector.y;            
00109         update_matrix.c.y =  _G_Dt * _gyro_vector.x;            
00110         update_matrix.c.z = 0;
00111  #endif
00112 
00113         temp_matrix = _dcm_matrix * update_matrix;
00114         _dcm_matrix = _dcm_matrix + temp_matrix;                // Equation 17
00115                 
00116 }
00117 
00118 
00119 /**************************************************/
00120 void 
00121 AP_DCM::accel_adjust(void)
00122 {
00123         Vector3f veloc, temp;
00124         float vel;
00125         
00126         veloc.x = _gps->ground_speed / 100;             // We are working with acceleration in m/s^2 units
00127         
00128         // We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive
00129 
00130         //_accel_vector -= _omega_integ_corr % _veloc;          // Equation 26  This line is giving the compiler a problem so we break it up below
00131         temp.x = 0;
00132         temp.y = _omega_integ_corr.z * veloc.x;                         // only computing the non-zero terms
00133         temp.z = -1.0f * _omega_integ_corr.y * veloc.x; // After looking at the compiler issue lets remove _veloc and simlify 
00134 
00135         _accel_vector -= temp;
00136 }
00137 
00138 
00139 /*************************************************
00140 Direction Cosine Matrix IMU: Theory
00141 William Premerlani and Paul Bizard
00142 
00143 Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5 
00144 to approximations rather than identities. In effect, the axes in the two frames of reference no 
00145 longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a 
00146 simple matter to stay ahead of it.
00147 We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
00148 */
00149 void 
00150 AP_DCM::normalize(void)
00151 {
00152         float error = 0;
00153         Vector3f        temporary[3];
00154         
00155         int problem = 0;
00156         
00157         error = _dcm_matrix.a * _dcm_matrix.b;                                                  // eq.18
00158 
00159         temporary[0] = _dcm_matrix.b;   
00160         temporary[1] = _dcm_matrix.a;           
00161         temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error));         // eq.19
00162         temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error));         // eq.19
00163 
00164         temporary[2] = temporary[0] % temporary[1];                                                     // c= a x b // eq.20
00165 
00166         _dcm_matrix.a = renorm(temporary[0], problem);
00167         _dcm_matrix.b = renorm(temporary[1], problem);
00168         _dcm_matrix.c = renorm(temporary[2], problem);
00169         
00170         if (problem == 1) {             // Our solution is blowing up and we will force back to initial condition.      Hope we are not upside down!
00171                 _dcm_matrix.a.x = 1.0f;
00172                 _dcm_matrix.a.y = 0.0f;
00173                 _dcm_matrix.a.z = 0.0f;
00174                 _dcm_matrix.b.x = 0.0f;
00175                 _dcm_matrix.b.y = 1.0f;
00176                 _dcm_matrix.b.z = 0.0f;
00177                 _dcm_matrix.c.x = 0.0f;
00178                 _dcm_matrix.c.y = 0.0f;
00179                 _dcm_matrix.c.z = 1.0f;
00180         }
00181 }
00182 
00183 /**************************************************/
00184 Vector3f
00185 AP_DCM::renorm(Vector3f const &a, int &problem)
00186 {
00187         float   renorm;
00188 
00189         renorm = a * a;
00190         
00191         if (renorm < 1.5625f && renorm > 0.64f) {                       // Check if we are OK to use Taylor expansion
00192                 renorm = 0.5 * (3 - renorm);                                    // eq.21
00193         } else if (renorm < 100.0f && renorm > 0.01f) {
00194                 renorm = 1.0 / sqrt(renorm);
00195                 renorm_sqrt_count++;
00196         } else {
00197                 problem = 1;
00198                 renorm_blowup_count++;
00199         }
00200 
00201         return(a * renorm);
00202 }
00203 
00204 /**************************************************/
00205 void 
00206 AP_DCM::drift_correction(void)
00207 {
00208         //Compensation the Roll, Pitch and Yaw drift. 
00209         //float mag_heading_x;
00210         //float mag_heading_y;
00211         float error_course;
00212         float accel_magnitude;
00213         float accel_weight;
00214         float integrator_magnitude;
00215         //static float scaled_omega_P[3];
00216         //static float scaled_omega_I[3];
00217         static bool in_motion = false;
00218         Matrix3f rot_mat;
00219         
00220         //*****Roll and Pitch***************
00221 
00222         // Calculate the magnitude of the accelerometer vector
00223         accel_magnitude = _accel_vector.length() / 9.80665f;
00224 
00225         // Dynamic weighting of accelerometer info (reliability filter)
00226         // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
00227         accel_weight = constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1);      //      
00228         
00229         //      We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
00230         _health += constrain((0.02 * (accel_weight - .5)), 0, 1);
00231 
00232         // adjust the ground of reference 
00233         _error_roll_pitch =  _dcm_matrix.c % _accel_vector;                     // Equation 27  *** sign changed from prev implementation???
00234 
00235         // error_roll_pitch are in Accel m/s^2 units
00236         // Limit max error_roll_pitch to limit max omega_P and omega_I
00237         _error_roll_pitch.x = constrain(_error_roll_pitch.x, -1.17f, 1.17f);
00238         _error_roll_pitch.y = constrain(_error_roll_pitch.y, -1.17f, 1.17f);
00239         _error_roll_pitch.z = constrain(_error_roll_pitch.z, -1.17f, 1.17f);
00240 
00241         _omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight);
00242         _omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight);
00243 
00244         
00245         //*****YAW***************
00246         
00247         if (_compass) {
00248                 // We make the gyro YAW drift correction based on compass magnetic heading
00249                 error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);       // Equation 23, Calculating YAW error   
00250                 
00251         } else {
00252         
00253                 // Use GPS Ground course to correct yaw gyro drift
00254                 if (_gps->ground_speed >= SPEEDFILT) {
00255                         _course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
00256                         _course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
00257                         if(in_motion) {
00258                                 error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x);   // Equation 23, Calculating YAW error
00259                         } else  {
00260                                 float cos_psi_err, sin_psi_err;
00261                                 // This is the case for when we first start moving and reset the DCM so that yaw matches the gps ground course
00262                                 // This is just to get a reasonable estimate faster
00263                                 yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
00264                                 cos_psi_err = cos(ToRad(_gps->ground_course/100.0) - yaw);
00265                                 sin_psi_err = sin(ToRad(_gps->ground_course/100.0) - yaw);
00266                                 // Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0
00267                                 // Ryx = sin psi err, Ryy = cos psi err,   Ryz = 0
00268                                 // Rzx = Rzy = 0, Rzz = 1
00269                                 rot_mat.a.x = cos_psi_err;
00270                                 rot_mat.a.y = - sin_psi_err;
00271                                 rot_mat.b.x = sin_psi_err;
00272                                 rot_mat.b.y = cos_psi_err;
00273                                 rot_mat.a.z = 0;
00274                                 rot_mat.b.z = 0;
00275                                 rot_mat.c.x = 0;
00276                                 rot_mat.c.y = 0;
00277                                 rot_mat.c.z = 1.0;
00278                                 
00279                                 _dcm_matrix = rot_mat * _dcm_matrix;
00280                                 in_motion =  true;
00281                                 error_course = 0;
00282                                 
00283                         }       
00284                 } else {
00285                         error_course = 0;
00286                         in_motion = false;
00287                 }       
00288         }
00289         
00290         _error_yaw = _dcm_matrix.c * error_course;      // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
00291         
00292         _omega_P += _error_yaw * Kp_YAW;                        // Adding yaw correction to proportional correction vector.
00293         _omega_I += _error_yaw * Ki_YAW;                        // adding yaw correction to integrator correction vector.        
00294 
00295         //      Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
00296         integrator_magnitude = _omega_I.length();
00297         if (integrator_magnitude > radians(300)) {
00298                 _omega_I *= (0.5f * radians(300) / integrator_magnitude);               // Why do we have this discontinuous?  EG, why the 0.5?
00299         }
00300         //Serial.print("*");
00301 }
00302 
00303 
00304 /**************************************************/
00305 void 
00306 AP_DCM::euler_angles(void)
00307 {
00308         #if (OUTPUTMODE == 2)                            // Only accelerometer info (debugging purposes)
00309         roll            = atan2(_accel_vector.y, -_accel_vector.z);             // atan2(acc_y, acc_z)
00310         pitch           = asin((_accel_vector.x) / (double)9.81); // asin(acc_x)
00311         yaw                     = 0;
00312         #else
00313         pitch           = -asin(_dcm_matrix.c.x);
00314         roll            = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
00315         yaw             = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
00316         #endif
00317         
00318         roll_sensor     = degrees(roll)  * 100;
00319         pitch_sensor    = degrees(pitch) * 100;
00320         yaw_sensor              = degrees(yaw)   * 100;
00321 
00322         if (yaw_sensor < 0)
00323                 yaw_sensor += 36000;
00324 }
00325 
00326 /**************************************************/
00327 
00328 float
00329 AP_DCM::get_health(void)
00330 {
00331         return _health;
00332 }
00333 
00334 

Generated for ArduPilot Libraries by doxygen