IMU10DOF.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 ported for HobbyKing's 10DOF stick on a mbed platform by Aloïs Wolff(wolffalois@gmail.com)
00005 
00006 Development of this code has been supported by the Department of Computer Science,
00007 Universita' degli Studi di Torino, Italy within the Piemonte Project
00008 http://www.piemonte.di.unito.it/
00009 
00010 
00011 This program is free software: you can redistribute it and/or modify
00012 it under the terms of the version 3 GNU General Public License as
00013 published by the Free Software Foundation.
00014 
00015 This program is distributed in the hope that it will be useful,
00016 but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018 GNU General Public License for more details.
00019 
00020 You should have received a copy of the GNU General Public License
00021 along with this program.  If not, see <http://www.gnu.org/licenses/>.
00022 
00023  Now ported to support DFRobot.com 10DOF MEMS IMU
00024  by Larry Littlefield svkatielee@yahoo.com
00025  
00026 */
00027 
00028 
00029 
00030 //#define DEBUG
00031 
00032 
00033 #include "IMU10DOF.h"
00034 #include <math.h> 
00035 #include "helper_3dmath.h"
00036  
00037 
00038 // #include "WireUtils.h"
00039 //#include "DebugUtils.h"
00040 //#include "vector_math.h"
00041 
00042 #ifndef M_PI
00043 #define     M_PI 3.1415926535897932384626433832795
00044 #endif
00045 
00046 IMU10DOF::IMU10DOF(PinName sda, PinName scl) : acc(sda,scl), magn(sda,scl),gyro(sda,scl), baro(sda,scl),pc(USBTX,USBRX)
00047 {
00048 
00049     pc.baud(115200);
00050     
00051     // initialize quaternion
00052     q0 = 1.0f;
00053     q1 = 0.0f;
00054     q2 = 0.0f;
00055     q3 = 0.0f;
00056     exInt = 0.0;
00057     eyInt = 0.0;
00058     ezInt = 0.0;
00059     twoKp = twoKpDef;
00060     twoKi = twoKiDef;
00061     integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;
00062 
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 
00090 
00091 
00092 /**
00093  * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
00094 */
00095 
00096 void IMU10DOF::init(bool fastmode)
00097 {
00098 
00099 
00100          //Sensors Initialization
00101      pc.printf("Initializing sensors...\n\r");
00102      magn.init();
00103      baro.getCalibrationData();
00104      acc.setPowerControl(0x00);
00105      wait_ms(10);
00106      acc.setDataFormatControl(0x0B);
00107      wait_ms(10);
00108      acc.setPowerControl(MeasurementMode);
00109      wait_ms(10);
00110      gyro.init();
00111     pc.printf("Sensors OK!\n\r");
00112 
00113     update.start();
00114     int dt_us=0;
00115     pc.printf("ZeroGyro\n\r");
00116     // zero gyro
00117     //zeroGyro();
00118     gyro.zeroCalibrate(128,5);
00119     pc.printf("ZeroGyro OK, CalLoad...\n\r");
00120     // load calibration from eeprom
00121     calLoad();
00122 
00123 }
00124 
00125 void IMU10DOF::calLoad()
00126 {
00127 
00128     
00129     acc_off_x = 0;
00130     acc_off_y = 0;
00131     acc_off_z = 0;
00132     acc_scale_x = 1;
00133     acc_scale_y = 1;
00134     acc_scale_z = 1;
00135 
00136     magn_off_x = 0.21;
00137     magn_off_y = -0.175;
00138     magn_off_z = 0;
00139     magn_scale_x = 1;
00140     magn_scale_y = 1;
00141     magn_scale_z = 1;
00142 }
00143 
00144 void IMU10DOF::getRawValues(int16_t * raw_values)
00145 {
00146     int16_t raw3[3];
00147     int raw[3];
00148     
00149     pc.printf("GetRaw IN \n\r");
00150 
00151     acc.getOutput(&raw_values[0], &raw_values[1], &raw_values[2]);
00152     pc.printf("GotACC\n\r");
00153     
00154     gyro.read(raw);
00155     
00156     raw_values[3]=(int16_t)raw[0];
00157     raw_values[4]=(int16_t)raw[1];
00158     raw_values[5]=(int16_t)raw[2];
00159     pc.printf("GotGYRO\n\r");
00160     
00161 
00162     magn.getXYZ(raw3);
00163     raw_values[6]=raw3[0];
00164     raw_values[7]=raw3[1];
00165     raw_values[8]=raw3[2];
00166     pc.printf("GotMAG\n\r");
00167     
00168 
00169 
00170     baro.readSensor();
00171     raw_values[9]=(int16_t)baro.temp();
00172     raw_values[10]=(int16_t)baro.press();
00173     pc.printf("GotBARO\n\r");
00174     
00175 
00176 }
00177 
00178 
00179 /**
00180  * Populates values with calibrated readings from the sensors
00181 */
00182 void IMU10DOF::getValues(float * values)
00183 {
00184 
00185     int16_t accval[3];
00186     float     gyrval[3];
00187     acc.getOutput(accval);
00188     values[0] = (float) accval[0];
00189     values[1] = (float) accval[1];
00190     values[2] = (float) accval[2];
00191     
00192     gyro.readFin(gyrval);
00193     values[3] =  gyrval[0];
00194     values[4] =  gyrval[1];
00195     values[5] =  gyrval[2];
00196     
00197     magn.getXYZ(accval);
00198     values[6]=(float)accval[0];
00199     values[7]=(float)accval[1];
00200     values[8]=(float)accval[2];
00201 
00202 
00203 #warning Accelerometer calibration active: have you calibrated your device?
00204     // remove offsets and scale accelerometer (calibration)
00205     values[0] = (values[0] - acc_off_x) / acc_scale_x;
00206     values[1] = (values[1] - acc_off_y) / acc_scale_y;
00207     values[2] = (values[2] - acc_off_z) / acc_scale_z;
00208 
00209     // calibration
00210 #warning Magnetometer calibration active: have you calibrated your device?
00211     values[6] = (values[6] - magn_off_x) / magn_scale_x;
00212     values[7] = (values[7] - magn_off_y) / magn_scale_y;
00213     values[8] = (values[8] - magn_off_z) / magn_scale_z;
00214 
00215     
00216     
00217 }
00218 
00219 
00220 /**
00221  * Computes gyro offsets
00222 */
00223 void IMU10DOF::zeroGyro()
00224 {
00225     const int totSamples = 3;
00226     int16_t raw[11];
00227     float tmpOffsets[] = {0,0,0};
00228 
00229     for (int i = 0; i < totSamples; i++) {
00230         getRawValues(raw);
00231         tmpOffsets[0] += raw[3];
00232         tmpOffsets[1] += raw[4];
00233         tmpOffsets[2] += raw[5];
00234     }
00235 
00236     gyro_off_x = tmpOffsets[0] / totSamples;
00237     gyro_off_y = tmpOffsets[1] / totSamples;
00238     gyro_off_z = tmpOffsets[2] / totSamples;
00239 }
00240 
00241 
00242 /**
00243  * Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
00244  * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
00245  * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
00246  * axis only.
00247  *
00248  * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
00249 */
00250 //#if IS_9DOM()
00251 void  IMU10DOF::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
00252 {
00253 
00254     float recipNorm;
00255     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
00256     float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
00257     float qa, qb, qc;
00258 
00259     // Auxiliary variables to avoid repeated arithmetic
00260     q0q0 = q0 * q0;
00261     q0q1 = q0 * q1;
00262     q0q2 = q0 * q2;
00263     q0q3 = q0 * q3;
00264     q1q1 = q1 * q1;
00265     q1q2 = q1 * q2;
00266     q1q3 = q1 * q3;
00267     q2q2 = q2 * q2;
00268     q2q3 = q2 * q3;
00269     q3q3 = q3 * q3;
00270 
00271     // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
00272     if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
00273         float hx, hy, bx, bz;
00274         float halfwx, halfwy, halfwz;
00275 
00276         // Normalise magnetometer measurement
00277         recipNorm = invSqrt(mx * mx + my * my + mz * mz);
00278         mx *= recipNorm;
00279         my *= recipNorm;
00280         mz *= recipNorm;
00281 
00282         // Reference direction of Earth's magnetic field
00283         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
00284         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
00285         bx = sqrt(hx * hx + hy * hy);
00286         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
00287 
00288         // Estimated direction of magnetic field
00289         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
00290         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
00291         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
00292 
00293         // Error is sum of cross product between estimated direction and measured direction of field vectors
00294         halfex = (my * halfwz - mz * halfwy);
00295         halfey = (mz * halfwx - mx * halfwz);
00296         halfez = (mx * halfwy - my * halfwx);
00297     }
00298 
00299 
00300     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00301     if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
00302         float halfvx, halfvy, halfvz;
00303 
00304         // Normalise accelerometer measurement
00305         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00306         ax *= recipNorm;
00307         ay *= recipNorm;
00308         az *= recipNorm;
00309 
00310         // Estimated direction of gravity
00311         halfvx = q1q3 - q0q2;
00312         halfvy = q0q1 + q2q3;
00313         halfvz = q0q0 - 0.5f + q3q3;
00314 
00315         // Error is sum of cross product between estimated direction and measured direction of field vectors
00316         halfex += (ay * halfvz - az * halfvy);
00317         halfey += (az * halfvx - ax * halfvz);
00318         halfez += (ax * halfvy - ay * halfvx);
00319     }
00320 
00321     // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
00322     if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
00323         // Compute and apply integral feedback if enabled
00324         if(twoKi > 0.0f) {
00325             integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
00326             integralFBy += twoKi * halfey * (1.0f / sampleFreq);
00327             integralFBz += twoKi * halfez * (1.0f / sampleFreq);
00328             gx += integralFBx;  // apply integral feedback
00329             gy += integralFBy;
00330             gz += integralFBz;
00331         } else {
00332             integralFBx = 0.0f; // prevent integral windup
00333             integralFBy = 0.0f;
00334             integralFBz = 0.0f;
00335         }
00336 
00337         // Apply proportional feedback
00338         gx += twoKp * halfex;
00339         gy += twoKp * halfey;
00340         gz += twoKp * halfez;
00341     }
00342 
00343     // Integrate rate of change of quaternion
00344     gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
00345     gy *= (0.5f * (1.0f / sampleFreq));
00346     gz *= (0.5f * (1.0f / sampleFreq));
00347     qa = q0;
00348     qb = q1;
00349     qc = q2;
00350     q0 += (-qb * gx - qc * gy - q3 * gz);
00351     q1 += (qa * gx + qc * gz - q3 * gy);
00352     q2 += (qa * gy - qb * gz + q3 * gx);
00353     q3 += (qa * gz + qb * gy - qc * gx);
00354 
00355     // Normalise quaternion
00356     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00357     q0 *= recipNorm;
00358     q1 *= recipNorm;
00359     q2 *= recipNorm;
00360     q3 *= recipNorm;
00361 }
00362 
00363 
00364 /**
00365  * Populates array q with a quaternion representing the IMU orientation with respect to the Earth
00366  *
00367  * @param q the quaternion to populate
00368 */
00369 void IMU10DOF::getQ(float * q)
00370 {
00371     float val[9];
00372     getValues(val);
00373     /*
00374         DEBUG_PRINT(val[3] * M_PI/180);
00375         DEBUG_PRINT(val[4] * M_PI/180);
00376         DEBUG_PRINT(val[5] * M_PI/180);
00377         DEBUG_PRINT(val[0]);
00378         DEBUG_PRINT(val[1]);
00379         DEBUG_PRINT(val[2]);
00380         DEBUG_PRINT(val[6]);
00381         DEBUG_PRINT(val[7]);
00382         DEBUG_PRINT(val[8]);
00383     */
00384 
00385     dt_us=update.read_us();
00386     sampleFreq = 1.0 / ((dt_us) / 1000000.0);
00387     update.reset();
00388 
00389 
00390 
00391     // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
00392     AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], val[6], val[7], val[8]);
00393 
00394 
00395 
00396     q[0] = q0;
00397     q[1] = q1;
00398     q[2] = q2;
00399     q[3] = q3;
00400 }
00401 
00402 
00403 
00404 const float def_sea_press = 1013.25;
00405 
00406 /**
00407  * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure
00408 */
00409 float IMU10DOF::getBaroAlt(float sea_press)
00410 {
00411     //float temp,press,res;
00412     baro.readSensor();
00413     //temp=(float)baro.temp();
00414     //press=(float)baro.press();
00415     
00416     
00417 
00418     return baro.altitud();
00419     //return(temp,press);
00420 }
00421 
00422 /**
00423  * Returns an altitude estimate from baromether readings only using a default sea level pressure
00424 */
00425 float IMU10DOF::getBaroAlt()
00426 {
00427     return getBaroAlt(def_sea_press);
00428 }
00429 
00430 
00431 /**
00432  * Compensates the accelerometer readings in the 3D vector acc expressed in the sensor frame for gravity
00433  * @param acc the accelerometer readings to compensate for gravity
00434  * @param q the quaternion orientation of the sensor board with respect to the world
00435 */
00436 void IMU10DOF::gravityCompensateAcc(float * acc, float * q)
00437 {
00438     float g[3];
00439 
00440     // get expected direction of gravity in the sensor frame
00441     g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
00442     g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
00443     g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
00444 
00445     // compensate accelerometer readings with the expected direction of gravity
00446     acc[0] = acc[0] - g[0];
00447     acc[1] = acc[1] - g[1];
00448     acc[2] = acc[2] - g[2];
00449 }
00450 
00451 
00452 
00453 /**
00454  * Returns the Euler angles in radians defined in the Aerospace sequence.
00455  * See Sebastian O.H. Madwick report "An efficient orientation filter for
00456  * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
00457  *
00458  * @param angles three floats array which will be populated by the Euler angles in radians
00459 */
00460 void IMU10DOF::getEulerRad(float * angles)
00461 {
00462     float q[4]; // quaternion
00463     getQ(q);
00464     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
00465     angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
00466     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
00467 }
00468 
00469 
00470 /**
00471  * Returns the Euler angles in degrees defined with the Aerospace sequence.
00472  * See Sebastian O.H. Madwick report "An efficient orientation filter for
00473  * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
00474  *
00475  * @param angles three floats array which will be populated by the Euler angles in degrees
00476 */
00477 void IMU10DOF::getEuler(float * angles)
00478 {
00479     getEulerRad(angles);
00480     arr3_rad_to_deg(angles);
00481 }
00482 
00483 
00484 /**
00485  * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
00486  * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
00487  * and the Earth ground plane and the IMU Y axis.
00488  *
00489  * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
00490  * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see IMU10DOF::getEuler
00491  *
00492  * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians
00493 */
00494 void IMU10DOF::getYawPitchRollRad(float * ypr)
00495 {
00496     float q[4]; // quaternion
00497     float gx, gy, gz; // estimated gravity direction
00498     getQ(q);
00499 
00500     gx = 2 * (q[1]*q[3] - q[0]*q[2]);
00501     gy = 2 * (q[0]*q[1] + q[2]*q[3]);
00502     gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
00503 
00504     ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
00505     ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
00506     ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
00507 }
00508 
00509 
00510 /**
00511  * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between
00512  * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
00513  * and the Earth ground plane and the IMU Y axis.
00514  *
00515  * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
00516  * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see IMU10DOF::getEuler
00517  *
00518  * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees
00519 */
00520 void IMU10DOF::getYawPitchRoll(float * ypr)
00521 {
00522     getYawPitchRollRad(ypr);
00523     arr3_rad_to_deg(ypr);
00524 }
00525 
00526 
00527 /**
00528  * Converts a 3 elements array arr of angles expressed in radians into degrees
00529 */
00530 void arr3_rad_to_deg(float * arr)
00531 {
00532     arr[0] *= 180/M_PI;
00533     arr[1] *= 180/M_PI;
00534     arr[2] *= 180/M_PI;
00535 }
00536 
00537 
00538 /**
00539  * Fast inverse square root implementation
00540  * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root
00541 */
00542 float invSqrt(float number)
00543 {
00544     volatile long i;
00545     volatile float x, y;
00546     volatile const float f = 1.5F;
00547 
00548     x = number * 0.5F;
00549     y = number;
00550     i = * ( long * ) &y;
00551     i = 0x5f375a86 - ( i >> 1 );
00552     y = * ( float * ) &i;
00553     y = y * ( f - ( x * y * y ) );
00554     return y;
00555 }
00556 
00557 
00558