10DOF FreeIMU port for FreeIMU v4 board and GY-86. This library was modified extensively to specifically suit the Mbed platform. Used threads and interrupts to achieve async mode.

Dependencies:   HMC58X3 AK8963 MS561101BA MODI2C MPU9250

Dependents:   MTQuadControl FreeIMU_serial FreeIMU_demo

Port of FreeIMU library from Arduino to Mbed

10DOF FreeIMU port for FreeIMU v4 board and GY-86. This library was modified extensively to specifically suit the Mbed platform. Maximum sampling rate of 500hz can be achieved using this library.

Improvements

Sensor fusion algorithm fast initialization

This library implements the ARHS hot start algorithm, meaning that you can get accurate readings seconds after the algorithm is started, much faster than the Arduino version, where outputs slowly converge to the correct value in about a minute.

Caching

Sensors are read at their maximum output rates. Read values are cached hence multiple consecutive queries will not cause multiple reads.

Fully async

Acc & Gyro reads are performed via timer interrupts. Magnetometer and barometer are read by RTOS thread. No interfering with main program logic.

Usage

Declare a global FreeIMU object like the one below. There should only be one FreeIMU instance existing at a time.

#include "mbed.h"
#include "FreeIMU.h"
FreeIMU imu;

int main(){
    imu.init(true);
}

Then, anywhere in the code, you may call imu.getQ(q) to get the quarternion, where q is an array of 4 floats representing the quarternion structure.

You are recommended to call getQ frequently to keep the filter updated. However, the frequency should not exceed 500hz to avoid redundant calculation. One way to do this is by using the RtosTimer:

void getIMUdata(void const *);     //method definition

//in main
RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)NULL);
IMUTimer.start(2);     //1 / 2ms = 500hz

