Added external magnetometer functionality
Dependencies: HMC58X31 MODI2C MPU6050 MS561101BA
Fork of FreeIMU by
FreeIMU.h@19:ab5318ef31c4, 2015-04-24 (annotated)
- Committer:
- joe4465
- Date:
- Fri Apr 24 16:49:14 2015 +0000
- Revision:
- 19:ab5318ef31c4
- Parent:
- 13:21b275eeeda2
24.04.15
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tyftyftyf | 0:21840c01d3d7 | 1 | /* |
tyftyftyf | 0:21840c01d3d7 | 2 | FreeIMU.h - A libre and easy to use orientation sensing library for Arduino |
tyftyftyf | 0:21840c01d3d7 | 3 | Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net> |
tyftyftyf | 0:21840c01d3d7 | 4 | |
tyftyftyf | 0:21840c01d3d7 | 5 | Development of this code has been supported by the Department of Computer Science, |
tyftyftyf | 0:21840c01d3d7 | 6 | Universita' degli Studi di Torino, Italy within the Piemonte Project |
tyftyftyf | 0:21840c01d3d7 | 7 | http://www.piemonte.di.unito.it/ |
tyftyftyf | 0:21840c01d3d7 | 8 | |
tyftyftyf | 0:21840c01d3d7 | 9 | |
tyftyftyf | 0:21840c01d3d7 | 10 | This program is free software: you can redistribute it and/or modify |
tyftyftyf | 0:21840c01d3d7 | 11 | it under the terms of the version 3 GNU General Public License as |
tyftyftyf | 0:21840c01d3d7 | 12 | published by the Free Software Foundation. |
tyftyftyf | 0:21840c01d3d7 | 13 | |
tyftyftyf | 0:21840c01d3d7 | 14 | This program is distributed in the hope that it will be useful, |
tyftyftyf | 0:21840c01d3d7 | 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
tyftyftyf | 0:21840c01d3d7 | 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
tyftyftyf | 0:21840c01d3d7 | 17 | GNU General Public License for more details. |
tyftyftyf | 0:21840c01d3d7 | 18 | |
tyftyftyf | 0:21840c01d3d7 | 19 | You should have received a copy of the GNU General Public License |
tyftyftyf | 0:21840c01d3d7 | 20 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
tyftyftyf | 0:21840c01d3d7 | 21 | |
tyftyftyf | 0:21840c01d3d7 | 22 | */ |
tyftyftyf | 0:21840c01d3d7 | 23 | |
tyftyftyf | 0:21840c01d3d7 | 24 | #ifndef FreeIMU_h |
tyftyftyf | 0:21840c01d3d7 | 25 | #define FreeIMU_h |
tyftyftyf | 0:21840c01d3d7 | 26 | |
tyftyftyf | 3:f9b100a9aa65 | 27 | #define I2C_SDA p28 |
tyftyftyf | 3:f9b100a9aa65 | 28 | #define I2C_SCL p27 |
tyftyftyf | 3:f9b100a9aa65 | 29 | |
tyftyftyf | 0:21840c01d3d7 | 30 | // Uncomment the appropriated version of FreeIMU you are using |
tyftyftyf | 0:21840c01d3d7 | 31 | //#define FREEIMU_v01 |
tyftyftyf | 0:21840c01d3d7 | 32 | //#define FREEIMU_v02 |
tyftyftyf | 0:21840c01d3d7 | 33 | //#define FREEIMU_v03 |
tyftyftyf | 0:21840c01d3d7 | 34 | //#define FREEIMU_v035 |
tyftyftyf | 0:21840c01d3d7 | 35 | //#define FREEIMU_v035_MS |
tyftyftyf | 0:21840c01d3d7 | 36 | //#define FREEIMU_v035_BMP |
tyftyftyf | 0:21840c01d3d7 | 37 | #define FREEIMU_v04 |
tyftyftyf | 0:21840c01d3d7 | 38 | |
tyftyftyf | 0:21840c01d3d7 | 39 | // 3rd party boards. Please consider donating or buying a FreeIMU board to support this library development. |
tyftyftyf | 0:21840c01d3d7 | 40 | //#define SEN_10121 //IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345 SEN-10121 http://www.sparkfun.com/products/10121 |
tyftyftyf | 0:21840c01d3d7 | 41 | //#define SEN_10736 //9 Degrees of Freedom - Razor IMU SEN-10736 http://www.sparkfun.com/products/10736 |
tyftyftyf | 0:21840c01d3d7 | 42 | //#define SEN_10724 //9 Degrees of Freedom - Sensor Stick SEN-10724 http://www.sparkfun.com/products/10724 |
tyftyftyf | 0:21840c01d3d7 | 43 | //#define SEN_10183 //9 Degrees of Freedom - Sensor Stick SEN-10183 http://www.sparkfun.com/products/10183 |
tyftyftyf | 0:21840c01d3d7 | 44 | //#define ARDUIMU_v3 // DIYDrones ArduIMU+ V3 http://store.diydrones.com/ArduIMU_V3_p/kt-arduimu-30.htm or https://www.sparkfun.com/products/11055 |
tyftyftyf | 0:21840c01d3d7 | 45 | #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. |
tyftyftyf | 0:21840c01d3d7 | 46 | |
tyftyftyf | 0:21840c01d3d7 | 47 | // *** No configuration needed below this line *** |
tyftyftyf | 0:21840c01d3d7 | 48 | |
tyftyftyf | 0:21840c01d3d7 | 49 | |
tyftyftyf | 0:21840c01d3d7 | 50 | #define FREEIMU_LIB_VERSION "20121122" |
tyftyftyf | 0:21840c01d3d7 | 51 | |
tyftyftyf | 0:21840c01d3d7 | 52 | #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net" |
tyftyftyf | 0:21840c01d3d7 | 53 | |
tyftyftyf | 0:21840c01d3d7 | 54 | #if F_CPU == 16000000L |
tyftyftyf | 0:21840c01d3d7 | 55 | #define FREEIMU_FREQ "16 MHz" |
tyftyftyf | 0:21840c01d3d7 | 56 | #elif F_CPU == 8000000L |
tyftyftyf | 0:21840c01d3d7 | 57 | #define FREEIMU_FREQ "8 MHz" |
tyftyftyf | 0:21840c01d3d7 | 58 | #endif |
tyftyftyf | 0:21840c01d3d7 | 59 | |
tyftyftyf | 0:21840c01d3d7 | 60 | |
tyftyftyf | 0:21840c01d3d7 | 61 | // board IDs |
tyftyftyf | 0:21840c01d3d7 | 62 | |
tyftyftyf | 0:21840c01d3d7 | 63 | #if defined(FREEIMU_v04) |
tyftyftyf | 0:21840c01d3d7 | 64 | #define FREEIMU_ID "FreeIMU v0.4" |
tyftyftyf | 0:21840c01d3d7 | 65 | #endif |
tyftyftyf | 0:21840c01d3d7 | 66 | |
tyftyftyf | 0:21840c01d3d7 | 67 | |
tyftyftyf | 0:21840c01d3d7 | 68 | #define HAS_MPU6050() (defined(FREEIMU_v04) || defined(GEN_MPU6050)) |
tyftyftyf | 0:21840c01d3d7 | 69 | |
tyftyftyf | 0:21840c01d3d7 | 70 | #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050)) |
tyftyftyf | 0:21840c01d3d7 | 71 | #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)) |
tyftyftyf | 0:21840c01d3d7 | 72 | |
tyftyftyf | 0:21840c01d3d7 | 73 | |
tyftyftyf | 0:21840c01d3d7 | 74 | |
tyftyftyf | 0:21840c01d3d7 | 75 | //#include <Wire.h> |
tyftyftyf | 0:21840c01d3d7 | 76 | |
tyftyftyf | 0:21840c01d3d7 | 77 | #include "mbed.h" |
tyftyftyf | 0:21840c01d3d7 | 78 | #include "calibration.h" |
tyftyftyf | 0:21840c01d3d7 | 79 | /* |
tyftyftyf | 0:21840c01d3d7 | 80 | #ifndef CALIBRATION_H |
tyftyftyf | 0:21840c01d3d7 | 81 | #include <EEPROM.h> |
tyftyftyf | 0:21840c01d3d7 | 82 | #endif |
tyftyftyf | 0:21840c01d3d7 | 83 | |
tyftyftyf | 0:21840c01d3d7 | 84 | #define FREEIMU_EEPROM_BASE 0x0A |
tyftyftyf | 0:21840c01d3d7 | 85 | #define FREEIMU_EEPROM_SIGNATURE 0x19 |
tyftyftyf | 0:21840c01d3d7 | 86 | */ |
tyftyftyf | 0:21840c01d3d7 | 87 | //#if FREEIMU_VER <= 3 |
tyftyftyf | 0:21840c01d3d7 | 88 | |
tyftyftyf | 0:21840c01d3d7 | 89 | #if HAS_MPU6050() |
tyftyftyf | 0:21840c01d3d7 | 90 | // #include <Wire.h> |
tyftyftyf | 0:21840c01d3d7 | 91 | #include "I2Cdev.h" |
tyftyftyf | 0:21840c01d3d7 | 92 | #include "MPU6050.h" |
tyftyftyf | 0:21840c01d3d7 | 93 | #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS |
tyftyftyf | 0:21840c01d3d7 | 94 | |
tyftyftyf | 0:21840c01d3d7 | 95 | #endif |
tyftyftyf | 0:21840c01d3d7 | 96 | |
tyftyftyf | 0:21840c01d3d7 | 97 | #include <HMC58X3.h> |
tyftyftyf | 0:21840c01d3d7 | 98 | #include <MS561101BA.h> |
tyftyftyf | 0:21840c01d3d7 | 99 | |
tyftyftyf | 0:21840c01d3d7 | 100 | |
tyftyftyf | 0:21840c01d3d7 | 101 | #define FIMU_BARO_ADDR MS561101BA_ADDR_CSB_LOW |
tyftyftyf | 0:21840c01d3d7 | 102 | |
tyftyftyf | 0:21840c01d3d7 | 103 | |
tyftyftyf | 0:21840c01d3d7 | 104 | #define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW |
tyftyftyf | 0:21840c01d3d7 | 105 | #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND |
tyftyftyf | 0:21840c01d3d7 | 106 | // HMC5843 address is fixed so don't bother to define it |
tyftyftyf | 0:21840c01d3d7 | 107 | |
tyftyftyf | 0:21840c01d3d7 | 108 | |
tyftyftyf | 9:a79af1283446 | 109 | #define twoKpDef (2.0f * 3.0f) // 2 * proportional gain |
tyftyftyf | 9:a79af1283446 | 110 | #define twoKiDef (2.0f * 0.4f) // 2 * integral gain |
tyftyftyf | 0:21840c01d3d7 | 111 | |
tyftyftyf | 0:21840c01d3d7 | 112 | #ifndef cbi |
tyftyftyf | 0:21840c01d3d7 | 113 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) |
tyftyftyf | 0:21840c01d3d7 | 114 | #endif |
tyftyftyf | 0:21840c01d3d7 | 115 | |
tyftyftyf | 0:21840c01d3d7 | 116 | class FreeIMU |
tyftyftyf | 0:21840c01d3d7 | 117 | { |
tyftyftyf | 0:21840c01d3d7 | 118 | public: |
tyftyftyf | 0:21840c01d3d7 | 119 | FreeIMU(); |
tyftyftyf | 0:21840c01d3d7 | 120 | void init(); |
tyftyftyf | 0:21840c01d3d7 | 121 | void init(bool fastmode); |
tyftyftyf | 0:21840c01d3d7 | 122 | |
tyftyftyf | 0:21840c01d3d7 | 123 | void init(int accgyro_addr, bool fastmode); |
joe4465 | 13:21b275eeeda2 | 124 | void sample(bool sampling); |
tyftyftyf | 0:21840c01d3d7 | 125 | |
tyftyftyf | 0:21840c01d3d7 | 126 | #ifndef CALIBRATION_H |
tyftyftyf | 0:21840c01d3d7 | 127 | void calLoad(); |
tyftyftyf | 0:21840c01d3d7 | 128 | #endif |
tyftyftyf | 0:21840c01d3d7 | 129 | void zeroGyro(); |
tyftyftyf | 0:21840c01d3d7 | 130 | void getRawValues(int16_t * raw_values); |
tyftyftyf | 0:21840c01d3d7 | 131 | void getValues(float * values); |
tyftyftyf | 0:21840c01d3d7 | 132 | void getQ(float * q); |
joe4465 | 13:21b275eeeda2 | 133 | void getRate(float * r); |
tyftyftyf | 0:21840c01d3d7 | 134 | void getEuler(float * angles); |
tyftyftyf | 0:21840c01d3d7 | 135 | void getYawPitchRoll(float * ypr); |
tyftyftyf | 0:21840c01d3d7 | 136 | void getEulerRad(float * angles); |
tyftyftyf | 0:21840c01d3d7 | 137 | void getYawPitchRollRad(float * ypr); |
tyftyftyf | 0:21840c01d3d7 | 138 | void gravityCompensateAcc(float * acc, float * q); |
tyftyftyf | 0:21840c01d3d7 | 139 | float getRawPressure(); |
tyftyftyf | 0:21840c01d3d7 | 140 | |
tyftyftyf | 0:21840c01d3d7 | 141 | float getBaroAlt(); |
tyftyftyf | 0:21840c01d3d7 | 142 | float getBaroAlt(float sea_press); |
tyftyftyf | 0:21840c01d3d7 | 143 | |
tyftyftyf | 2:5c419926dcd7 | 144 | void getQ_simple(float* q); |
tyftyftyf | 2:5c419926dcd7 | 145 | |
tyftyftyf | 0:21840c01d3d7 | 146 | // we make them public so that users can interact directly with device classes |
tyftyftyf | 0:21840c01d3d7 | 147 | |
tyftyftyf | 3:f9b100a9aa65 | 148 | MPU6050 *accgyro; |
tyftyftyf | 3:f9b100a9aa65 | 149 | MS561101BA *baro; |
tyftyftyf | 3:f9b100a9aa65 | 150 | HMC58X3 *magn; |
tyftyftyf | 0:21840c01d3d7 | 151 | |
tyftyftyf | 0:21840c01d3d7 | 152 | int* raw_acc, raw_gyro, raw_magn; |
tyftyftyf | 0:21840c01d3d7 | 153 | // calibration parameters |
tyftyftyf | 0:21840c01d3d7 | 154 | int16_t gyro_off_x, gyro_off_y, gyro_off_z; |
tyftyftyf | 0:21840c01d3d7 | 155 | int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z; |
tyftyftyf | 0:21840c01d3d7 | 156 | float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z; |
tyftyftyf | 0:21840c01d3d7 | 157 | |
tyftyftyf | 0:21840c01d3d7 | 158 | private: |
tyftyftyf | 0:21840c01d3d7 | 159 | |
tyftyftyf | 3:f9b100a9aa65 | 160 | void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, bool _magn_valid); |
tyftyftyf | 0:21840c01d3d7 | 161 | |
tyftyftyf | 0:21840c01d3d7 | 162 | //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation |
tyftyftyf | 0:21840c01d3d7 | 163 | float iq0, iq1, iq2, iq3; |
tyftyftyf | 0:21840c01d3d7 | 164 | float exInt, eyInt, ezInt; // scaled integral error |
tyftyftyf | 0:21840c01d3d7 | 165 | volatile float twoKp; // 2 * proportional gain (Kp) |
tyftyftyf | 0:21840c01d3d7 | 166 | volatile float twoKi; // 2 * integral gain (Ki) |
tyftyftyf | 6:6b1185b32814 | 167 | volatile float twoKiz, twoKpz; |
tyftyftyf | 0:21840c01d3d7 | 168 | volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame |
tyftyftyf | 0:21840c01d3d7 | 169 | volatile float integralFBx, integralFBy, integralFBz; |
tyftyftyf | 0:21840c01d3d7 | 170 | Timer update; |
tyftyftyf | 0:21840c01d3d7 | 171 | int dt_us; |
tyftyftyf | 0:21840c01d3d7 | 172 | //unsigned long lastUpdate, now; // sample period expressed in milliseconds |
tyftyftyf | 0:21840c01d3d7 | 173 | float sampleFreq; // half the sample period expressed in seconds |
tyftyftyf | 2:5c419926dcd7 | 174 | |
tyftyftyf | 0:21840c01d3d7 | 175 | }; |
tyftyftyf | 0:21840c01d3d7 | 176 | |
tyftyftyf | 0:21840c01d3d7 | 177 | float invSqrt(float number); |
tyftyftyf | 0:21840c01d3d7 | 178 | void arr3_rad_to_deg(float * arr); |
tyftyftyf | 0:21840c01d3d7 | 179 | |
tyftyftyf | 0:21840c01d3d7 | 180 | |
tyftyftyf | 0:21840c01d3d7 | 181 | |
tyftyftyf | 0:21840c01d3d7 | 182 | #endif // FreeIMU_h |
tyftyftyf | 0:21840c01d3d7 | 183 |