IMU 10dof MEMS from DR Robot adapted from HK10DOF Changed gyro to ITG3200
Fork of HK10DOF by
WARNING: This project is not complete, but this library seems ok so far.
I have the module DFRobotics.com 10DOF MEMS IMU. I wanted a concise module for resolving direction and movement.
I found HK10DOF library (http://developer.mbed.org/users/pommzorz/code/HK10DOF/) with quaternions. But it used a different gyro. So I modified that code to use the same higher level calls but use the ITG3200 low level calls.
IMU10DOF.h@3:450acaa4f52d, 2014-11-18 (annotated)
- Committer:
- svkatielee
- Date:
- Tue Nov 18 06:25:57 2014 +0000
- Revision:
- 3:450acaa4f52d
- Parent:
- HK10DOF.h@0:9a1682a09c50
Updated the code from off-line, for switching platform to NUCLEO-F401RE and IMU from HK to DFRobot
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pommzorz | 0:9a1682a09c50 | 1 | /* |
pommzorz | 0:9a1682a09c50 | 2 | FreeIMU.h - A libre and easy to use orientation sensing library for Arduino |
pommzorz | 0:9a1682a09c50 | 3 | Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net> |
pommzorz | 0:9a1682a09c50 | 4 | |
pommzorz | 0:9a1682a09c50 | 5 | Development of this code has been supported by the Department of Computer Science, |
pommzorz | 0:9a1682a09c50 | 6 | Universita' degli Studi di Torino, Italy within the Piemonte Project |
pommzorz | 0:9a1682a09c50 | 7 | http://www.piemonte.di.unito.it/ |
pommzorz | 0:9a1682a09c50 | 8 | |
pommzorz | 0:9a1682a09c50 | 9 | |
pommzorz | 0:9a1682a09c50 | 10 | This program is free software: you can redistribute it and/or modify |
pommzorz | 0:9a1682a09c50 | 11 | it under the terms of the version 3 GNU General Public License as |
pommzorz | 0:9a1682a09c50 | 12 | published by the Free Software Foundation. |
pommzorz | 0:9a1682a09c50 | 13 | |
pommzorz | 0:9a1682a09c50 | 14 | This program is distributed in the hope that it will be useful, |
pommzorz | 0:9a1682a09c50 | 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
pommzorz | 0:9a1682a09c50 | 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
pommzorz | 0:9a1682a09c50 | 17 | GNU General Public License for more details. |
pommzorz | 0:9a1682a09c50 | 18 | |
pommzorz | 0:9a1682a09c50 | 19 | You should have received a copy of the GNU General Public License |
pommzorz | 0:9a1682a09c50 | 20 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
pommzorz | 0:9a1682a09c50 | 21 | |
pommzorz | 0:9a1682a09c50 | 22 | */ |
pommzorz | 0:9a1682a09c50 | 23 | |
svkatielee | 3:450acaa4f52d | 24 | #ifndef IMU10DOF_h |
svkatielee | 3:450acaa4f52d | 25 | #define IMU10DOF_h |
pommzorz | 0:9a1682a09c50 | 26 | |
pommzorz | 0:9a1682a09c50 | 27 | // Uncomment the appropriated version of FreeIMU you are using |
pommzorz | 0:9a1682a09c50 | 28 | //#define FREEIMU_v01 |
pommzorz | 0:9a1682a09c50 | 29 | //#define FREEIMU_v02 |
pommzorz | 0:9a1682a09c50 | 30 | //#define FREEIMU_v03 |
pommzorz | 0:9a1682a09c50 | 31 | //#define FREEIMU_v035 |
pommzorz | 0:9a1682a09c50 | 32 | //#define FREEIMU_v035_MS |
pommzorz | 0:9a1682a09c50 | 33 | //#define FREEIMU_v035_BMP |
pommzorz | 0:9a1682a09c50 | 34 | #define FREEIMU_v04 |
pommzorz | 0:9a1682a09c50 | 35 | |
pommzorz | 0:9a1682a09c50 | 36 | |
pommzorz | 0:9a1682a09c50 | 37 | |
pommzorz | 0:9a1682a09c50 | 38 | #define FREEIMU_LIB_VERSION "20121122" |
pommzorz | 0:9a1682a09c50 | 39 | |
pommzorz | 0:9a1682a09c50 | 40 | #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net" |
pommzorz | 0:9a1682a09c50 | 41 | |
pommzorz | 0:9a1682a09c50 | 42 | #if F_CPU == 16000000L |
pommzorz | 0:9a1682a09c50 | 43 | #define FREEIMU_FREQ "16 MHz" |
pommzorz | 0:9a1682a09c50 | 44 | #elif F_CPU == 8000000L |
pommzorz | 0:9a1682a09c50 | 45 | #define FREEIMU_FREQ "8 MHz" |
pommzorz | 0:9a1682a09c50 | 46 | #endif |
pommzorz | 0:9a1682a09c50 | 47 | |
pommzorz | 0:9a1682a09c50 | 48 | |
pommzorz | 0:9a1682a09c50 | 49 | // board IDs |
pommzorz | 0:9a1682a09c50 | 50 | |
pommzorz | 0:9a1682a09c50 | 51 | |
pommzorz | 0:9a1682a09c50 | 52 | #define FREEIMU_ID "Zboub" |
pommzorz | 0:9a1682a09c50 | 53 | |
pommzorz | 0:9a1682a09c50 | 54 | //#define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050)) |
pommzorz | 0:9a1682a09c50 | 55 | #define IS_9DOM() |
pommzorz | 0:9a1682a09c50 | 56 | //#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)) |
pommzorz | 0:9a1682a09c50 | 57 | |
pommzorz | 0:9a1682a09c50 | 58 | |
pommzorz | 0:9a1682a09c50 | 59 | |
pommzorz | 0:9a1682a09c50 | 60 | |
pommzorz | 0:9a1682a09c50 | 61 | #include "mbed.h" |
pommzorz | 0:9a1682a09c50 | 62 | |
pommzorz | 0:9a1682a09c50 | 63 | //#include "calibration.h" |
pommzorz | 0:9a1682a09c50 | 64 | /* |
pommzorz | 0:9a1682a09c50 | 65 | #ifndef CALIBRATION_H |
pommzorz | 0:9a1682a09c50 | 66 | #include <EEPROM.h> |
pommzorz | 0:9a1682a09c50 | 67 | #endif |
pommzorz | 0:9a1682a09c50 | 68 | #define FREEIMU_EEPROM_BASE 0x0A |
pommzorz | 0:9a1682a09c50 | 69 | #define FREEIMU_EEPROM_SIGNATURE 0x19 |
pommzorz | 0:9a1682a09c50 | 70 | */ |
pommzorz | 0:9a1682a09c50 | 71 | |
pommzorz | 0:9a1682a09c50 | 72 | //include des biblis des senseurs |
pommzorz | 0:9a1682a09c50 | 73 | |
pommzorz | 0:9a1682a09c50 | 74 | #include "ADXL345_I2C.h" |
pommzorz | 0:9a1682a09c50 | 75 | #include "HMC5883L.h" |
pommzorz | 0:9a1682a09c50 | 76 | #include "BMP085.h" |
svkatielee | 3:450acaa4f52d | 77 | // #include "L3G4200D.h" |
svkatielee | 3:450acaa4f52d | 78 | #include "ITG3200.h" |
pommzorz | 0:9a1682a09c50 | 79 | |
pommzorz | 0:9a1682a09c50 | 80 | // default I2C 7-bit addresses of the sensors |
pommzorz | 0:9a1682a09c50 | 81 | #define FIMU_ACC_ADDR ADXL345_ADDR_ALT_LOW // SDO connected to GND |
pommzorz | 0:9a1682a09c50 | 82 | //#define FIMU_ADXL345_DEF_ADDR ADXL345_ADDR_ALT_HIGH // SDO connected to GND |
pommzorz | 0:9a1682a09c50 | 83 | |
pommzorz | 0:9a1682a09c50 | 84 | #define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW |
pommzorz | 0:9a1682a09c50 | 85 | #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND |
pommzorz | 0:9a1682a09c50 | 86 | // HMC5843 address is fixed so don't bother to define it |
pommzorz | 0:9a1682a09c50 | 87 | |
pommzorz | 0:9a1682a09c50 | 88 | /* |
pommzorz | 0:9a1682a09c50 | 89 | #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain |
pommzorz | 0:9a1682a09c50 | 90 | #define twoKiDef (2.0f * 0.1f) // 2 * integral gain |
pommzorz | 0:9a1682a09c50 | 91 | */ |
pommzorz | 0:9a1682a09c50 | 92 | #define twoKpDef (2.0f * 5.0f) // 2 * proportional gain |
pommzorz | 0:9a1682a09c50 | 93 | #define twoKiDef (2.0f * 0.1f) // 2 * integral gain |
pommzorz | 0:9a1682a09c50 | 94 | |
pommzorz | 0:9a1682a09c50 | 95 | |
pommzorz | 0:9a1682a09c50 | 96 | #ifndef cbi |
pommzorz | 0:9a1682a09c50 | 97 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) |
pommzorz | 0:9a1682a09c50 | 98 | #endif |
pommzorz | 0:9a1682a09c50 | 99 | |
svkatielee | 3:450acaa4f52d | 100 | class IMU10DOF |
pommzorz | 0:9a1682a09c50 | 101 | { |
pommzorz | 0:9a1682a09c50 | 102 | public: |
svkatielee | 3:450acaa4f52d | 103 | IMU10DOF(PinName sda, PinName scl); |
pommzorz | 0:9a1682a09c50 | 104 | //I2C i2c_; |
pommzorz | 0:9a1682a09c50 | 105 | |
pommzorz | 0:9a1682a09c50 | 106 | //void init(); |
pommzorz | 0:9a1682a09c50 | 107 | //void init(int accgyro_addr, bool fastmode); |
pommzorz | 0:9a1682a09c50 | 108 | void init(bool fastmode); |
pommzorz | 0:9a1682a09c50 | 109 | #ifndef CALIBRATION_H |
pommzorz | 0:9a1682a09c50 | 110 | void calLoad(); |
pommzorz | 0:9a1682a09c50 | 111 | #endif |
pommzorz | 0:9a1682a09c50 | 112 | void zeroGyro(); |
pommzorz | 0:9a1682a09c50 | 113 | void getRawValues(int16_t * raw_values); |
pommzorz | 0:9a1682a09c50 | 114 | void getValues(float * values); |
pommzorz | 0:9a1682a09c50 | 115 | void getQ(float * q); |
pommzorz | 0:9a1682a09c50 | 116 | void getEuler(float * angles); |
pommzorz | 0:9a1682a09c50 | 117 | void getYawPitchRoll(float * ypr); |
pommzorz | 0:9a1682a09c50 | 118 | void getEulerRad(float * angles); |
pommzorz | 0:9a1682a09c50 | 119 | void getYawPitchRollRad(float * ypr); |
pommzorz | 0:9a1682a09c50 | 120 | |
pommzorz | 0:9a1682a09c50 | 121 | float getBaroAlt(); |
pommzorz | 0:9a1682a09c50 | 122 | float getBaroAlt(float sea_press); |
pommzorz | 0:9a1682a09c50 | 123 | //float getEstimatedAlt(); |
pommzorz | 0:9a1682a09c50 | 124 | //float getEstimatedAlt(float sea_press); |
pommzorz | 0:9a1682a09c50 | 125 | |
pommzorz | 0:9a1682a09c50 | 126 | void gravityCompensateAcc(float * acc, float * q); |
pommzorz | 0:9a1682a09c50 | 127 | |
pommzorz | 0:9a1682a09c50 | 128 | // we make them public so that users can interact directly with device classes |
pommzorz | 0:9a1682a09c50 | 129 | |
pommzorz | 0:9a1682a09c50 | 130 | ADXL345_I2C acc; |
pommzorz | 0:9a1682a09c50 | 131 | HMC5883L magn; |
pommzorz | 0:9a1682a09c50 | 132 | BMP085 baro; |
svkatielee | 3:450acaa4f52d | 133 | //L3G4200D gyro; |
svkatielee | 3:450acaa4f52d | 134 | ITG3200 gyro; |
svkatielee | 3:450acaa4f52d | 135 | Serial pc; |
pommzorz | 0:9a1682a09c50 | 136 | |
pommzorz | 0:9a1682a09c50 | 137 | |
pommzorz | 0:9a1682a09c50 | 138 | |
pommzorz | 0:9a1682a09c50 | 139 | |
pommzorz | 0:9a1682a09c50 | 140 | int* raw_acc, raw_gyro, raw_magn; |
pommzorz | 0:9a1682a09c50 | 141 | // calibration parameters |
pommzorz | 0:9a1682a09c50 | 142 | int16_t gyro_off_x, gyro_off_y, gyro_off_z; |
pommzorz | 0:9a1682a09c50 | 143 | int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z; |
pommzorz | 0:9a1682a09c50 | 144 | float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z; |
pommzorz | 0:9a1682a09c50 | 145 | |
pommzorz | 0:9a1682a09c50 | 146 | private: |
pommzorz | 0:9a1682a09c50 | 147 | Timer update; |
pommzorz | 0:9a1682a09c50 | 148 | int dt_us; |
pommzorz | 0:9a1682a09c50 | 149 | |
pommzorz | 0:9a1682a09c50 | 150 | void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); |
pommzorz | 0:9a1682a09c50 | 151 | |
pommzorz | 0:9a1682a09c50 | 152 | //void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az); for 6DOF sensors |
pommzorz | 0:9a1682a09c50 | 153 | |
pommzorz | 0:9a1682a09c50 | 154 | //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation |
pommzorz | 0:9a1682a09c50 | 155 | float iq0, iq1, iq2, iq3; |
pommzorz | 0:9a1682a09c50 | 156 | float exInt, eyInt, ezInt; // scaled integral error |
pommzorz | 0:9a1682a09c50 | 157 | volatile float twoKp; // 2 * proportional gain (Kp) |
pommzorz | 0:9a1682a09c50 | 158 | volatile float twoKi; // 2 * integral gain (Ki) |
pommzorz | 0:9a1682a09c50 | 159 | volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame |
pommzorz | 0:9a1682a09c50 | 160 | volatile float integralFBx, integralFBy, integralFBz; |
pommzorz | 0:9a1682a09c50 | 161 | //unsigned long lastUpdate, now; // sample period expressed in milliseconds |
pommzorz | 0:9a1682a09c50 | 162 | float sampleFreq; // half the sample period expressed in seconds |
pommzorz | 0:9a1682a09c50 | 163 | |
pommzorz | 0:9a1682a09c50 | 164 | }; |
pommzorz | 0:9a1682a09c50 | 165 | |
pommzorz | 0:9a1682a09c50 | 166 | float invSqrt(float number); |
pommzorz | 0:9a1682a09c50 | 167 | void arr3_rad_to_deg(float * arr); |
pommzorz | 0:9a1682a09c50 | 168 | |
pommzorz | 0:9a1682a09c50 | 169 | |
pommzorz | 0:9a1682a09c50 | 170 | |
pommzorz | 0:9a1682a09c50 | 171 | #endif // FreeIMU_h |
pommzorz | 0:9a1682a09c50 | 172 |