Larry Littlefield / IMU10DOF

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of HK10DOF by Aloïs Wolff

HK10DOF.h@0:9a1682a09c50, 2013-07-17 (annotated)

Committer:
pommzorz
Date:
Wed Jul 17 18:50:28 2013 +0000
Revision:
0:9a1682a09c50
mbed implementation of the FreeIMU imu for the HobbyKing 10DOF board

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