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.
Diff: IMU10DOF.h
- Revision:
- 3:450acaa4f52d
- Parent:
- 0:9a1682a09c50
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU10DOF.h Tue Nov 18 06:25:57 2014 +0000 @@ -0,0 +1,172 @@ +/* +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 +