Added external magnetometer functionality

Dependencies:   HMC58X31 MODI2C MPU6050 MS561101BA

Dependents:   Quadcopter_mk2

Fork of FreeIMU by Yifei Teng

Committer:
tyftyftyf
Date:
Fri Jan 10 05:41:40 2014 +0000
Revision:
9:a79af1283446
Parent:
6:6b1185b32814
Child:
13:21b275eeeda2
Tuning parameters

Who changed what in which revision?

UserRevisionLine numberNew 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);
tyftyftyf 0:21840c01d3d7 124
tyftyftyf 0:21840c01d3d7 125 #ifndef CALIBRATION_H
tyftyftyf 0:21840c01d3d7 126 void calLoad();
tyftyftyf 0:21840c01d3d7 127 #endif
tyftyftyf 0:21840c01d3d7 128 void zeroGyro();
tyftyftyf 0:21840c01d3d7 129 void getRawValues(int16_t * raw_values);
tyftyftyf 0:21840c01d3d7 130 void getValues(float * values);
tyftyftyf 0:21840c01d3d7 131 void getQ(float * q);
tyftyftyf 0:21840c01d3d7 132 void getEuler(float * angles);
tyftyftyf 0:21840c01d3d7 133 void getYawPitchRoll(float * ypr);
tyftyftyf 0:21840c01d3d7 134 void getEulerRad(float * angles);
tyftyftyf 0:21840c01d3d7 135 void getYawPitchRollRad(float * ypr);
tyftyftyf 0:21840c01d3d7 136 void gravityCompensateAcc(float * acc, float * q);
tyftyftyf 0:21840c01d3d7 137 float getRawPressure();
tyftyftyf 0:21840c01d3d7 138
tyftyftyf 0:21840c01d3d7 139 float getBaroAlt();
tyftyftyf 0:21840c01d3d7 140 float getBaroAlt(float sea_press);
tyftyftyf 0:21840c01d3d7 141
tyftyftyf 2:5c419926dcd7 142 void getQ_simple(float* q);
tyftyftyf 2:5c419926dcd7 143
tyftyftyf 0:21840c01d3d7 144 // we make them public so that users can interact directly with device classes
tyftyftyf 0:21840c01d3d7 145
tyftyftyf 3:f9b100a9aa65 146 MPU6050 *accgyro;
tyftyftyf 3:f9b100a9aa65 147 MS561101BA *baro;
tyftyftyf 3:f9b100a9aa65 148 HMC58X3 *magn;
tyftyftyf 0:21840c01d3d7 149
tyftyftyf 0:21840c01d3d7 150 int* raw_acc, raw_gyro, raw_magn;
tyftyftyf 0:21840c01d3d7 151 // calibration parameters
tyftyftyf 0:21840c01d3d7 152 int16_t gyro_off_x, gyro_off_y, gyro_off_z;
tyftyftyf 0:21840c01d3d7 153 int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
tyftyftyf 0:21840c01d3d7 154 float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
tyftyftyf 0:21840c01d3d7 155
tyftyftyf 0:21840c01d3d7 156 private:
tyftyftyf 0:21840c01d3d7 157
tyftyftyf 3:f9b100a9aa65 158 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 159
tyftyftyf 0:21840c01d3d7 160 //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
tyftyftyf 0:21840c01d3d7 161 float iq0, iq1, iq2, iq3;
tyftyftyf 0:21840c01d3d7 162 float exInt, eyInt, ezInt; // scaled integral error
tyftyftyf 0:21840c01d3d7 163 volatile float twoKp; // 2 * proportional gain (Kp)
tyftyftyf 0:21840c01d3d7 164 volatile float twoKi; // 2 * integral gain (Ki)
tyftyftyf 6:6b1185b32814 165 volatile float twoKiz, twoKpz;
tyftyftyf 0:21840c01d3d7 166 volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
tyftyftyf 0:21840c01d3d7 167 volatile float integralFBx, integralFBy, integralFBz;
tyftyftyf 0:21840c01d3d7 168 Timer update;
tyftyftyf 0:21840c01d3d7 169 int dt_us;
tyftyftyf 0:21840c01d3d7 170 //unsigned long lastUpdate, now; // sample period expressed in milliseconds
tyftyftyf 0:21840c01d3d7 171 float sampleFreq; // half the sample period expressed in seconds
tyftyftyf 2:5c419926dcd7 172
tyftyftyf 0:21840c01d3d7 173 };
tyftyftyf 0:21840c01d3d7 174
tyftyftyf 0:21840c01d3d7 175 float invSqrt(float number);
tyftyftyf 0:21840c01d3d7 176 void arr3_rad_to_deg(float * arr);
tyftyftyf 0:21840c01d3d7 177
tyftyftyf 0:21840c01d3d7 178
tyftyftyf 0:21840c01d3d7 179
tyftyftyf 0:21840c01d3d7 180 #endif // FreeIMU_h
tyftyftyf 0:21840c01d3d7 181