FreeIMU.h

00001 /*
00002 FreeIMU.h - A libre and easy to use orientation sensing library for Arduino
00003 Copyright (C) 2011 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 */
00023 
00024 #ifndef FreeIMU_h
00025 #define FreeIMU_h
00026 
00027 #include "mbed.h"
00028 
00029 #ifndef I2C_SDA
00030     #define I2C_SDA p9
00031     #define I2C_SCL p10
00032 #endif
00033 
00034 // Uncomment the appropriated version of FreeIMU you are using
00035 //#define FREEIMU_v01
00036 //#define FREEIMU_v02
00037 //#define FREEIMU_v03
00038 //#define FREEIMU_v035
00039 //#define FREEIMU_v035_MS
00040 //#define FREEIMU_v035_BMP
00041 #define FREEIMU_v04
00042 
00043 // 3rd party boards. Please consider donating or buying a FreeIMU board to support this library development.
00044 //#define SEN_10121 //IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345 SEN-10121 http://www.sparkfun.com/products/10121
00045 //#define SEN_10736 //9 Degrees of Freedom - Razor IMU SEN-10736 http://www.sparkfun.com/products/10736
00046 //#define SEN_10724 //9 Degrees of Freedom - Sensor Stick SEN-10724 http://www.sparkfun.com/products/10724
00047 //#define SEN_10183 //9 Degrees of Freedom - Sensor Stick  SEN-10183 http://www.sparkfun.com/products/10183
00048 //#define ARDUIMU_v3 //  DIYDrones ArduIMU+ V3 http://store.diydrones.com/ArduIMU_V3_p/kt-arduimu-30.htm or https://www.sparkfun.com/products/11055
00049 //#define GEN_MPU6050 // Generic MPU6050 breakout board. Compatible with GY-521, SEN-11028 and other MPU6050 wich have the MPU6050 AD0 pin connected to GND.
00050 #define GEN_MPU9250
00051 
00052 // *** No configuration needed below this line ***
00053 
00054 
00055 #define FREEIMU_LIB_VERSION "20121122"
00056 
00057 #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
00058 
00059 #if F_CPU == 16000000L
00060   #define FREEIMU_FREQ "16 MHz"
00061 #elif F_CPU == 8000000L
00062   #define FREEIMU_FREQ "8 MHz"
00063 #endif
00064 
00065 
00066 // board IDs
00067 
00068 #if defined(FREEIMU_v04)
00069   #define FREEIMU_ID "FreeIMU v0.4"
00070 #endif
00071 
00072 
00073 #define HAS_MPU6050() (defined(GEN_MPU6050))
00074 #define HAS_MPU9250() (defined(GEN_MPU9250))
00075 
00076 #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
00077 #define HAS_AXIS_ALIGNED() (defined(FREEIMU_v01) || defined(FREEIMU_v02) || defined(FREEIMU_v03) || defined(FREEIMU_v035) || defined(FREEIMU_v035_MS) || defined(FREEIMU_v035_BMP) || defined(FREEIMU_v04) || defined(SEN_10121) || defined(SEN_10736))
00078 
00079 
00080 
00081 //#include <Wire.h>
00082 
00083 #include "mbed.h"
00084 #include "calibration.h"
00085 /*
00086 #ifndef CALIBRATION_H
00087 #include <EEPROM.h>
00088 #endif
00089 
00090 #define FREEIMU_EEPROM_BASE 0x0A
00091 #define FREEIMU_EEPROM_SIGNATURE 0x19
00092 */
00093 //#if FREEIMU_VER <= 3
00094 
00095 #if HAS_MPU6050()
00096  // #include <Wire.h>
00097   #include "I2Cdev.h"
00098   #include "MPU6050.h"
00099   #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS
00100   #include <HMC58X3.h>
00101 #endif
00102 
00103 #if HAS_MPU9250()
00104  // #include <Wire.h>
00105   #include "I2Cdev.h"
00106   #include "MPU6050.h"
00107   #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS
00108 #endif
00109 
00110 
00111 #include <MS561101BA.h>
00112 
00113 
00114 #define FIMU_BARO_ADDR MS561101BA_ADDR_CSB_LOW
00115 
00116 
00117 #define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW
00118 #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND
00119 // HMC5843 address is fixed so don't bother to define it
00120 
00121 #if defined(DFROBOT) 
00122     #define twoKpDef  (2.0f * 0.5f)
00123     #define twoKiDef  (2.0f * 0.00002f)
00124     #define betaDef  0.1f
00125     //Used for DCM filter
00126     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00127     const float Ki_ROLLPITCH = 0.0234f;
00128     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00129     const float Ki_YAW = 0.002f;
00130 #elif defined(FREEIMU_v04)
00131     #define twoKpDef  (2.0f * 0.75f)    //works with and without mag enabled
00132     #define twoKiDef  (2.0f * 0.1625f)
00133     #define betaDef  0.085f
00134     //Used for DCM filter
00135     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00136     const float Ki_ROLLPITCH = 0.0234f;
00137     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00138     const float Ki_YAW = 0.002f;
00139 #elif defined(GEN_MPU6050)
00140     #define twoKpDef  (2.0f * 0.5f)
00141     #define twoKiDef  (2.0f * 0.25f)
00142     #define betaDef   0.2f
00143     //Used for DCM filter
00144     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00145     const float Ki_ROLLPITCH = 0.0234f;
00146     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00147     const float Ki_YAW = 0.002f;
00148 #elif defined(GEN_MPU9150) 
00149     #define twoKpDef  (2.0f * 0.75f)
00150     #define twoKiDef  (2.0f * 0.1f) 
00151     #define betaDef   0.01f
00152     //Used for DCM filter
00153     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00154     const float Ki_ROLLPITCH = 0.0234f;
00155     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00156     const float Ki_YAW = 0.002f;
00157 #elif defined(Altimu10)
00158     //#define twoKpDef  (2.0f * 1.01f)
00159     //#define twoKiDef  (2.0f * 0.00002f)   
00160     #define twoKpDef  (2.0f * 2.75f)
00161     #define twoKiDef  (2.0f * 0.1625f)
00162     #define betaDef  2.0f
00163     //Used for DCM filter
00164     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00165     const float Ki_ROLLPITCH = 0.0234f;
00166     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00167     const float Ki_YAW = 0.002f;
00168 #elif defined(GEN_MPU9250) || defined(MPU9250_5611) || defined(MPU9250_5637)
00169     #define twoKpDef  (2.0f * 0.95f) // was 0.95
00170     #define twoKiDef  (2.0f * 0.05f) // was 0.05    
00171     #define betaDef   0.515f
00172     //Used for DCM filter
00173     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00174     const float Ki_ROLLPITCH = 0.0234f;
00175     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00176     const float Ki_YAW = 0.002f;
00177 #elif defined(APM_2_5)
00178     #define twoKpDef  (2.0f * 0.5f)
00179     #define twoKiDef  (2.0f * 0.25f)
00180     #define betaDef   0.015f    //was 0.015
00181     //Used for DCM filter
00182     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00183     const float Ki_ROLLPITCH = 0.0234f;
00184     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00185     const float Ki_YAW = 0.002f;
00186 #elif defined(Microduino)
00187     #define twoKpDef  (2.0f * 1.75f)    //works with and without mag enabled, 1.75
00188     #define twoKiDef  (2.0f * 0.0075f)  //.1625f
00189     #define betaDef  0.015f
00190     //Used for DCM filter
00191     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00192     const float Ki_ROLLPITCH = 0.0234f;
00193     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00194     const float Ki_YAW = 0.002f;
00195 #elif (defined(ST_LSM9DS0) || defined(LSM9DS0_MS5637))
00196     //Madgwick's implementation of Mayhony's AHRS algorithm (option 0)
00197     #define twoKpDef  (2.0f * 1.75f)    //works with and without mag enabled
00198     #define twoKiDef  (2.0f * 0.025f)
00199     //Implementation of Madgwick's IMU and AHRS algorithms (option 1)
00200     #define betaDef  0.15f
00201     //Used for DCM filter
00202     const float Kp_ROLLPITCH = 1.2f;  //was .3423, was 1.2(2/16/17)
00203     const float Ki_ROLLPITCH = 0.0234f;
00204     const float Kp_YAW = 1.2f;   // was 1.2 and 0.02, was 1.2 (2/16/17)
00205     const float Ki_YAW = 0.02f;
00206 #elif defined(ST_LSM9DS1) || defined(ST_LSM9DS1_MS5611)
00207     //Madgwick's implementation of Mayhony's AHRS algorithm
00208     #define twoKpDef  (2.0f * 1.75f)    //works with and without mag enabled
00209     #define twoKiDef  (2.0f * 0.025f)
00210     //Implementation of Madgwick's IMU and AHRS algorithms
00211     #define betaDef  0.15f
00212     //Used for DCM filter
00213     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00214     const float Ki_ROLLPITCH = 0.0234f;
00215     const float Kp_YAW = 1.2f;   // was 1.2 and 0.02
00216     const float Ki_YAW = 0.02f;
00217 #elif defined(CurieImu) || defined(CurieIMU_Mag)
00218     #define twoKpDef  (2.0f * 1.25f)
00219     #define twoKiDef  (2.0f * 0.1f)
00220     #define betaDef   0.07f
00221     //Used for DCM filter
00222     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00223     const float Ki_ROLLPITCH = 0.0234f;
00224     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00225     const float Ki_YAW = 0.002f;
00226 #elif defined(PropShield)
00227     #define twoKpDef  (2.0f * 4.25f) // was 0.95, 3.25
00228     #define twoKiDef  (2.0f * 0.25) // was 0.05 , 0.25
00229     #define betaDef   0.35
00230     //Used for DCM filter
00231     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00232     const float Ki_ROLLPITCH = 0.0234f;
00233     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00234     const float Ki_YAW = 0.002f;
00235 #else
00236     #define twoKpDef  (2.0f * 0.5f)
00237     #define twoKiDef  (2.0f * 0.1f)
00238     #define betaDef  0.1f
00239     //Used for DCM filter
00240     const float Kp_ROLLPITCH = 1.2f;  //was .3423
00241     const float Ki_ROLLPITCH = 0.0234f;
00242     const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
00243     const float Ki_YAW = 0.002f;
00244 #endif 
00245 
00246 #ifndef cbi
00247 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
00248 #endif
00249 
00250 class FreeIMU
00251 {
00252   public:
00253     FreeIMU();
00254     void init();
00255     void init(bool fastmode);
00256     
00257     void init(int accgyro_addr, bool fastmode);
00258     void sample(bool sampling);
00259     
00260     #ifndef CALIBRATION_H
00261     void calLoad();
00262     #endif
00263     void zeroGyro();
00264     void getRawValues(int16_t * raw_values);
00265     void getValues(float * values);
00266     void getQ(float * q);
00267     void getRate(float * r);
00268     void getEuler(float * angles);
00269     void getYawPitchRoll(float * ypr);
00270     void getEulerRad(float * angles);
00271     void getYawPitchRollRad(float * ypr);
00272     void gravityCompensateAcc(float * acc, float * q);
00273     float getRawPressure();
00274     
00275     float getBaroAlt();
00276     float getBaroAlt(float sea_press);
00277     
00278     void getQ_simple(float* q);
00279     
00280     // we make them public so that users can interact directly with device classes
00281 
00282       MPU6050 *accgyro;
00283       MS561101BA *baro;
00284 
00285 #if HAS_MPU6050()
00286       HMC58X3 *magn;
00287 #endif
00288     
00289     int* raw_acc, raw_gyro, raw_magn;
00290     // calibration parameters
00291     int16_t gyro_off_x, gyro_off_y, gyro_off_z;
00292     int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
00293     float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
00294     
00295   private:
00296 
00297     void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, bool _magn_valid);
00298 
00299     //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
00300     float iq0, iq1, iq2, iq3;
00301     float exInt, eyInt, ezInt;  // scaled integral error
00302     volatile float twoKp;      // 2 * proportional gain (Kp)
00303     volatile float twoKi;      // 2 * integral gain (Ki)
00304     volatile float twoKiz, twoKpz;
00305     volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
00306     volatile float integralFBx,  integralFBy, integralFBz;
00307     Timer update;
00308     int dt_us;
00309     //unsigned long lastUpdate, now; // sample period expressed in milliseconds
00310     float sampleFreq; // half the sample period expressed in seconds
00311 
00312 };
00313 
00314 float invSqrt(float number);
00315 void arr3_rad_to_deg(float * arr);
00316 
00317 
00318 
00319 #endif // FreeIMU_h
00320