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 "MODI2C.h"
00029 #include "FreeIMU.h"
00030 #include "rtos.h"
00031 
00032 #define     M_PI 3.1415926535897932384626433832795
00033 
00034 FreeIMU::FreeIMU()
00035 {
00036 
00037     // initialize quaternion
00038     q0 = 1.0f;
00039     q1 = 0.0f;
00040     q2 = 0.0f;
00041     q3 = 0.0f;
00042     exInt = 0.0;
00043     eyInt = 0.0;
00044     ezInt = 0.0;
00045     twoKp = twoKpDef;
00046     twoKi = twoKiDef;
00047     
00048     twoKiz = twoKiDef / 4.0;
00049     twoKpz = twoKpDef * 6.0;
00050     
00051     integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;
00052 
00053     update.start();
00054     dt_us=0;
00055     /*
00056     lastUpdate = 0;
00057     now = 0;
00058     */
00059 #ifndef CALIBRATION_H
00060     // initialize scale factors to neutral values
00061     acc_scale_x = 1;
00062     acc_scale_y = 1;
00063     acc_scale_z = 1;
00064     magn_scale_x = 1;
00065     magn_scale_y = 1;
00066     magn_scale_z = 1;
00067 #else
00068     // get values from global variables of same name defined in calibration.h
00069     acc_off_x = ::acc_off_x;
00070     acc_off_y = ::acc_off_y;
00071     acc_off_z = ::acc_off_z;
00072     acc_scale_x = ::acc_scale_x;
00073     acc_scale_y = ::acc_scale_y;
00074     acc_scale_z = ::acc_scale_z;
00075     magn_off_x = ::magn_off_x;
00076     magn_off_y = ::magn_off_y;
00077     magn_off_z = ::magn_off_z;
00078     magn_scale_x = ::magn_scale_x;
00079     magn_scale_y = ::magn_scale_y;
00080     magn_scale_z = ::magn_scale_z;
00081 #endif
00082 }
00083 
00084 void FreeIMU::init()
00085 {
00086 
00087     init(FIMU_ACCGYRO_ADDR, false);
00088 
00089 }
00090 
00091 void FreeIMU::init(bool fastmode)
00092 {
00093 
00094     init(FIMU_ACCGYRO_ADDR, fastmode);
00095 
00096 }
00097 
00098 void FreeIMU::sample(bool sampling)
00099 {
00100     accgyro->sample(sampling);
00101 }
00102 
00103 /**
00104  * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
00105 */
00106 
00107 void FreeIMU::init(int accgyro_addr, bool fastmode)
00108 {
00109     accgyro = new MPU6050();
00110     Thread::wait(10);
00111     baro = new MS561101BA();
00112     magn = new HMC58X3();
00113 
00114     Thread::wait(10);
00115 
00116     accgyro->initialize();
00117     accgyro->setI2CMasterModeEnabled(0);
00118     accgyro->setI2CBypassEnabled(0);
00119     accgyro->setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
00120     accgyro->setDLPFMode(0);
00121     accgyro->setRate(0);
00122     Thread::wait(20);
00123 
00124     accgyro->start_sampling();
00125 
00126     Thread::wait(10);
00127 
00128     // init HMC5843
00129     magn->init(false); // Don't set mode yet, we'll do that later on.
00130     magn->setGain(1);
00131     // Calibrate HMC using self test, not recommended to change the gain after calibration.
00132     magn->calibrate(1, 8); // Use gain 1=default, valid 0-7, 7 not recommended.
00133     Thread::wait(30);
00134     magn->setDOR(6);
00135     Thread::wait(30);
00136     magn->start_sampling();
00137     Thread::wait(30);
00138     baro->init(FIMU_BARO_ADDR);
00139 
00140     // zero gyro
00141     zeroGyro();
00142 
00143 #ifndef CALIBRATION_H
00144     // load calibration from eeprom
00145     calLoad();
00146 #endif
00147 
00148     Thread::wait(30);
00149 
00150     getQ_simple(NULL);
00151 
00152     baro->start_sampling(MS561101BA_OSR_4096);
00153 }
00154 
00155 void FreeIMU::getQ_simple(float* q)
00156 {
00157     float values[9];
00158     getValues(values);
00159 
00160     float pitch = atan2(values[0], sqrt(values[1]*values[1]+values[2]*values[2]));
00161     float roll = -atan2(values[1], sqrt(values[0]*values[0]+values[2]*values[2]));
00162 
00163     float xh = values[6]*cos(pitch)+values[7]*sin(roll)*sin(pitch)-values[8]*cos(roll)*sin(pitch);
00164     float yh = values[7]*cos(roll)+values[8]*sin(roll);
00165     float yaw = -atan2(yh, xh);
00166 
00167     float rollOver2 = (roll + M_PI) * 0.5f;
00168     float sinRollOver2 = (float)sin(rollOver2);
00169     float cosRollOver2 = (float)cos(rollOver2);
00170     float pitchOver2 = pitch * 0.5f;
00171     float sinPitchOver2 = (float)sin(pitchOver2);
00172     float cosPitchOver2 = (float)cos(pitchOver2);
00173     float yawOver2 = yaw * 0.5f;
00174     float sinYawOver2 = (float)sin(yawOver2);
00175     float cosYawOver2 = (float)cos(yawOver2);
00176 
00177     q0 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
00178     q1 = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
00179     q2 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
00180     q3 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
00181 
00182     if (q!=NULL) {
00183         q[0] = q0;
00184         q[1] = q1;
00185         q[2] = q2;
00186         q[3] = q3;
00187     }
00188 }
00189 
00190 /**
00191  * Populates raw_values with the raw_values from the sensors
00192 */
00193 void FreeIMU::getRawValues(int16_t * raw_values)
00194 {
00195 
00196     accgyro->getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
00197     magn->getValues(&raw_values[6], &raw_values[7], &raw_values[8]);
00198 
00199     int temp, press;
00200     //TODO: possible loss of precision
00201     temp = baro->rawTemperature();
00202     raw_values[9] = temp;
00203     press = baro->rawPressure();
00204     raw_values[10] = press;
00205 }
00206 
00207 
00208 /**
00209  * Populates values with calibrated readings from the sensors
00210 */
00211 void FreeIMU::getValues(float * values)
00212 {
00213 
00214 // MPU6050
00215     int16_t accgyroval[6];
00216     accgyro->getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
00217 
00218     // remove offsets from the gyroscope
00219     accgyroval[3] = accgyroval[3] - gyro_off_x;
00220     accgyroval[4] = accgyroval[4] - gyro_off_y;
00221     accgyroval[5] = accgyroval[5] - gyro_off_z;
00222 
00223     for(int i = 0; i<6; i++) {
00224         if(i < 3) {
00225             values[i] = (float) accgyroval[i];
00226         } else {
00227             values[i] = ((float) accgyroval[i]) / 32.8f; // 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     magn->getValues(&values[6]);
00240     // calibration
00241 #warning Magnetometer calibration active: have you calibrated your device?
00242     values[6] = (values[6] - magn_off_x) / magn_scale_x;
00243     values[7] = (values[7] - magn_off_y) / magn_scale_y;
00244     values[8] = (values[8] - magn_off_z) / magn_scale_z;
00245 
00246 }
00247 
00248 
00249 /**
00250  * Computes gyro offsets
00251 */
00252 void FreeIMU::zeroGyro()
00253 {
00254     const int totSamples = 64;
00255     int16_t raw[11];
00256     float tmpOffsets[] = {0,0,0};
00257 
00258     for (int i = 0; i < totSamples; i++) {
00259         getRawValues(raw);
00260         tmpOffsets[0] += raw[3];
00261         tmpOffsets[1] += raw[4];
00262         tmpOffsets[2] += raw[5];
00263         Thread::wait(3);
00264     }
00265 
00266     gyro_off_x = tmpOffsets[0] / totSamples;
00267     gyro_off_y = tmpOffsets[1] / totSamples;
00268     gyro_off_z = tmpOffsets[2] / totSamples;
00269 }
00270 
00271 extern volatile bool magn_valid;
00272 
00273 /**
00274  * Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
00275  * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
00276  * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
00277  * axis only.
00278  *
00279  * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
00280 */
00281 
00282 void  FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, bool _magn_valid)
00283 {
00284 
00285     float recipNorm;
00286     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
00287     float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
00288     float qa, qb, qc;
00289 
00290     // Auxiliary variables to avoid repeated arithmetic
00291     q0q0 = q0 * q0;
00292     q0q1 = q0 * q1;
00293     q0q2 = q0 * q2;
00294     q0q3 = q0 * q3;
00295     q1q1 = q1 * q1;
00296     q1q2 = q1 * q2;
00297     q1q3 = q1 * q3;
00298     q2q2 = q2 * q2;
00299     q2q3 = q2 * q3;
00300     q3q3 = q3 * q3;
00301 
00302     // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
00303     if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f) && _magn_valid) {
00304         float hx, hy, bx, bz;
00305         float halfwx, halfwy, halfwz;
00306 
00307         // Normalise magnetometer measurement
00308         recipNorm = invSqrt(mx * mx + my * my + mz * mz);
00309         mx *= recipNorm;
00310         my *= recipNorm;
00311         mz *= recipNorm;
00312 
00313         // Reference direction of Earth's magnetic field
00314         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
00315         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
00316         bx = sqrt(hx * hx + hy * hy);
00317         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
00318 
00319         // Estimated direction of magnetic field
00320         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
00321         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
00322         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
00323 
00324         // Error is sum of cross product between estimated direction and measured direction of field vectors
00325         halfex = (my * halfwz - mz * halfwy);
00326         halfey = (mz * halfwx - mx * halfwz);
00327         halfez = (mx * halfwy - my * halfwx);
00328 
00329         magn_valid = false;
00330     }
00331 
00332     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00333     if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
00334         float halfvx, halfvy, halfvz;
00335 
00336         // Normalise accelerometer measurement
00337         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00338         ax *= recipNorm;
00339         ay *= recipNorm;
00340         az *= recipNorm;
00341 
00342         // Estimated direction of gravity
00343         halfvx = q1q3 - q0q2;
00344         halfvy = q0q1 + q2q3;
00345         halfvz = q0q0 - 0.5f + q3q3;
00346 
00347         // Error is sum of cross product between estimated direction and measured direction of field vectors
00348         halfex += (ay * halfvz - az * halfvy);
00349         halfey += (az * halfvx - ax * halfvz);
00350         halfez += (ax * halfvy - ay * halfvx);
00351     }
00352 
00353     // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
00354     if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
00355         // Compute and apply integral feedback if enabled
00356         if(twoKi > 0.0f) {
00357             integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
00358             integralFBy += twoKi * halfey * (1.0f / sampleFreq);
00359             integralFBz += twoKiz * halfez * (1.0f / sampleFreq);
00360             gx += integralFBx;  // apply integral feedback
00361             gy += integralFBy;
00362             gz += integralFBz;
00363         } else {
00364             integralFBx = 0.0f; // prevent integral windup
00365             integralFBy = 0.0f;
00366             integralFBz = 0.0f;
00367         }
00368 
00369         // Apply proportional feedback
00370         gx += twoKp * halfex;
00371         gy += twoKp * halfey;
00372         gz += twoKpz * halfez;
00373     }
00374 
00375     // Integrate rate of change of quaternion
00376     gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
00377     gy *= (0.5f * (1.0f / sampleFreq));
00378     gz *= (0.5f * (1.0f / sampleFreq));
00379     qa = q0;
00380     qb = q1;
00381     qc = q2;
00382     q0 += (-qb * gx - qc * gy - q3 * gz);
00383     q1 += (qa * gx + qc * gz - q3 * gy);
00384     q2 += (qa * gy - qb * gz + q3 * gx);
00385     q3 += (qa * gz + qb * gy - qc * gx);
00386 
00387     // Normalise quaternion
00388     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00389     q0 *= recipNorm;
00390     q1 *= recipNorm;
00391     q2 *= recipNorm;
00392     q3 *= recipNorm;
00393 }
00394 
00395 
00396 /**
00397  * Populates array q with a quaternion representing the IMU orientation with respect to the Earth
00398  *
00399  * @param q the quaternion to populate
00400 */
00401 
00402 float val[9];
00403 void FreeIMU::getQ(float * q)
00404 {
00405     getValues(val);
00406 
00407     dt_us=update.read_us();
00408     sampleFreq = 1.0 / ((dt_us) / 1000000.0);
00409     update.reset();
00410 // lastUpdate = now;
00411     // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
00412 
00413     AHRSupdate(val[3] * M_PI/180.0, val[4] * M_PI/180.0, val[5] * M_PI/180.0, val[0], val[1], val[2], val[6], -val[7], -val[8], magn_valid);
00414 
00415     if (q!=NULL) {
00416         q[0] = q0;
00417         q[1] = q1;
00418         q[2] = q2;
00419         q[3] = q3;
00420     }
00421 }
00422 
00423 void FreeIMU::getRate(float * r)
00424 {    
00425     r[0] = val[3];
00426     r[1] = val[4];
00427     r[2] = val[5];
00428 }
00429 
00430 const float def_sea_press = 1013.25;
00431 
00432 /**
00433  * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure
00434 */
00435 float FreeIMU::getBaroAlt(float sea_press)
00436 {
00437     float temp = baro->getTemperature();
00438     float press = baro->getPressure();
00439     return ((pow((float)(sea_press / press), 1.0f/5.257f) - 1.0f) * (temp + 273.15f)) / 0.0065f;
00440 }
00441 
00442 /**
00443  * Returns an altitude estimate from baromether readings only using a default sea level pressure
00444 */
00445 float FreeIMU::getBaroAlt()
00446 {
00447     return getBaroAlt(def_sea_press);
00448 }
00449 
00450 float FreeIMU::getRawPressure()
00451 {
00452     return baro->getPressure();
00453 }
00454 
00455 
00456 /**
00457  * Compensates the accelerometer readings in the 3D vector acc expressed in the sensor frame for gravity
00458  * @param acc the accelerometer readings to compensate for gravity
00459  * @param q the quaternion orientation of the sensor board with respect to the world
00460 */
00461 void FreeIMU::gravityCompensateAcc(float * acc, float * q)
00462 {
00463     float g[3];
00464 
00465     // get expected direction of gravity in the sensor frame
00466     g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
00467     g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
00468     g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
00469 
00470     // compensate accelerometer readings with the expected direction of gravity
00471     acc[0] = acc[0] - g[0];
00472     acc[1] = acc[1] - g[1];
00473     acc[2] = acc[2] - g[2];
00474 }
00475 
00476 
00477 /**
00478  * Returns the Euler angles in radians defined in the Aerospace sequence.
00479  * See Sebastian O.H. Madwick report "An efficient orientation filter for
00480  * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
00481  *
00482  * @param angles three floats array which will be populated by the Euler angles in radians
00483 */
00484 void FreeIMU::getEulerRad(float * angles)
00485 {
00486     float q[4]; // quaternion
00487     getQ(q);
00488     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
00489     angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
00490     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
00491 }
00492 
00493 
00494 /**
00495  * Returns the Euler angles in degrees defined with the Aerospace sequence.
00496  * See Sebastian O.H. Madwick report "An efficient orientation filter for
00497  * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
00498  *
00499  * @param angles three floats array which will be populated by the Euler angles in degrees
00500 */
00501 void FreeIMU::getEuler(float * angles)
00502 {
00503     getEulerRad(angles);
00504     arr3_rad_to_deg(angles);
00505 }
00506 
00507 
00508 /**
00509  * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
00510  * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
00511  * and the Earth ground plane and the IMU Y axis.
00512  *
00513  * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
00514  * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
00515  *
00516  * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians
00517 */
00518 void FreeIMU::getYawPitchRollRad(float * ypr)
00519 {
00520     float q[4]; // quaternion
00521     float gx, gy, gz; // estimated gravity direction
00522     getQ(q);
00523 
00524     gx = 2 * (q[1]*q[3] - q[0]*q[2]);
00525     gy = 2 * (q[0]*q[1] + q[2]*q[3]);
00526     gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
00527 
00528     ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
00529     ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
00530     ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
00531 }
00532 
00533 
00534 /**
00535  * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between
00536  * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
00537  * and the Earth ground plane and the IMU Y axis.
00538  *
00539  * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
00540  * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
00541  *
00542  * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees
00543 */
00544 void FreeIMU::getYawPitchRoll(float * ypr)
00545 {
00546     getYawPitchRollRad(ypr);
00547     arr3_rad_to_deg(ypr);
00548 }
00549 
00550 
00551 /**
00552  * Converts a 3 elements array arr of angles expressed in radians into degrees
00553 */
00554 void arr3_rad_to_deg(float * arr)
00555 {
00556     arr[0] *= 180/M_PI;
00557     arr[1] *= 180/M_PI;
00558     arr[2] *= 180/M_PI;
00559 }
00560 
00561 
00562 /**
00563  * Fast inverse square root implementation
00564  * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root
00565 */
00566 float invSqrt(float number)
00567 {
00568     volatile long i;
00569     volatile float x, y;
00570     volatile const float f = 1.5F;
00571 
00572     x = number * 0.5F;
00573     y = number;
00574     i = * ( long * ) &y;
00575     i = 0x5f375a86 - ( i >> 1 );
00576     y = * ( float * ) &i;
00577     y = y * ( f - ( x * y * y ) );
00578     return y;
00579 }
00580 
00581