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