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