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
- Committer:
- svkatielee
- Date:
- 2014-11-18
- Revision:
- 3:450acaa4f52d
- Parent:
- HK10DOF.h@ 0:9a1682a09c50
File content as of revision 3:450acaa4f52d:
/* FreeIMU.h - A libre and easy to use orientation sensing library for Arduino Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net> Development of this code has been supported by the Department of Computer Science, Universita' degli Studi di Torino, Italy within the Piemonte Project http://www.piemonte.di.unito.it/ This program is free software: you can redistribute it and/or modify it under the terms of the version 3 GNU General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ #ifndef IMU10DOF_h #define IMU10DOF_h // Uncomment the appropriated version of FreeIMU you are using //#define FREEIMU_v01 //#define FREEIMU_v02 //#define FREEIMU_v03 //#define FREEIMU_v035 //#define FREEIMU_v035_MS //#define FREEIMU_v035_BMP #define FREEIMU_v04 #define FREEIMU_LIB_VERSION "20121122" #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net" #if F_CPU == 16000000L #define FREEIMU_FREQ "16 MHz" #elif F_CPU == 8000000L #define FREEIMU_FREQ "8 MHz" #endif // board IDs #define FREEIMU_ID "Zboub" //#define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050)) #define IS_9DOM() //#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)) #include "mbed.h" //#include "calibration.h" /* #ifndef CALIBRATION_H #include <EEPROM.h> #endif #define FREEIMU_EEPROM_BASE 0x0A #define FREEIMU_EEPROM_SIGNATURE 0x19 */ //include des biblis des senseurs #include "ADXL345_I2C.h" #include "HMC5883L.h" #include "BMP085.h" // #include "L3G4200D.h" #include "ITG3200.h" // default I2C 7-bit addresses of the sensors #define FIMU_ACC_ADDR ADXL345_ADDR_ALT_LOW // SDO connected to GND //#define FIMU_ADXL345_DEF_ADDR ADXL345_ADDR_ALT_HIGH // SDO connected to GND #define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND // HMC5843 address is fixed so don't bother to define it /* #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain #define twoKiDef (2.0f * 0.1f) // 2 * integral gain */ #define twoKpDef (2.0f * 5.0f) // 2 * proportional gain #define twoKiDef (2.0f * 0.1f) // 2 * integral gain #ifndef cbi #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) #endif class IMU10DOF { public: IMU10DOF(PinName sda, PinName scl); //I2C i2c_; //void init(); //void init(int accgyro_addr, bool fastmode); void init(bool fastmode); #ifndef CALIBRATION_H void calLoad(); #endif void zeroGyro(); void getRawValues(int16_t * raw_values); void getValues(float * values); void getQ(float * q); void getEuler(float * angles); void getYawPitchRoll(float * ypr); void getEulerRad(float * angles); void getYawPitchRollRad(float * ypr); float getBaroAlt(); float getBaroAlt(float sea_press); //float getEstimatedAlt(); //float getEstimatedAlt(float sea_press); void gravityCompensateAcc(float * acc, float * q); // we make them public so that users can interact directly with device classes ADXL345_I2C acc; HMC5883L magn; BMP085 baro; //L3G4200D gyro; ITG3200 gyro; Serial pc; int* raw_acc, raw_gyro, raw_magn; // calibration parameters int16_t gyro_off_x, gyro_off_y, gyro_off_z; int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z; float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z; private: Timer update; int dt_us; void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); //void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az); for 6DOF sensors //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation float iq0, iq1, iq2, iq3; float exInt, eyInt, ezInt; // scaled integral error volatile float twoKp; // 2 * proportional gain (Kp) volatile float twoKi; // 2 * integral gain (Ki) volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame volatile float integralFBx, integralFBy, integralFBz; //unsigned long lastUpdate, now; // sample period expressed in milliseconds float sampleFreq; // half the sample period expressed in seconds }; float invSqrt(float number); void arr3_rad_to_deg(float * arr); #endif // FreeIMU_h