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 Mar 28 20:26:02 2018 +0000
Revision:
18:3f4803a943d3
Parent:
13:21b275eeeda2
Child:
20:a2edd3ced80e
wip

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