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

AP_IMU_Oilpan.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
00002 //
00003 //
00004 //      AP_IMU.cpp - IMU Sensor Library for Ardupilot Mega
00005 //              Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
00006 //
00007 //      This library works with the ArduPilot Mega and "Oilpan"
00008 //
00009 //      This library is free software; you can redistribute it and/or
00010 //      modify it under the terms of the GNU Lesser General Public
00011 //      License as published by the Free Software Foundation; either
00012 //      version 2.1 of the License, or (at your option) any later version.
00013 //
00014 
00017 
00018 #include <FastSerial.h>
00019 #include <AP_Common.h>
00020 
00021 #include <avr/eeprom.h>
00022 
00023 #include "AP_IMU_Oilpan.h"
00024 
00025 #define A_LED_PIN 37                    //37 = A,       35 = C
00026 #define C_LED_PIN 35
00027 
00028 // ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
00029 // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
00030 // Tested value : 418
00031 #define GRAVITY                 418.0                           // 1G in the raw data coming from the accelerometer
00032 #define accel_scale(x)  (x*9.80665/GRAVITY)     // Scaling the raw data of the accel to actual acceleration in m/s/s
00033 
00034 // IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
00035 // Tested values : 0.4026, ?, 0.4192
00036 #define _gyro_gain_x    0.4                                     //X axis Gyro gain
00037 #define _gyro_gain_y    0.41                            //Y axis Gyro gain
00038 #define _gyro_gain_z    0.41                            //Z axis Gyro
00039 
00040 #define ADC_CONSTRAINT  900
00041 
00042 // Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
00043 const uint8_t AP_IMU_Oilpan::_sensors[6]       = { 1, 2, 0, 4, 5, 6};   // For ArduPilot Mega Sensor Shield Hardware
00044 const int8_t  AP_IMU_Oilpan::_sensor_signs[6]   = {     1,-1,-1, 1,-1,-1};
00045 
00046 // Temp compensation curve constants
00047 // These must be produced by measuring data and curve fitting
00048 // [X/Y/Z gyro][A/B/C or 0 order/1st order/2nd order constants]
00049 const float   AP_IMU_Oilpan::_gyro_temp_curve[3][3] = {
00050         {1665,0,0},
00051         {1665,0,0},
00052         {1665,0,0}
00053 };      // To Do - make additional constructors to pass this in.
00054 
00055 void
00056 AP_IMU_Oilpan::init(Start_style style)
00057 {
00058         init_gyro(style);
00059         init_accel(style);
00060 }
00061 
00062 /**************************************************/
00063 
00064 void
00065 AP_IMU_Oilpan::init_gyro(Start_style style)
00066 {
00067         float temp;
00068         int flashcount = 0;
00069         int tc_temp;
00070         float adc_in[6];
00071 
00072         // warm start, load saved cal from EEPROM
00073         if ((WARM_START == style) && (0 != _address)) {
00074                 _adc_offset[0] = read_EE_float(_address );
00075                 _adc_offset[1] = read_EE_float(_address + 4);
00076                 _adc_offset[2] = read_EE_float(_address + 8);
00077                 return;
00078         }
00079 
00080         // cold start
00081         tc_temp = _adc->Ch(_gyro_temp_ch);
00082         delay(500);
00083         Serial.println("Init Gyro");
00084 
00085         for(int c = 0; c < 200; c++){
00086                 digitalWrite(A_LED_PIN, LOW);
00087                 digitalWrite(C_LED_PIN, HIGH);
00088                 delay(20);
00089 
00090                 for (int i = 0; i < 6; i++)
00091                         adc_in[i] = _adc->Ch(_sensors[i]);
00092 
00093                 digitalWrite(A_LED_PIN, HIGH);
00094                 digitalWrite(C_LED_PIN, LOW);
00095                 delay(20);
00096         }
00097 
00098         for(int i = 0; i < 200; i++){
00099                 for (int j = 0; j <= 2; j++){
00100                         adc_in[j] = _adc->Ch(_sensors[j]);
00101 
00102                         // Subtract temp compensated typical gyro bias
00103                         adc_in[j] -= _gyro_temp_comp(j, tc_temp);
00104 
00105                         // filter
00106                         _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1;
00107                         //Serial.print(_adc_offset[j], 1);
00108                         //Serial.print(", ");
00109                 }
00110                 //Serial.println(" ");
00111 
00112                 delay(20);
00113                 if(flashcount == 5) {
00114                         Serial.print("*");
00115                         digitalWrite(A_LED_PIN, LOW);
00116                         digitalWrite(C_LED_PIN, HIGH);
00117                 }
00118 
00119                 if(flashcount >= 10) {
00120                         flashcount = 0;
00121                         digitalWrite(C_LED_PIN, LOW);
00122                         digitalWrite(A_LED_PIN, HIGH);
00123                 }
00124                 flashcount++;
00125         }
00126         Serial.println(" ");
00127 
00128         _save_gyro_cal();
00129 }
00130 
00131 
00132 void
00133 AP_IMU_Oilpan::init_accel(Start_style style) // 3, 4, 5
00134 {
00135         float temp;
00136         int flashcount = 0;
00137         float adc_in[6];
00138 
00139         // warm start, load our saved cal from EEPROM
00140         if ((WARM_START == style) && (0 != _address)) {
00141                 _adc_offset[3] = read_EE_float(_address + 12);
00142                 _adc_offset[4] = read_EE_float(_address + 16);
00143                 _adc_offset[5] = read_EE_float(_address + 20);
00144                 return;
00145         }
00146 
00147         // cold start
00148         delay(500);
00149 
00150         Serial.println("Init Accel");
00151 
00152         for (int j = 3; j <= 5; j++){
00153                 adc_in[j]               = _adc->Ch(_sensors[j]);
00154                 adc_in[j]               -= 2025;                // XXX bias value?
00155                 _adc_offset[j]  = adc_in[j];
00156         }
00157 
00158         for(int i = 0; i < 200; i++){           // We take some readings...
00159 
00160                 delay(20);
00161 
00162                 for (int j = 3; j <= 5; j++){
00163                         adc_in[j]               = _adc->Ch(_sensors[j]);
00164                         adc_in[j]               -= 2025;
00165                         _adc_offset[j]  = _adc_offset[j] * 0.9 + adc_in[j] * 0.1;
00166                         //Serial.print(j);
00167                         //Serial.print(": ");
00168                         //Serial.print(adc_in[j], 1);
00169                         //Serial.print(" | ");
00170                         //Serial.print(_adc_offset[j], 1);
00171                         //Serial.print(", ");
00172                 }
00173 
00174                 //Serial.println(" ");
00175 
00176                 if(flashcount == 5) {
00177                         Serial.print("*");
00178                         digitalWrite(A_LED_PIN, LOW);
00179                         digitalWrite(C_LED_PIN, HIGH);
00180                 }
00181 
00182                 if(flashcount >= 10) {
00183                         flashcount = 0;
00184                         digitalWrite(C_LED_PIN, LOW);
00185                         digitalWrite(A_LED_PIN, HIGH);
00186                 }
00187                 flashcount++;
00188         }
00189         Serial.println(" ");
00190         _adc_offset[5] += GRAVITY * _sensor_signs[5];
00191 
00192         _save_accel_cal();
00193 }
00194 
00195 void
00196 AP_IMU_Oilpan::zero_accel(void) // 3, 4, 5
00197 {
00198         _adc_offset[3] = 0;
00199         _adc_offset[4] = 0;
00200         _adc_offset[5] = 0;
00201         _save_accel_cal();
00202 }
00203 
00204 void
00205 AP_IMU_Oilpan::_save_gyro_cal(void)
00206 {
00207         // save cal to EEPROM for warm start
00208         if (0 != _address) {
00209                 write_EE_float(_adc_offset[0], _address);
00210                 write_EE_float(_adc_offset[1], _address + 4);
00211                 write_EE_float(_adc_offset[2], _address + 8);
00212         }
00213 }
00214 
00215 void
00216 AP_IMU_Oilpan::_save_accel_cal(void)
00217 {
00218         // save cal to EEPROM for warm start
00219         if (0 != _address) {
00220                 write_EE_float(_adc_offset[3], _address + 12);
00221                 write_EE_float(_adc_offset[4], _address + 16);
00222                 write_EE_float(_adc_offset[5], _address + 20);
00223         }
00224 }
00225 
00226 /**************************************************/
00227 // Returns the temperature compensated raw gyro value
00228 //---------------------------------------------------
00229 float
00230 AP_IMU_Oilpan::_gyro_temp_comp(int i, int temp) const
00231 {
00232         // We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2
00233         //------------------------------------------------------------------------
00234         return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp;
00235 }
00236 
00237 float
00238 AP_IMU_Oilpan::_gyro_in(uint8_t channel, int temperature)
00239 {
00240         float   adc_in;
00241 
00242         adc_in = _adc->Ch(_sensors[channel]);
00243         adc_in -= _gyro_temp_comp(channel, temperature);                // Subtract temp compensated typical gyro bias
00244         if (_sensor_signs[channel] < 0) {
00245                 adc_in = _adc_offset[channel] - adc_in;
00246         } else {
00247                 adc_in = adc_in - _adc_offset[channel];
00248         }
00249 
00250         if (fabs(adc_in) > ADC_CONSTRAINT) {
00251                 adc_constraints++;                                                                                              // We keep track of the number of times
00252                 adc_in = constrain(adc_in, -ADC_CONSTRAINT, ADC_CONSTRAINT);    // Throw out nonsensical values
00253         }
00254         return adc_in;
00255 }
00256 
00257 float
00258 AP_IMU_Oilpan::_accel_in(uint8_t channel)
00259 {
00260         float   adc_in;
00261 
00262         adc_in = _adc->Ch(_sensors[channel]);
00263         adc_in -= 2025;                                                                         // Subtract typical accel bias
00264 
00265         if (_sensor_signs[channel] < 0) {
00266                 adc_in = _adc_offset[channel] - adc_in;
00267         } else {
00268                 adc_in = adc_in - _adc_offset[channel];
00269         }
00270 
00271         if (fabs(adc_in) > ADC_CONSTRAINT) {
00272                 adc_constraints++;                                                                                              // We keep track of the number of times
00273                 adc_in = constrain(adc_in, -ADC_CONSTRAINT, ADC_CONSTRAINT);    // Throw out nonsensical values
00274         }
00275         return adc_in;
00276 }
00277 
00278 bool
00279 AP_IMU_Oilpan::update(void)
00280 {
00281         int tc_temp = _adc->Ch(_gyro_temp_ch);
00282         float adc_in[6];
00283 #if 0
00284         // get current gyro readings
00285         for (int i = 0; i < 3; i++) {
00286                 adc_in[i] = _adc->Ch(_sensors[i]);
00287                 adc_in[i] -= _gyro_temp_comp(i,tc_temp);                // Subtract temp compensated typical gyro bias
00288                 if (_sensor_signs[i] < 0)
00289                         adc_in[i] = (_adc_offset[i] - adc_in[i]);
00290                 else
00291                         adc_in[i] = (adc_in[i] - _adc_offset[i]);
00292 
00293                 if (fabs(adc_in[i]) > ADC_CONSTRAINT) {
00294                         adc_constraints++;                                                                                                      // We keep track of the number of times
00295                         adc_in[i] = constrain(adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT);      // Throw out nonsensical values
00296                 }
00297         }
00298 #endif
00299         _gyro.x = ToRad(_gyro_gain_x) * _gyro_in(0, tc_temp);
00300         _gyro.y = ToRad(_gyro_gain_y) * _gyro_in(1, tc_temp);
00301         _gyro.z = ToRad(_gyro_gain_z) * _gyro_in(2, tc_temp);
00302 #if 0
00303         // get current accelerometer readings
00304         for (int i = 3; i < 6; i++) {
00305                 adc_in[i] = _adc->Ch(_sensors[i]);
00306                 adc_in[i] -= 2025;                                                              // Subtract typical accel bias
00307 
00308                 if (_sensor_signs[i] < 0)
00309                         adc_in[i] = _adc_offset[i] - adc_in[i];
00310                 else
00311                         adc_in[i] = adc_in[i] - _adc_offset[i];
00312 
00313                 if (fabs(adc_in[i]) > ADC_CONSTRAINT) {
00314                         adc_constraints++;                                                                                                      // We keep track of the number of times
00315                         adc_in[i] = constrain(adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT);      // Throw out nonsensical values
00316                 }
00317         }
00318 #endif
00319         _accel.x = accel_scale(_accel_in(3));
00320         _accel.y = accel_scale(_accel_in(4));
00321         _accel.z = accel_scale(_accel_in(5));
00322 
00323         // always updated
00324         return true;
00325 }
00326 
00327 /********************************************************************************/
00328 
00329 void
00330 AP_IMU_Oilpan::print_accel_offsets(void)
00331 {
00332         Serial.print("Accel offsets: ");
00333         Serial.print(_adc_offset[3], 2);
00334         Serial.print(", ");
00335         Serial.print(_adc_offset[4], 2);
00336         Serial.print(", ");
00337         Serial.println(_adc_offset[5], 2);
00338 }
00339 
00340 void
00341 AP_IMU_Oilpan::print_gyro_offsets(void)
00342 {
00343         Serial.print("Gyro offsets: ");
00344         Serial.print(_adc_offset[0], 2);
00345         Serial.print(", ");
00346         Serial.print(_adc_offset[1], 2);
00347         Serial.print(", ");
00348         Serial.println(_adc_offset[2], 2);
00349 }
00350 
00351 /********************************************************************************/
00352 
00353 float
00354 AP_IMU_Oilpan::read_EE_float(int address)
00355 {
00356         union {
00357                 byte bytes[4];
00358                 float value;
00359         } _floatOut;
00360 
00361         for (int i = 0; i < 4; i++)
00362                 _floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
00363         return _floatOut.value;
00364 }
00365 
00366 void
00367 AP_IMU_Oilpan::write_EE_float(float value, int address)
00368 {
00369         union {
00370                 byte bytes[4];
00371                 float value;
00372         } _floatIn;
00373 
00374         _floatIn.value = value;
00375         for (int i = 0; i < 4; i++)
00376                 eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
00377 }
00378 

Generated for ArduPilot Libraries by doxygen