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 #define I2C_SDA p28
00028 #define I2C_SCL p27
00029 
00030 // Uncomment the appropriated version of FreeIMU you are using
00031 //#define FREEIMU_v01
00032 //#define FREEIMU_v02
00033 //#define FREEIMU_v03
00034 //#define FREEIMU_v035
00035 //#define FREEIMU_v035_MS
00036 //#define FREEIMU_v035_BMP
00037 #define FREEIMU_v04
00038 
00039 // 3rd party boards. Please consider donating or buying a FreeIMU board to support this library development.
00040 //#define SEN_10121 //IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345 SEN-10121 http://www.sparkfun.com/products/10121
00041 //#define SEN_10736 //9 Degrees of Freedom - Razor IMU SEN-10736 http://www.sparkfun.com/products/10736
00042 //#define SEN_10724 //9 Degrees of Freedom - Sensor Stick SEN-10724 http://www.sparkfun.com/products/10724
00043 //#define SEN_10183 //9 Degrees of Freedom - Sensor Stick  SEN-10183 http://www.sparkfun.com/products/10183
00044 //#define ARDUIMU_v3 //  DIYDrones ArduIMU+ V3 http://store.diydrones.com/ArduIMU_V3_p/kt-arduimu-30.htm or https://www.sparkfun.com/products/11055
00045 #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.
00046 
00047 // *** No configuration needed below this line ***
00048 
00049 
00050 #define FREEIMU_LIB_VERSION "20121122"
00051 
00052 #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
00053 
00054 #if F_CPU == 16000000L
00055   #define FREEIMU_FREQ "16 MHz"
00056 #elif F_CPU == 8000000L
00057   #define FREEIMU_FREQ "8 MHz"
00058 #endif
00059 
00060 
00061 // board IDs
00062 
00063 #if defined(FREEIMU_v04)
00064   #define FREEIMU_ID "FreeIMU v0.4"
00065 #endif
00066 
00067 
00068 #define HAS_MPU6050() (defined(FREEIMU_v04) || defined(GEN_MPU6050))
00069 
00070 #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
00071 #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))
00072 
00073 
00074 
00075 //#include <Wire.h>
00076 
00077 #include "mbed.h"
00078 #include "calibration.h"
00079 /*
00080 #ifndef CALIBRATION_H
00081 #include <EEPROM.h>
00082 #endif
00083 
00084 #define FREEIMU_EEPROM_BASE 0x0A
00085 #define FREEIMU_EEPROM_SIGNATURE 0x19
00086 */
00087 //#if FREEIMU_VER <= 3
00088 
00089 #if HAS_MPU6050()
00090  // #include <Wire.h>
00091   #include "I2Cdev.h"
00092   #include "MPU6050.h"
00093   #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS
00094 
00095 #endif
00096 
00097 #include <HMC58X3.h>
00098 #include <MS561101BA.h>
00099 
00100 
00101 #define FIMU_BARO_ADDR MS561101BA_ADDR_CSB_LOW
00102 
00103 
00104 #define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW
00105 #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND
00106 // HMC5843 address is fixed so don't bother to define it
00107 
00108 
00109 #define twoKpDef  (2.0f * 3.0f) // 2 * proportional gain
00110 #define twoKiDef  (2.0f * 0.4f) // 2 * integral gain
00111 
00112 #ifndef cbi
00113 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
00114 #endif
00115 
00116 class FreeIMU
00117 {
00118   public:
00119     FreeIMU();
00120     void init();
00121     void init(bool fastmode);
00122     
00123     void init(int accgyro_addr, bool fastmode);
00124     void sample(bool sampling);
00125     
00126     #ifndef CALIBRATION_H
00127     void calLoad();
00128     #endif
00129     void zeroGyro();
00130     void getRawValues(int16_t * raw_values);
00131     void getValues(float * values);
00132     void getQ(float * q);
00133     void getRate(float * r);
00134     void getEuler(float * angles);
00135     void getYawPitchRoll(float * ypr);
00136     void getEulerRad(float * angles);
00137     void getYawPitchRollRad(float * ypr);
00138     void gravityCompensateAcc(float * acc, float * q);
00139     float getRawPressure();
00140     
00141     float getBaroAlt();
00142     float getBaroAlt(float sea_press);
00143     
00144     void getQ_simple(float* q);
00145     
00146     // we make them public so that users can interact directly with device classes
00147 
00148       MPU6050 *accgyro;
00149       MS561101BA *baro;
00150       HMC58X3 *magn;
00151     
00152     int* raw_acc, raw_gyro, raw_magn;
00153     // calibration parameters
00154     int16_t gyro_off_x, gyro_off_y, gyro_off_z;
00155     int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
00156     float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
00157     
00158   private:
00159 
00160     void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, bool _magn_valid);
00161 
00162     //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
00163     float iq0, iq1, iq2, iq3;
00164     float exInt, eyInt, ezInt;  // scaled integral error
00165     volatile float twoKp;      // 2 * proportional gain (Kp)
00166     volatile float twoKi;      // 2 * integral gain (Ki)
00167     volatile float twoKiz, twoKpz;
00168     volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
00169     volatile float integralFBx,  integralFBy, integralFBz;
00170     Timer update;
00171     int dt_us;
00172     //unsigned long lastUpdate, now; // sample period expressed in milliseconds
00173     float sampleFreq; // half the sample period expressed in seconds
00174 
00175 };
00176 
00177 float invSqrt(float number);
00178 void arr3_rad_to_deg(float * arr);
00179 
00180 
00181 
00182 #endif // FreeIMU_h
00183