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

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

Go to the documentation of this file.
00001 /*
00002         AP_IMU.cpp - IMU Sensor Library 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                 quick_init()    : For air restart
00014                 init()                  : Calibration
00015                 gyro_init()                     : For ground start using saved accel offsets
00016                                 get_gyro()              : Returns gyro vector.  Elements in radians/second
00017                                 get_accel()             : Returns acceleration vector.  Elements in meters/seconds squared
00018 
00019 */
00020 
00021 #include <AP_IMU.h>
00022 
00023 #define A_LED_PIN 37                    //37 = A,       35 = C
00024 #define C_LED_PIN 35
00025 
00026 // ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
00027 // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
00028 // Tested value : 418
00029 #define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer 
00030 #define accel_scale(x) (x*9.80665/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared
00031 
00032 #define ToRad(x) (x*0.01745329252)      // *pi/180
00033 #define ToDeg(x) (x*57.2957795131)      // *180/pi
00034 
00035 // IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
00036 // Tested values : 0.4026, ?, 0.4192
00037 #define _gyro_gain_x 0.4 //X axis Gyro gain
00038 #define _gyro_gain_y 0.41 //Y axis Gyro gain
00039 #define _gyro_gain_z 0.41 //Z axis Gyro 
00040 
00041 #define ADC_CONSTRAINT 900
00042 
00043 // Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
00044 const uint8_t AP_IMU::_sensors[6]       = {1,2,0,4,5,6};        // For ArduPilot Mega Sensor Shield Hardware
00045 const int     AP_IMU::_sensor_signs[]   = {      1, -1, -1,
00046                                                                                          1, -1, -1};    
00047 
00048 // Temp compensation curve constants
00049 // These must be produced by measuring data and curve fitting
00050 // [X/Y/Z gyro][A/B/C or 0 order/1st order/2nd order constants]
00051 const float   AP_IMU::_gyro_temp_curve[3][3] = {
00052         {1665,0,0},
00053         {1665,0,0},
00054         {1665,0,0}
00055 };      // To Do - make additional constructors to pass this in.
00056 
00057 void
00058 AP_IMU::init(void)
00059 {
00060         init_gyro();
00061         init_accel();
00062 }
00063 
00064 /**************************************************/
00065 
00066 void
00067 AP_IMU::init_gyro(void)
00068 {
00069         
00070         float temp;
00071         int flashcount = 0;
00072         int tc_temp = _adc->Ch(_gyro_temp_ch);
00073         delay(500);
00074         Serial.println("Init Gyro");
00075         
00076         for(int c = 0; c < 200; c++){
00077                 digitalWrite(A_LED_PIN, LOW);
00078                 digitalWrite(C_LED_PIN, HIGH);
00079                 delay(20);
00080                 
00081                 for (int i = 0; i < 6; i++)
00082                         _adc_in[i] = _adc->Ch(_sensors[i]);
00083 
00084                 digitalWrite(A_LED_PIN, HIGH);
00085                 digitalWrite(C_LED_PIN, LOW);
00086                 delay(20);
00087         }
00088 
00089         for(int i = 0; i < 200; i++){
00090                 for (int j = 0; j <= 2; j++){
00091                         _adc_in[j] = _adc->Ch(_sensors[j]);
00092                         
00093                         // Subtract temp compensated typical gyro bias
00094                         _adc_in[j] -= gyro_temp_comp(j, tc_temp);
00095                         
00096                         // filter
00097                         _adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1;
00098                         //Serial.print(_adc_offset[j], 1);
00099                         //Serial.print(", ");
00100                 }
00101                 //Serial.println(" ");
00102 
00103                 delay(20);
00104                 if(flashcount == 5) {
00105                         Serial.print("*");
00106                         digitalWrite(A_LED_PIN, LOW);
00107                         digitalWrite(C_LED_PIN, HIGH);
00108                 }
00109                 
00110                 if(flashcount >= 10) {
00111                         flashcount = 0;
00112                         digitalWrite(C_LED_PIN, LOW);
00113                         digitalWrite(A_LED_PIN, HIGH);
00114                 }
00115                 flashcount++;
00116         }
00117         Serial.println(" ");
00118 
00119         save_gyro_eeprom();
00120 }
00121 
00122 
00123 void
00124 AP_IMU::init_accel(void) // 3, 4, 5
00125 {
00126         float temp;
00127         int flashcount = 0;
00128         delay(500);
00129 
00130         Serial.println("Init Accel");
00131 
00132         for (int j = 3; j <= 5; j++){
00133                 _adc_in[j]              = _adc->Ch(_sensors[j]);
00134                 _adc_in[j]              -= 2025;
00135                 _adc_offset[j]  = _adc_in[j];
00136         }
00137         
00138         for(int i = 0; i < 200; i++){           // We take some readings...
00139                 
00140                 delay(20);
00141                 
00142                 for (int j = 3; j <= 5; j++){
00143                         _adc_in[j]              = _adc->Ch(_sensors[j]);
00144                         _adc_in[j]              -= 2025;
00145                         _adc_offset[j]  = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1;
00146                         //Serial.print(j);
00147                         //Serial.print(": ");
00148                         //Serial.print(_adc_in[j], 1);
00149                         //Serial.print(" | ");
00150                         //Serial.print(_adc_offset[j], 1);
00151                         //Serial.print(", ");
00152                 }
00153                 
00154                 //Serial.println(" ");
00155 
00156                 if(flashcount == 5) {
00157                         Serial.print("*");
00158                         digitalWrite(A_LED_PIN, LOW);
00159                         digitalWrite(C_LED_PIN, HIGH);
00160                 }
00161                 
00162                 if(flashcount >= 10) {
00163                         flashcount = 0;
00164                         digitalWrite(C_LED_PIN, LOW);
00165                         digitalWrite(A_LED_PIN, HIGH);
00166                 }
00167                 flashcount++;
00168         }
00169         Serial.println(" ");
00170         _adc_offset[5] += GRAVITY * _sensor_signs[5];
00171         save_accel_eeprom();
00172 }
00173 
00174 void
00175 AP_IMU::zero_accel(void) // 3, 4, 5
00176 {
00177         _adc_offset[3] = 0;
00178         _adc_offset[4] = 0;
00179         _adc_offset[5] = 0;
00180         save_accel_eeprom();
00181 }
00182 /**************************************************/
00183 // Returns the temperature compensated raw gyro value
00184 //---------------------------------------------------
00185 float
00186 AP_IMU::gyro_temp_comp(int i, int temp) const
00187 {
00188         // We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2
00189         //------------------------------------------------------------------------
00190         return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp;   
00191 }
00192 
00193 /**************************************************/
00194 Vector3f
00195 AP_IMU::get_gyro(void)
00196 {
00197         int tc_temp = _adc->Ch(_gyro_temp_ch);
00198         
00199         for (int i = 0; i < 3; i++) {
00200                 _adc_in[i] = _adc->Ch(_sensors[i]);
00201                 _adc_in[i] -= gyro_temp_comp(i,tc_temp);                // Subtract temp compensated typical gyro bias
00202                 if (_sensor_signs[i] < 0)
00203                         _adc_in[i] = (_adc_offset[i] - _adc_in[i]);
00204                 else
00205                         _adc_in[i] = (_adc_in[i] - _adc_offset[i]);
00206                         
00207                 if (fabs(_adc_in[i]) > ADC_CONSTRAINT) {
00208                         adc_constraints++;                                                                                                              // We keep track of the number of times         
00209                         _adc_in[i] = constrain(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT);    // Throw out nonsensical values
00210                 }
00211         }
00212 
00213         _gyro_vector.x = ToRad(_gyro_gain_x) * _adc_in[0];
00214         _gyro_vector.y = ToRad(_gyro_gain_y) * _adc_in[1];
00215         _gyro_vector.z = ToRad(_gyro_gain_z) * _adc_in[2];
00216         
00217         return _gyro_vector;
00218 }
00219 
00220 /**************************************************/
00221 Vector3f
00222 AP_IMU::get_accel(void)
00223 {       
00224         for (int i = 3; i < 6; i++) {
00225                 _adc_in[i] = _adc->Ch(_sensors[i]);
00226                 _adc_in[i] -= 2025;                                                             // Subtract typical accel bias
00227 
00228                 if (_sensor_signs[i] < 0)
00229                         _adc_in[i] = _adc_offset[i] - _adc_in[i];
00230                 else
00231                         _adc_in[i] = _adc_in[i] - _adc_offset[i];
00232                         
00233                 if (fabs(_adc_in[i]) > ADC_CONSTRAINT) {
00234                         adc_constraints++;                                                                                              // We keep track of the number of times         
00235                         _adc_in[i] = constrain(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT);    // Throw out nonsensical values
00236                 }
00237         }
00238         
00239         _accel_vector.x = accel_scale(_adc_in[3]);
00240         _accel_vector.y = accel_scale(_adc_in[4]);
00241         _accel_vector.z = accel_scale(_adc_in[5]);      
00242         
00243         return _accel_vector;
00244 }
00245 
00246 /********************************************************************************/
00247 
00248 void
00249 AP_IMU::load_gyro_eeprom(void)
00250 {
00251         _adc_offset[0] = read_EE_float(_address );
00252         _adc_offset[1] = read_EE_float(_address + 4);
00253         _adc_offset[2] = read_EE_float(_address + 8);
00254 }
00255 
00256 void
00257 AP_IMU::save_gyro_eeprom(void)
00258 {
00259         write_EE_float(_adc_offset[0], _address);
00260         write_EE_float(_adc_offset[1], _address + 4);
00261         write_EE_float(_adc_offset[2], _address + 8);
00262 }
00263 
00264 /********************************************************************************/
00265 
00266 void
00267 AP_IMU::load_accel_eeprom(void)
00268 {
00269         _adc_offset[3] = read_EE_float(_address + 12);
00270         _adc_offset[4] = read_EE_float(_address + 16);
00271         _adc_offset[5] = read_EE_float(_address + 20);
00272 }
00273 
00274 void
00275 AP_IMU::save_accel_eeprom(void)
00276 {
00277         write_EE_float(_adc_offset[3], _address + 12);
00278         write_EE_float(_adc_offset[4], _address + 16);
00279         write_EE_float(_adc_offset[5], _address + 20);
00280 }
00281 
00282 void 
00283 AP_IMU::print_accel_offsets(void)
00284 {
00285         Serial.print("Accel offsets: ");
00286         Serial.print(_adc_offset[3], 2);
00287         Serial.print(", ");
00288         Serial.print(_adc_offset[4], 2);
00289         Serial.print(", ");
00290         Serial.println(_adc_offset[5], 2);
00291 }
00292 
00293 void 
00294 AP_IMU::print_gyro_offsets(void)
00295 {
00296         Serial.print("Gyro offsets: ");
00297         Serial.print(_adc_offset[0], 2);
00298         Serial.print(", ");
00299         Serial.print(_adc_offset[1], 2);
00300         Serial.print(", ");
00301         Serial.println(_adc_offset[2], 2);
00302 }
00303 
00304 
00305 
00306 /********************************************************************************/
00307 
00308 float
00309 AP_IMU::read_EE_float(int address)
00310 {
00311         union {
00312                 byte bytes[4];
00313                 float value;
00314         } _floatOut;
00315         
00316         for (int i = 0; i < 4; i++) 
00317                 _floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
00318         return _floatOut.value;
00319 }
00320 
00321 void
00322 AP_IMU::write_EE_float(float value, int address)
00323 {
00324         union {
00325                 byte bytes[4];
00326                 float value;
00327         } _floatIn;
00328         
00329         _floatIn.value = value;
00330         for (int i = 0; i < 4; i++) 
00331                 eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
00332 }
00333 

Generated for ArduPilot Libraries by doxygen