Aloïs Wolff / HK10DOF

mbed implementation of the FreeIMU IMU for HobbyKing's 10DOF board

Dependents:   testHK10DOF

HK10DOF.h@1:85fcfcb7b137, 2013-07-17 (annotated)

Committer:
pommzorz
Date:
Wed Jul 17 18:53:37 2013 +0000
Revision:
1:85fcfcb7b137
Parent:
0:9a1682a09c50 
oops forgot one file...

Who changed what in which revision?

UserRevisionLine numberNew 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
pommzorz 0:9a1682a09c50 24 #ifndef HK10DOF_h
pommzorz 0:9a1682a09c50 25 #define HK10DOF_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"
pommzorz 0:9a1682a09c50 77 #include "L3G4200D.h"
pommzorz 0:9a1682a09c50 78
pommzorz 0:9a1682a09c50 79 // default I2C 7-bit addresses of the sensors
pommzorz 0:9a1682a09c50 80 #define FIMU_ACC_ADDR ADXL345_ADDR_ALT_LOW // SDO connected to GND
pommzorz 0:9a1682a09c50 81 //#define FIMU_ADXL345_DEF_ADDR ADXL345_ADDR_ALT_HIGH // SDO connected to GND
pommzorz 0:9a1682a09c50 82
pommzorz 0:9a1682a09c50 83 #define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW
pommzorz 0:9a1682a09c50 84 #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND
pommzorz 0:9a1682a09c50 85 // HMC5843 address is fixed so don't bother to define it
pommzorz 0:9a1682a09c50 86
pommzorz 0:9a1682a09c50 87 /*
pommzorz 0:9a1682a09c50 88 #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
pommzorz 0:9a1682a09c50 89 #define twoKiDef (2.0f * 0.1f) // 2 * integral gain
pommzorz 0:9a1682a09c50 90 */
pommzorz 0:9a1682a09c50 91 #define twoKpDef (2.0f * 5.0f) // 2 * proportional gain
pommzorz 0:9a1682a09c50 92 #define twoKiDef (2.0f * 0.1f) // 2 * integral gain
pommzorz 0:9a1682a09c50 93
pommzorz 0:9a1682a09c50 94
pommzorz 0:9a1682a09c50 95 #ifndef cbi
pommzorz 0:9a1682a09c50 96 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
pommzorz 0:9a1682a09c50 97 #endif
pommzorz 0:9a1682a09c50 98
pommzorz 0:9a1682a09c50 99 class HK10DOF
pommzorz 0:9a1682a09c50 100 {
pommzorz 0:9a1682a09c50 101 public:
pommzorz 0:9a1682a09c50 102 HK10DOF(PinName sda, PinName scl);
pommzorz 0:9a1682a09c50 103 //I2C i2c_;
pommzorz 0:9a1682a09c50 104
pommzorz 0:9a1682a09c50 105 //void init();
pommzorz 0:9a1682a09c50 106 //void init(bool fastmode);
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;
pommzorz 0:9a1682a09c50 133 L3G4200D gyro;
pommzorz 0:9a1682a09c50 134 Serial pc;
pommzorz 0:9a1682a09c50 135
pommzorz 0:9a1682a09c50 136
pommzorz 0:9a1682a09c50 137
pommzorz 0:9a1682a09c50 138
pommzorz 0:9a1682a09c50 139 int* raw_acc, raw_gyro, raw_magn;
pommzorz 0:9a1682a09c50 140 // calibration parameters
pommzorz 0:9a1682a09c50 141 int16_t gyro_off_x, gyro_off_y, gyro_off_z;
pommzorz 0:9a1682a09c50 142 int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
pommzorz 0:9a1682a09c50 143 float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
pommzorz 0:9a1682a09c50 144
pommzorz 0:9a1682a09c50 145 private:
pommzorz 0:9a1682a09c50 146 Timer update;
pommzorz 0:9a1682a09c50 147 int dt_us;
pommzorz 0:9a1682a09c50 148
pommzorz 0:9a1682a09c50 149 void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
pommzorz 0:9a1682a09c50 150
pommzorz 0:9a1682a09c50 151 //void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az); for 6DOF sensors
pommzorz 0:9a1682a09c50 152
pommzorz 0:9a1682a09c50 153 //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
pommzorz 0:9a1682a09c50 154 float iq0, iq1, iq2, iq3;
pommzorz 0:9a1682a09c50 155 float exInt, eyInt, ezInt; // scaled integral error
pommzorz 0:9a1682a09c50 156 volatile float twoKp; // 2 * proportional gain (Kp)
pommzorz 0:9a1682a09c50 157 volatile float twoKi; // 2 * integral gain (Ki)
pommzorz 0:9a1682a09c50 158 volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
pommzorz 0:9a1682a09c50 159 volatile float integralFBx, integralFBy, integralFBz;
pommzorz 0:9a1682a09c50 160 //unsigned long lastUpdate, now; // sample period expressed in milliseconds
pommzorz 0:9a1682a09c50 161 float sampleFreq; // half the sample period expressed in seconds
pommzorz 0:9a1682a09c50 162
pommzorz 0:9a1682a09c50 163 };
pommzorz 0:9a1682a09c50 164
pommzorz 0:9a1682a09c50 165 float invSqrt(float number);
pommzorz 0:9a1682a09c50 166 void arr3_rad_to_deg(float * arr);
pommzorz 0:9a1682a09c50 167
pommzorz 0:9a1682a09c50 168
pommzorz 0:9a1682a09c50 169
pommzorz 0:9a1682a09c50 170 #endif // FreeIMU_h
pommzorz 0:9a1682a09c50 171