Aloïs Wolff / Mbed 2 deprecated MbedFreeIMU

quick and probably buggy port from the FreeIMU 0.4 library adapted for MBED and MPU6050 only...

Dependencies:   MPU6050_tmp mbed

FreeIMU.h@0:c7a5b6fa0171, 2013-02-20 (annotated)

Committer:
pommzorz
Date:
Wed Feb 20 15:53:00 2013 +0000
Revision:
0:c7a5b6fa0171
release.

Who changed what in which revision?

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