//getIMUdata function
void getIMUdata(void const *dummy){
    imu.getQ(NULL);
}
Committer:
tyftyftyf
Date:
Wed Apr 18 20:25:07 2018 +0000
Revision:
29:484a501b8674
Parent:
28:de24fce0509a
change to I2C 1

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 20:a2edd3ced80e 27 #include "mbed.h"
tyftyftyf 20:a2edd3ced80e 28
tyftyftyf 18:3f4803a943d3 29 #ifndef I2C_SDA
tyftyftyf 29:484a501b8674 30 #define I2C_SDA p9
tyftyftyf 29:484a501b8674 31 #define I2C_SCL p10
tyftyftyf 18:3f4803a943d3 32 #endif
tyftyftyf 3:f9b100a9aa65 33
tyftyftyf 0:21840c01d3d7 34 // Uncomment the appropriated version of FreeIMU you are using
tyftyftyf 0:21840c01d3d7 35 //#define FREEIMU_v01
tyftyftyf 0:21840c01d3d7 36 //#define FREEIMU_v02
tyftyftyf 0:21840c01d3d7 37 //#define FREEIMU_v03
tyftyftyf 0:21840c01d3d7 38 //#define FREEIMU_v035
tyftyftyf 0:21840c01d3d7 39 //#define FREEIMU_v035_MS
tyftyftyf 0:21840c01d3d7 40 //#define FREEIMU_v035_BMP
tyftyftyf 0:21840c01d3d7 41 #define FREEIMU_v04
tyftyftyf 0:21840c01d3d7 42
tyftyftyf 0:21840c01d3d7 43 // 3rd party boards. Please consider donating or buying a FreeIMU board to support this library development.
tyftyftyf 0:21840c01d3d7 44 //#define SEN_10121 //IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345 SEN-10121 http://www.sparkfun.com/products/10121
tyftyftyf 0:21840c01d3d7 45 //#define SEN_10736 //9 Degrees of Freedom - Razor IMU SEN-10736 http://www.sparkfun.com/products/10736
tyftyftyf 0:21840c01d3d7 46 //#define SEN_10724 //9 Degrees of Freedom - Sensor Stick SEN-10724 http://www.sparkfun.com/products/10724
tyftyftyf 0:21840c01d3d7 47 //#define SEN_10183 //9 Degrees of Freedom - Sensor Stick SEN-10183 http://www.sparkfun.com/products/10183
tyftyftyf 0:21840c01d3d7 48 //#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 18:3f4803a943d3 49 //#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 18:3f4803a943d3 50 #define GEN_MPU9250
tyftyftyf 0:21840c01d3d7 51
tyftyftyf 0:21840c01d3d7 52 // *** No configuration needed below this line ***
tyftyftyf 0:21840c01d3d7 53
tyftyftyf 0:21840c01d3d7 54
tyftyftyf 0:21840c01d3d7 55 #define FREEIMU_LIB_VERSION "20121122"
tyftyftyf 0:21840c01d3d7 56
tyftyftyf 0:21840c01d3d7 57 #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
tyftyftyf 0:21840c01d3d7 58
tyftyftyf 0:21840c01d3d7 59 #if F_CPU == 16000000L
tyftyftyf 0:21840c01d3d7 60 #define FREEIMU_FREQ "16 MHz"
tyftyftyf 0:21840c01d3d7 61 #elif F_CPU == 8000000L
tyftyftyf 0:21840c01d3d7 62 #define FREEIMU_FREQ "8 MHz"
tyftyftyf 0:21840c01d3d7 63 #endif
tyftyftyf 0:21840c01d3d7 64
tyftyftyf 0:21840c01d3d7 65
tyftyftyf 0:21840c01d3d7 66 // board IDs
tyftyftyf 0:21840c01d3d7 67
tyftyftyf 0:21840c01d3d7 68 #if defined(FREEIMU_v04)
tyftyftyf 0:21840c01d3d7 69 #define FREEIMU_ID "FreeIMU v0.4"
tyftyftyf 0:21840c01d3d7 70 #endif
tyftyftyf 0:21840c01d3d7 71
tyftyftyf 0:21840c01d3d7 72
tyftyftyf 20:a2edd3ced80e 73 #define HAS_MPU6050() (defined(GEN_MPU6050))
tyftyftyf 20:a2edd3ced80e 74 #define HAS_MPU9250() (defined(GEN_MPU9250))
tyftyftyf 0:21840c01d3d7 75
tyftyftyf 0:21840c01d3d7 76 #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
tyftyftyf 0:21840c01d3d7 77 #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 78
tyftyftyf 0:21840c01d3d7 79
tyftyftyf 0:21840c01d3d7 80
tyftyftyf 0:21840c01d3d7 81 //#include <Wire.h>
tyftyftyf 0:21840c01d3d7 82
tyftyftyf 0:21840c01d3d7 83 #include "mbed.h"
tyftyftyf 0:21840c01d3d7 84 #include "calibration.h"
tyftyftyf 0:21840c01d3d7 85 /*
tyftyftyf 0:21840c01d3d7 86 #ifndef CALIBRATION_H
tyftyftyf 0:21840c01d3d7 87 #include <EEPROM.h>
tyftyftyf 0:21840c01d3d7 88 #endif
tyftyftyf 0:21840c01d3d7 89
tyftyftyf 0:21840c01d3d7 90 #define FREEIMU_EEPROM_BASE 0x0A
tyftyftyf 0:21840c01d3d7 91 #define FREEIMU_EEPROM_SIGNATURE 0x19
tyftyftyf 0:21840c01d3d7 92 */
tyftyftyf 0:21840c01d3d7 93 //#if FREEIMU_VER <= 3
tyftyftyf 0:21840c01d3d7 94
tyftyftyf 0:21840c01d3d7 95 #if HAS_MPU6050()
tyftyftyf 0:21840c01d3d7 96 // #include <Wire.h>
tyftyftyf 0:21840c01d3d7 97 #include "I2Cdev.h"
tyftyftyf 0:21840c01d3d7 98 #include "MPU6050.h"
tyftyftyf 0:21840c01d3d7 99 #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS
tyftyftyf 18:3f4803a943d3 100 #include <HMC58X3.h>
tyftyftyf 0:21840c01d3d7 101 #endif
tyftyftyf 0:21840c01d3d7 102
tyftyftyf 18:3f4803a943d3 103 #if HAS_MPU9250()
tyftyftyf 18:3f4803a943d3 104 // #include <Wire.h>
tyftyftyf 18:3f4803a943d3 105 #include "I2Cdev.h"
tyftyftyf 18:3f4803a943d3 106 #include "MPU6050.h"
tyftyftyf 18:3f4803a943d3 107 #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS
tyftyftyf 18:3f4803a943d3 108 #endif
tyftyftyf 18:3f4803a943d3 109
tyftyftyf 18:3f4803a943d3 110
tyftyftyf 0:21840c01d3d7 111 #include <MS561101BA.h>
tyftyftyf 0:21840c01d3d7 112
tyftyftyf 0:21840c01d3d7 113
tyftyftyf 0:21840c01d3d7 114 #define FIMU_BARO_ADDR MS561101BA_ADDR_CSB_LOW
tyftyftyf 0:21840c01d3d7 115
tyftyftyf 0:21840c01d3d7 116
tyftyftyf 0:21840c01d3d7 117 #define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW
tyftyftyf 0:21840c01d3d7 118 #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND
tyftyftyf 0:21840c01d3d7 119 // HMC5843 address is fixed so don't bother to define it
tyftyftyf 0:21840c01d3d7 120
tyftyftyf 18:3f4803a943d3 121 #if defined(DFROBOT)
tyftyftyf 18:3f4803a943d3 122 #define twoKpDef (2.0f * 0.5f)
tyftyftyf 18:3f4803a943d3 123 #define twoKiDef (2.0f * 0.00002f)
tyftyftyf 18:3f4803a943d3 124 #define betaDef 0.1f
tyftyftyf 18:3f4803a943d3 125 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 126 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 127 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 128 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 129 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 130 #elif defined(FREEIMU_v04)
tyftyftyf 18:3f4803a943d3 131 #define twoKpDef (2.0f * 0.75f) //works with and without mag enabled
tyftyftyf 18:3f4803a943d3 132 #define twoKiDef (2.0f * 0.1625f)
tyftyftyf 18:3f4803a943d3 133 #define betaDef 0.085f
tyftyftyf 18:3f4803a943d3 134 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 135 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 136 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 137 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 138 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 139 #elif defined(GEN_MPU6050)
tyftyftyf 18:3f4803a943d3 140 #define twoKpDef (2.0f * 0.5f)
tyftyftyf 18:3f4803a943d3 141 #define twoKiDef (2.0f * 0.25f)
tyftyftyf 18:3f4803a943d3 142 #define betaDef 0.2f
tyftyftyf 18:3f4803a943d3 143 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 144 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 145 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 146 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 147 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 148 #elif defined(GEN_MPU9150)
tyftyftyf 18:3f4803a943d3 149 #define twoKpDef (2.0f * 0.75f)
tyftyftyf 18:3f4803a943d3 150 #define twoKiDef (2.0f * 0.1f)
tyftyftyf 18:3f4803a943d3 151 #define betaDef 0.01f
tyftyftyf 18:3f4803a943d3 152 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 153 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 154 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 155 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 156 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 157 #elif defined(Altimu10)
tyftyftyf 18:3f4803a943d3 158 //#define twoKpDef (2.0f * 1.01f)
tyftyftyf 18:3f4803a943d3 159 //#define twoKiDef (2.0f * 0.00002f)
tyftyftyf 18:3f4803a943d3 160 #define twoKpDef (2.0f * 2.75f)
tyftyftyf 18:3f4803a943d3 161 #define twoKiDef (2.0f * 0.1625f)
tyftyftyf 18:3f4803a943d3 162 #define betaDef 2.0f
tyftyftyf 18:3f4803a943d3 163 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 164 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 165 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 166 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 167 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 168 #elif defined(GEN_MPU9250) || defined(MPU9250_5611) || defined(MPU9250_5637)
tyftyftyf 28:de24fce0509a 169 #define twoKpDef (2.0f * 0.95f) // was 0.95
tyftyftyf 18:3f4803a943d3 170 #define twoKiDef (2.0f * 0.05f) // was 0.05
tyftyftyf 18:3f4803a943d3 171 #define betaDef 0.515f
tyftyftyf 18:3f4803a943d3 172 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 173 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 174 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 175 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 176 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 177 #elif defined(APM_2_5)
tyftyftyf 18:3f4803a943d3 178 #define twoKpDef (2.0f * 0.5f)
tyftyftyf 18:3f4803a943d3 179 #define twoKiDef (2.0f * 0.25f)
tyftyftyf 18:3f4803a943d3 180 #define betaDef 0.015f //was 0.015
tyftyftyf 18:3f4803a943d3 181 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 182 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 183 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 184 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 185 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 186 #elif defined(Microduino)
tyftyftyf 18:3f4803a943d3 187 #define twoKpDef (2.0f * 1.75f) //works with and without mag enabled, 1.75
tyftyftyf 18:3f4803a943d3 188 #define twoKiDef (2.0f * 0.0075f) //.1625f
tyftyftyf 18:3f4803a943d3 189 #define betaDef 0.015f
tyftyftyf 18:3f4803a943d3 190 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 191 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 192 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 193 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 194 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 195 #elif (defined(ST_LSM9DS0) || defined(LSM9DS0_MS5637))
tyftyftyf 18:3f4803a943d3 196 //Madgwick's implementation of Mayhony's AHRS algorithm (option 0)
tyftyftyf 18:3f4803a943d3 197 #define twoKpDef (2.0f * 1.75f) //works with and without mag enabled
tyftyftyf 18:3f4803a943d3 198 #define twoKiDef (2.0f * 0.025f)
tyftyftyf 18:3f4803a943d3 199 //Implementation of Madgwick's IMU and AHRS algorithms (option 1)
tyftyftyf 18:3f4803a943d3 200 #define betaDef 0.15f
tyftyftyf 18:3f4803a943d3 201 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 202 const float Kp_ROLLPITCH = 1.2f; //was .3423, was 1.2(2/16/17)
tyftyftyf 18:3f4803a943d3 203 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 204 const float Kp_YAW = 1.2f; // was 1.2 and 0.02, was 1.2 (2/16/17)
tyftyftyf 18:3f4803a943d3 205 const float Ki_YAW = 0.02f;
tyftyftyf 18:3f4803a943d3 206 #elif defined(ST_LSM9DS1) || defined(ST_LSM9DS1_MS5611)
tyftyftyf 18:3f4803a943d3 207 //Madgwick's implementation of Mayhony's AHRS algorithm
tyftyftyf 18:3f4803a943d3 208 #define twoKpDef (2.0f * 1.75f) //works with and without mag enabled
tyftyftyf 18:3f4803a943d3 209 #define twoKiDef (2.0f * 0.025f)
tyftyftyf 18:3f4803a943d3 210 //Implementation of Madgwick's IMU and AHRS algorithms
tyftyftyf 18:3f4803a943d3 211 #define betaDef 0.15f
tyftyftyf 18:3f4803a943d3 212 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 213 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 214 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 215 const float Kp_YAW = 1.2f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 216 const float Ki_YAW = 0.02f;
tyftyftyf 18:3f4803a943d3 217 #elif defined(CurieImu) || defined(CurieIMU_Mag)
tyftyftyf 18:3f4803a943d3 218 #define twoKpDef (2.0f * 1.25f)
tyftyftyf 18:3f4803a943d3 219 #define twoKiDef (2.0f * 0.1f)
tyftyftyf 18:3f4803a943d3 220 #define betaDef 0.07f
tyftyftyf 18:3f4803a943d3 221 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 222 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 223 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 224 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 225 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 226 #elif defined(PropShield)
tyftyftyf 18:3f4803a943d3 227 #define twoKpDef (2.0f * 4.25f) // was 0.95, 3.25
tyftyftyf 18:3f4803a943d3 228 #define twoKiDef (2.0f * 0.25) // was 0.05 , 0.25
tyftyftyf 18:3f4803a943d3 229 #define betaDef 0.35
tyftyftyf 18:3f4803a943d3 230 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 231 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 232 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 233 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 234 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 235 #else
tyftyftyf 18:3f4803a943d3 236 #define twoKpDef (2.0f * 0.5f)
tyftyftyf 18:3f4803a943d3 237 #define twoKiDef (2.0f * 0.1f)
tyftyftyf 18:3f4803a943d3 238 #define betaDef 0.1f
tyftyftyf 18:3f4803a943d3 239 //Used for DCM filter
tyftyftyf 18:3f4803a943d3 240 const float Kp_ROLLPITCH = 1.2f; //was .3423
tyftyftyf 18:3f4803a943d3 241 const float Ki_ROLLPITCH = 0.0234f;
tyftyftyf 18:3f4803a943d3 242 const float Kp_YAW = 1.75f; // was 1.2 and 0.02
tyftyftyf 18:3f4803a943d3 243 const float Ki_YAW = 0.002f;
tyftyftyf 18:3f4803a943d3 244 #endif
tyftyftyf 0:21840c01d3d7 245
tyftyftyf 0:21840c01d3d7 246 #ifndef cbi
tyftyftyf 0:21840c01d3d7 247 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
tyftyftyf 0:21840c01d3d7 248 #endif
tyftyftyf 0:21840c01d3d7 249
tyftyftyf 0:21840c01d3d7 250 class FreeIMU
tyftyftyf 0:21840c01d3d7 251 {
tyftyftyf 0:21840c01d3d7 252 public:
tyftyftyf 0:21840c01d3d7 253 FreeIMU();
tyftyftyf 0:21840c01d3d7 254 void init();
tyftyftyf 0:21840c01d3d7 255 void init(bool fastmode);
tyftyftyf 0:21840c01d3d7 256
tyftyftyf 0:21840c01d3d7 257 void init(int accgyro_addr, bool fastmode);
joe4465 13:21b275eeeda2 258 void sample(bool sampling);
tyftyftyf 0:21840c01d3d7 259
tyftyftyf 0:21840c01d3d7 260 #ifndef CALIBRATION_H
tyftyftyf 0:21840c01d3d7 261 void calLoad();
tyftyftyf 0:21840c01d3d7 262 #endif
tyftyftyf 0:21840c01d3d7 263 void zeroGyro();
tyftyftyf 0:21840c01d3d7 264 void getRawValues(int16_t * raw_values);
tyftyftyf 0:21840c01d3d7 265 void getValues(float * values);
tyftyftyf 0:21840c01d3d7 266 void getQ(float * q);
joe4465 13:21b275eeeda2 267 void getRate(float * r);
tyftyftyf 0:21840c01d3d7 268 void getEuler(float * angles);
tyftyftyf 0:21840c01d3d7 269 void getYawPitchRoll(float * ypr);
tyftyftyf 0:21840c01d3d7 270 void getEulerRad(float * angles);
tyftyftyf 0:21840c01d3d7 271 void getYawPitchRollRad(float * ypr);
tyftyftyf 0:21840c01d3d7 272 void gravityCompensateAcc(float * acc, float * q);
tyftyftyf 0:21840c01d3d7 273 float getRawPressure();
tyftyftyf 0:21840c01d3d7 274
tyftyftyf 0:21840c01d3d7 275 float getBaroAlt();
tyftyftyf 0:21840c01d3d7 276 float getBaroAlt(float sea_press);
tyftyftyf 0:21840c01d3d7 277
tyftyftyf 2:5c419926dcd7 278 void getQ_simple(float* q);
tyftyftyf 2:5c419926dcd7 279
tyftyftyf 0:21840c01d3d7 280 // we make them public so that users can interact directly with device classes
tyftyftyf 0:21840c01d3d7 281
tyftyftyf 3:f9b100a9aa65 282 MPU6050 *accgyro;
tyftyftyf 3:f9b100a9aa65 283 MS561101BA *baro;
tyftyftyf 18:3f4803a943d3 284
tyftyftyf 18:3f4803a943d3 285 #if HAS_MPU6050()
tyftyftyf 3:f9b100a9aa65 286 HMC58X3 *magn;
tyftyftyf 18:3f4803a943d3 287 #endif
tyftyftyf 0:21840c01d3d7 288
tyftyftyf 0:21840c01d3d7 289 int* raw_acc, raw_gyro, raw_magn;
tyftyftyf 0:21840c01d3d7 290 // calibration parameters
tyftyftyf 0:21840c01d3d7 291 int16_t gyro_off_x, gyro_off_y, gyro_off_z;
tyftyftyf 0:21840c01d3d7 292 int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
tyftyftyf 0:21840c01d3d7 293 float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
tyftyftyf 0:21840c01d3d7 294
tyftyftyf 0:21840c01d3d7 295 private:
tyftyftyf 0:21840c01d3d7 296
tyftyftyf 3:f9b100a9aa65 297 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 298
tyftyftyf 0:21840c01d3d7 299 //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
tyftyftyf 0:21840c01d3d7 300 float iq0, iq1, iq2, iq3;
tyftyftyf 0:21840c01d3d7 301 float exInt, eyInt, ezInt; // scaled integral error
tyftyftyf 0:21840c01d3d7 302 volatile float twoKp; // 2 * proportional gain (Kp)
tyftyftyf 0:21840c01d3d7 303 volatile float twoKi; // 2 * integral gain (Ki)
tyftyftyf 6:6b1185b32814 304 volatile float twoKiz, twoKpz;
tyftyftyf 0:21840c01d3d7 305 volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
tyftyftyf 0:21840c01d3d7 306 volatile float integralFBx, integralFBy, integralFBz;
tyftyftyf 0:21840c01d3d7 307 Timer update;
tyftyftyf 0:21840c01d3d7 308 int dt_us;
tyftyftyf 0:21840c01d3d7 309 //unsigned long lastUpdate, now; // sample period expressed in milliseconds
tyftyftyf 0:21840c01d3d7 310 float sampleFreq; // half the sample period expressed in seconds
tyftyftyf 2:5c419926dcd7 311
tyftyftyf 0:21840c01d3d7 312 };
tyftyftyf 0:21840c01d3d7 313
tyftyftyf 0:21840c01d3d7 314 float invSqrt(float number);
tyftyftyf 0:21840c01d3d7 315 void arr3_rad_to_deg(float * arr);
tyftyftyf 0:21840c01d3d7 316
tyftyftyf 0:21840c01d3d7 317
tyftyftyf 0:21840c01d3d7 318
tyftyftyf 0:21840c01d3d7 319 #endif // FreeIMU_h
tyftyftyf 0:21840c01d3d7 320