FreeIMU.cpp

00001 /*
00002 FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino
00003 Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net>
00004 
00005 Development of this code has been supported by the Department of Computer Science,
00006 Universita' degli Studi di Torino, Italy within the Piemonte Project
00007 http://www.piemonte.di.unito.it/
00008 
00009 
00010 This program is free software: you can redistribute it and/or modify
00011 it under the terms of the version 3 GNU General Public License as
00012 published by the Free Software Foundation.
00013 
00014 This program is distributed in the hope that it will be useful,
00015 but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017 GNU General Public License for more details.
00018 
00019 You should have received a copy of the GNU General Public License
00020 along with this program.  If not, see <http://www.gnu.org/licenses/>.
00021 
00022 02/20/2013 - Modified by Aloïs Wolff for MBED with MPU6050 only (wolffalois@gmail.com)
00023 */
00024 
00025 //#include <inttypes.h>
00026 //#include <stdint.h>
00027 //#define DEBUG
00028 #include "FreeIMU.h"
00029 #define     M_PI 3.1415926535897932384626433832795
00030 
00031 #ifdef DEBUG
00032  #define DEBUG_PRINT(x) Serial.println(x)
00033  #else
00034  #define DEBUG_PRINT(x)
00035  #endif
00036 // #include "WireUtils.h"
00037 //#include "DebugUtils.h"
00038 
00039 //#include "vector_math.h"
00040 
00041 FreeIMU::FreeIMU() {
00042 
00043     accgyro = MPU6050(0x69); // I2C
00044 
00045   // initialize quaternion
00046   q0 = 1.0f;
00047   q1 = 0.0f;
00048   q2 = 0.0f;
00049   q3 = 0.0f;
00050   exInt = 0.0;
00051   eyInt = 0.0;
00052   ezInt = 0.0;
00053   twoKp = twoKpDef;
00054   twoKi = twoKiDef;
00055   integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;
00056   
00057   
00058   update.start();
00059   int dt_us=0;
00060   /*
00061   lastUpdate = 0;
00062   now = 0;
00063   */
00064   #ifndef CALIBRATION_H
00065   // initialize scale factors to neutral values
00066   acc_scale_x = 1;
00067   acc_scale_y = 1;
00068   acc_scale_z = 1;
00069   magn_scale_x = 1;
00070   magn_scale_y = 1;
00071   magn_scale_z = 1;
00072   #else
00073   // get values from global variables of same name defined in calibration.h
00074   acc_off_x = ::acc_off_x;
00075   acc_off_y = ::acc_off_y;
00076   acc_off_z = ::acc_off_z;
00077   acc_scale_x = ::acc_scale_x;
00078   acc_scale_y = ::acc_scale_y;
00079   acc_scale_z = ::acc_scale_z;
00080   magn_off_x = ::magn_off_x;
00081   magn_off_y = ::magn_off_y;
00082   magn_off_z = ::magn_off_z;
00083   magn_scale_x = ::magn_scale_x;
00084   magn_scale_y = ::magn_scale_y;
00085   magn_scale_z = ::magn_scale_z;
00086   #endif
00087 }
00088 
00089 void FreeIMU::init() {
00090 
00091   init(FIMU_ACCGYRO_ADDR, false);
00092 
00093 }
00094 
00095 void FreeIMU::init(bool fastmode) {
00096  
00097   init(FIMU_ACCGYRO_ADDR, fastmode);
00098  
00099 }
00100 
00101 
00102 /**
00103  * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
00104 */
00105 
00106 void FreeIMU::init(int accgyro_addr, bool fastmode) {
00107 
00108   wait_ms(5);
00109   /*
00110   // disable internal pullups of the ATMEGA which Wire enable by default
00111   #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
00112     // deactivate internal pull-ups for twi
00113     // as per note from atmega8 manual pg167
00114     cbi(PORTC, 4);
00115     cbi(PORTC, 5);
00116   #else
00117     // deactivate internal pull-ups for twi
00118     // as per note from atmega128 manual pg204
00119     cbi(PORTD, 0);
00120     cbi(PORTD, 1);
00121   #endif
00122   */
00123   
00124   /*
00125   if(fastmode) { // switch to 400KHz I2C - eheheh
00126     TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c
00127   }
00128 */
00129   //accgyro = MPU6050(false, accgyro_addr);
00130   accgyro = MPU6050(0x69);
00131   accgyro.initialize();
00132   accgyro.setI2CMasterModeEnabled(0);
00133   accgyro.setI2CBypassEnabled(1);
00134   accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00135   wait_ms(5);
00136 
00137   
00138   // zero gyro
00139   zeroGyro();
00140   
00141   #ifndef CALIBRATION_H
00142   // load calibration from eeprom
00143   calLoad();
00144   #endif
00145 }
00146 /*
00147 #ifndef CALIBRATION_H
00148 
00149 static uint8_t location; // assuming ordered reads
00150 
00151 void eeprom_read_var(uint8_t size, byte * var) {
00152   for(uint8_t i = 0; i<size; i++) {
00153     var[i] = EEPROM.read(location + i);
00154   }
00155   location += size;
00156 }
00157 */
00158 void FreeIMU::calLoad() {
00159 /*
00160   if(EEPROM.read(FREEIMU_EEPROM_BASE) == FREEIMU_EEPROM_SIGNATURE) { // check if signature is ok so we have good data
00161     location = FREEIMU_EEPROM_BASE + 1; // reset location
00162     
00163     eeprom_read_var(sizeof(acc_off_x), (byte *) &acc_off_x);
00164     eeprom_read_var(sizeof(acc_off_y), (byte *) &acc_off_y);
00165     eeprom_read_var(sizeof(acc_off_z), (byte *) &acc_off_z);
00166     
00167     eeprom_read_var(sizeof(magn_off_x), (byte *) &magn_off_x);
00168     eeprom_read_var(sizeof(magn_off_y), (byte *) &magn_off_y);
00169     eeprom_read_var(sizeof(magn_off_z), (byte *) &magn_off_z);
00170     
00171     eeprom_read_var(sizeof(acc_scale_x), (byte *) &acc_scale_x);
00172     eeprom_read_var(sizeof(acc_scale_y), (byte *) &acc_scale_y);
00173     eeprom_read_var(sizeof(acc_scale_z), (byte *) &acc_scale_z);
00174     
00175     eeprom_read_var(sizeof(magn_scale_x), (byte *) &magn_scale_x);
00176     eeprom_read_var(sizeof(magn_scale_y), (byte *) &magn_scale_y);
00177     eeprom_read_var(sizeof(magn_scale_z), (byte *) &magn_scale_z);
00178   }
00179   else {
00180   */
00181     acc_off_x = 0;
00182     acc_off_y = 0;
00183     acc_off_z = 0;
00184     acc_scale_x = 1;
00185     acc_scale_y = 1;
00186     acc_scale_z = 1;
00187 
00188     magn_off_x = 0;
00189     magn_off_y = 0;
00190     magn_off_z = 0;
00191     magn_scale_x = 1;
00192     magn_scale_y = 1;
00193     magn_scale_z = 1;
00194  // }
00195 }
00196 //#endif
00197 
00198 /**
00199  * Populates raw_values with the raw_values from the sensors
00200 */
00201 void FreeIMU::getRawValues(int16_t * raw_values) {
00202 
00203     accgyro.getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
00204 
00205 }
00206 
00207 
00208 /**
00209  * Populates values with calibrated readings from the sensors
00210 */
00211 void FreeIMU::getValues(float * values) {  
00212 
00213 // MPU6050
00214     int16_t accgyroval[6];
00215     accgyro.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
00216     
00217     // remove offsets from the gyroscope
00218     accgyroval[3] = accgyroval[3] - gyro_off_x;
00219     accgyroval[4] = accgyroval[4] - gyro_off_y;
00220     accgyroval[5] = accgyroval[5] - gyro_off_z;
00221 
00222     for(int i = 0; i<6; i++) {
00223       if(i < 3) {
00224         values[i] = (float) accgyroval[i];
00225       }
00226       else {
00227         values[i] = ((float) accgyroval[i]) / 16.4f; // NOTE: this depends on the sensitivity chosen
00228       }
00229     }
00230 
00231   
00232   
00233   #warning Accelerometer calibration active: have you calibrated your device?
00234   // remove offsets and scale accelerometer (calibration)
00235   values[0] = (values[0] - acc_off_x) / acc_scale_x;
00236   values[1] = (values[1] - acc_off_y) / acc_scale_y;
00237   values[2] = (values[2] - acc_off_z) / acc_scale_z;
00238   
00239   
00240  
00241 }
00242 
00243 
00244 /**
00245  * Computes gyro offsets
00246 */
00247 void FreeIMU::zeroGyro() {
00248   const int totSamples = 3;
00249   int16_t raw[11];
00250   float tmpOffsets[] = {0,0,0};
00251   
00252   for (int i = 0; i < totSamples; i++){
00253     getRawValues(raw);
00254     tmpOffsets[0] += raw[3];
00255     tmpOffsets[1] += raw[4];
00256     tmpOffsets[2] += raw[5];
00257   }
00258   
00259   gyro_off_x = tmpOffsets[0] / totSamples;
00260   gyro_off_y = tmpOffsets[1] / totSamples;
00261   gyro_off_z = tmpOffsets[2] / totSamples;
00262 }
00263 
00264 
00265 /**
00266  * Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
00267  * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
00268  * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
00269  * axis only.
00270  * 
00271  * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
00272 */
00273 
00274 void  FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) {
00275 
00276   float recipNorm;
00277   float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
00278   float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
00279   float qa, qb, qc;
00280 
00281   // Auxiliary variables to avoid repeated arithmetic
00282   q0q0 = q0 * q0;
00283   q0q1 = q0 * q1;
00284   q0q2 = q0 * q2;
00285   q0q3 = q0 * q3;
00286   q1q1 = q1 * q1;
00287   q1q2 = q1 * q2;
00288   q1q3 = q1 * q3;
00289   q2q2 = q2 * q2;
00290   q2q3 = q2 * q3;
00291   q3q3 = q3 * q3;
00292   
00293   
00294 
00295   // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00296   if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
00297     float halfvx, halfvy, halfvz;
00298     
00299     // Normalise accelerometer measurement
00300     recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00301     ax *= recipNorm;
00302     ay *= recipNorm;
00303     az *= recipNorm;
00304     
00305     // Estimated direction of gravity
00306     halfvx = q1q3 - q0q2;
00307     halfvy = q0q1 + q2q3;
00308     halfvz = q0q0 - 0.5f + q3q3;
00309   
00310     // Error is sum of cross product between estimated direction and measured direction of field vectors
00311     halfex += (ay * halfvz - az * halfvy);
00312     halfey += (az * halfvx - ax * halfvz);
00313     halfez += (ax * halfvy - ay * halfvx);
00314   }
00315 
00316   // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
00317   if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
00318     // Compute and apply integral feedback if enabled
00319     if(twoKi > 0.0f) {
00320       integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
00321       integralFBy += twoKi * halfey * (1.0f / sampleFreq);
00322       integralFBz += twoKi * halfez * (1.0f / sampleFreq);
00323       gx += integralFBx;  // apply integral feedback
00324       gy += integralFBy;
00325       gz += integralFBz;
00326     }
00327     else {
00328       integralFBx = 0.0f; // prevent integral windup
00329       integralFBy = 0.0f;
00330       integralFBz = 0.0f;
00331     }
00332 
00333     // Apply proportional feedback
00334     gx += twoKp * halfex;
00335     gy += twoKp * halfey;
00336     gz += twoKp * halfez;
00337   }
00338   
00339   // Integrate rate of change of quaternion
00340   gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
00341   gy *= (0.5f * (1.0f / sampleFreq));
00342   gz *= (0.5f * (1.0f / sampleFreq));
00343   qa = q0;
00344   qb = q1;
00345   qc = q2;
00346   q0 += (-qb * gx - qc * gy - q3 * gz);
00347   q1 += (qa * gx + qc * gz - q3 * gy);
00348   q2 += (qa * gy - qb * gz + q3 * gx);
00349   q3 += (qa * gz + qb * gy - qc * gx);
00350   
00351   // Normalise quaternion
00352   recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00353   q0 *= recipNorm;
00354   q1 *= recipNorm;
00355   q2 *= recipNorm;
00356   q3 *= recipNorm;
00357 }
00358 
00359 
00360 /**
00361  * Populates array q with a quaternion representing the IMU orientation with respect to the Earth
00362  * 
00363  * @param q the quaternion to populate
00364 */
00365 void FreeIMU::getQ(float * q) {
00366   float val[9];
00367   getValues(val);
00368   
00369   DEBUG_PRINT(val[3] * M_PI/180);
00370   DEBUG_PRINT(val[4] * M_PI/180);
00371   DEBUG_PRINT(val[5] * M_PI/180);
00372   DEBUG_PRINT(val[0]);
00373   DEBUG_PRINT(val[1]);
00374   DEBUG_PRINT(val[2]);
00375   DEBUG_PRINT(val[6]);
00376   DEBUG_PRINT(val[7]);
00377   DEBUG_PRINT(val[8]);
00378   
00379   //now = micros();
00380   dt_us=update.read_us();
00381   sampleFreq = 1.0 / ((dt_us) / 1000000.0);
00382   update.reset();
00383  // lastUpdate = now;
00384   // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
00385 
00386     AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);
00387 
00388   
00389   q[0] = q0;
00390   q[1] = q1;
00391   q[2] = q2;
00392   q[3] = q3;
00393 }
00394 
00395 
00396 
00397 /**
00398  * Returns the Euler angles in radians defined in the Aerospace sequence.
00399  * See Sebastian O.H. Madwick report "An efficient orientation filter for 
00400  * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
00401  * 
00402  * @param angles three floats array which will be populated by the Euler angles in radians
00403 */
00404 void FreeIMU::getEulerRad(float * angles) {
00405   float q[4]; // quaternion
00406   getQ(q);
00407   angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
00408   angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
00409   angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
00410 }
00411 
00412 
00413 /**
00414  * Returns the Euler angles in degrees defined with the Aerospace sequence.
00415  * See Sebastian O.H. Madwick report "An efficient orientation filter for 
00416  * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
00417  * 
00418  * @param angles three floats array which will be populated by the Euler angles in degrees
00419 */
00420 void FreeIMU::getEuler(float * angles) {
00421   getEulerRad(angles);
00422   arr3_rad_to_deg(angles);
00423 }
00424 
00425 
00426 /**
00427  * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
00428  * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
00429  * and the Earth ground plane and the IMU Y axis.
00430  * 
00431  * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
00432  * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
00433  * 
00434  * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians
00435 */
00436 void FreeIMU::getYawPitchRollRad(float * ypr) {
00437   float q[4]; // quaternion
00438   float gx, gy, gz; // estimated gravity direction
00439   getQ(q);
00440   
00441   gx = 2 * (q[1]*q[3] - q[0]*q[2]);
00442   gy = 2 * (q[0]*q[1] + q[2]*q[3]);
00443   gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
00444   
00445   ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
00446   ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
00447   ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
00448 }
00449 
00450 
00451 /**
00452  * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between
00453  * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
00454  * and the Earth ground plane and the IMU Y axis.
00455  * 
00456  * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
00457  * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
00458  * 
00459  * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees
00460 */
00461 void FreeIMU::getYawPitchRoll(float * ypr) {
00462   getYawPitchRollRad(ypr);
00463   arr3_rad_to_deg(ypr);
00464 }
00465 
00466 
00467 /**
00468  * Converts a 3 elements array arr of angles expressed in radians into degrees
00469 */
00470 void arr3_rad_to_deg(float * arr) {
00471   arr[0] *= 180/M_PI;
00472   arr[1] *= 180/M_PI;
00473   arr[2] *= 180/M_PI;
00474 }
00475 
00476 
00477 /**
00478  * Fast inverse square root implementation
00479  * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root
00480 */
00481 float invSqrt(float number) {
00482   volatile long i;
00483   volatile float x, y;
00484   volatile const float f = 1.5F;
00485 
00486   x = number * 0.5F;
00487   y = number;
00488   i = * ( long * ) &y;
00489   i = 0x5f375a86 - ( i >> 1 );
00490   y = * ( float * ) &i;
00491   y = y * ( f - ( x * y * y ) );
00492   return y;
00493 }
00494 
00495 
00496 
00497