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); }
FreeIMU.cpp@0:21840c01d3d7, 2013-11-02 (annotated)
- Committer:
- tyftyftyf
- Date:
- Sat Nov 02 17:25:51 2013 +0000
- Revision:
- 0:21840c01d3d7
- Child:
- 1:794e9cdbc2a0
Initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tyftyftyf | 0:21840c01d3d7 | 1 | /* |
tyftyftyf | 0:21840c01d3d7 | 2 | FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino |
tyftyftyf | 0:21840c01d3d7 | 3 | Copyright (C) 2011-2012 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 | 02/20/2013 - Modified by Aloïs Wolff for MBED with MPU6050 only (wolffalois@gmail.com) |
tyftyftyf | 0:21840c01d3d7 | 23 | */ |
tyftyftyf | 0:21840c01d3d7 | 24 | |
tyftyftyf | 0:21840c01d3d7 | 25 | //#include <inttypes.h> |
tyftyftyf | 0:21840c01d3d7 | 26 | //#include <stdint.h> |
tyftyftyf | 0:21840c01d3d7 | 27 | //#define DEBUG |
tyftyftyf | 0:21840c01d3d7 | 28 | #include "FreeIMU.h" |
tyftyftyf | 0:21840c01d3d7 | 29 | #define M_PI 3.1415926535897932384626433832795 |
tyftyftyf | 0:21840c01d3d7 | 30 | |
tyftyftyf | 0:21840c01d3d7 | 31 | #ifdef DEBUG |
tyftyftyf | 0:21840c01d3d7 | 32 | #define DEBUG_PRINT(x) Serial.println(x) |
tyftyftyf | 0:21840c01d3d7 | 33 | #else |
tyftyftyf | 0:21840c01d3d7 | 34 | #define DEBUG_PRINT(x) |
tyftyftyf | 0:21840c01d3d7 | 35 | #endif |
tyftyftyf | 0:21840c01d3d7 | 36 | // #include "WireUtils.h" |
tyftyftyf | 0:21840c01d3d7 | 37 | //#include "DebugUtils.h" |
tyftyftyf | 0:21840c01d3d7 | 38 | |
tyftyftyf | 0:21840c01d3d7 | 39 | //#include "vector_math.h" |
tyftyftyf | 0:21840c01d3d7 | 40 | |
tyftyftyf | 0:21840c01d3d7 | 41 | I2C i2c(I2C_SDA,I2C_SCL); |
tyftyftyf | 0:21840c01d3d7 | 42 | |
tyftyftyf | 0:21840c01d3d7 | 43 | FreeIMU::FreeIMU() { |
tyftyftyf | 0:21840c01d3d7 | 44 | |
tyftyftyf | 0:21840c01d3d7 | 45 | i2c.frequency(400000); |
tyftyftyf | 0:21840c01d3d7 | 46 | accgyro = MPU6050(i2c); // I2C |
tyftyftyf | 0:21840c01d3d7 | 47 | magn = HMC58X3(i2c); |
tyftyftyf | 0:21840c01d3d7 | 48 | baro = MS561101BA(i2c); |
tyftyftyf | 0:21840c01d3d7 | 49 | |
tyftyftyf | 0:21840c01d3d7 | 50 | // initialize quaternion |
tyftyftyf | 0:21840c01d3d7 | 51 | q0 = 1.0f; |
tyftyftyf | 0:21840c01d3d7 | 52 | q1 = 0.0f; |
tyftyftyf | 0:21840c01d3d7 | 53 | q2 = 0.0f; |
tyftyftyf | 0:21840c01d3d7 | 54 | q3 = 0.0f; |
tyftyftyf | 0:21840c01d3d7 | 55 | exInt = 0.0; |
tyftyftyf | 0:21840c01d3d7 | 56 | eyInt = 0.0; |
tyftyftyf | 0:21840c01d3d7 | 57 | ezInt = 0.0; |
tyftyftyf | 0:21840c01d3d7 | 58 | twoKp = twoKpDef; |
tyftyftyf | 0:21840c01d3d7 | 59 | twoKi = twoKiDef; |
tyftyftyf | 0:21840c01d3d7 | 60 | integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; |
tyftyftyf | 0:21840c01d3d7 | 61 | |
tyftyftyf | 0:21840c01d3d7 | 62 | |
tyftyftyf | 0:21840c01d3d7 | 63 | update.start(); |
tyftyftyf | 0:21840c01d3d7 | 64 | dt_us=0; |
tyftyftyf | 0:21840c01d3d7 | 65 | /* |
tyftyftyf | 0:21840c01d3d7 | 66 | lastUpdate = 0; |
tyftyftyf | 0:21840c01d3d7 | 67 | now = 0; |
tyftyftyf | 0:21840c01d3d7 | 68 | */ |
tyftyftyf | 0:21840c01d3d7 | 69 | #ifndef CALIBRATION_H |
tyftyftyf | 0:21840c01d3d7 | 70 | // initialize scale factors to neutral values |
tyftyftyf | 0:21840c01d3d7 | 71 | acc_scale_x = 1; |
tyftyftyf | 0:21840c01d3d7 | 72 | acc_scale_y = 1; |
tyftyftyf | 0:21840c01d3d7 | 73 | acc_scale_z = 1; |
tyftyftyf | 0:21840c01d3d7 | 74 | magn_scale_x = 1; |
tyftyftyf | 0:21840c01d3d7 | 75 | magn_scale_y = 1; |
tyftyftyf | 0:21840c01d3d7 | 76 | magn_scale_z = 1; |
tyftyftyf | 0:21840c01d3d7 | 77 | #else |
tyftyftyf | 0:21840c01d3d7 | 78 | // get values from global variables of same name defined in calibration.h |
tyftyftyf | 0:21840c01d3d7 | 79 | acc_off_x = ::acc_off_x; |
tyftyftyf | 0:21840c01d3d7 | 80 | acc_off_y = ::acc_off_y; |
tyftyftyf | 0:21840c01d3d7 | 81 | acc_off_z = ::acc_off_z; |
tyftyftyf | 0:21840c01d3d7 | 82 | acc_scale_x = ::acc_scale_x; |
tyftyftyf | 0:21840c01d3d7 | 83 | acc_scale_y = ::acc_scale_y; |
tyftyftyf | 0:21840c01d3d7 | 84 | acc_scale_z = ::acc_scale_z; |
tyftyftyf | 0:21840c01d3d7 | 85 | magn_off_x = ::magn_off_x; |
tyftyftyf | 0:21840c01d3d7 | 86 | magn_off_y = ::magn_off_y; |
tyftyftyf | 0:21840c01d3d7 | 87 | magn_off_z = ::magn_off_z; |
tyftyftyf | 0:21840c01d3d7 | 88 | magn_scale_x = ::magn_scale_x; |
tyftyftyf | 0:21840c01d3d7 | 89 | magn_scale_y = ::magn_scale_y; |
tyftyftyf | 0:21840c01d3d7 | 90 | magn_scale_z = ::magn_scale_z; |
tyftyftyf | 0:21840c01d3d7 | 91 | #endif |
tyftyftyf | 0:21840c01d3d7 | 92 | } |
tyftyftyf | 0:21840c01d3d7 | 93 | |
tyftyftyf | 0:21840c01d3d7 | 94 | void FreeIMU::init() { |
tyftyftyf | 0:21840c01d3d7 | 95 | |
tyftyftyf | 0:21840c01d3d7 | 96 | init(FIMU_ACCGYRO_ADDR, false); |
tyftyftyf | 0:21840c01d3d7 | 97 | |
tyftyftyf | 0:21840c01d3d7 | 98 | } |
tyftyftyf | 0:21840c01d3d7 | 99 | |
tyftyftyf | 0:21840c01d3d7 | 100 | void FreeIMU::init(bool fastmode) { |
tyftyftyf | 0:21840c01d3d7 | 101 | |
tyftyftyf | 0:21840c01d3d7 | 102 | init(FIMU_ACCGYRO_ADDR, fastmode); |
tyftyftyf | 0:21840c01d3d7 | 103 | |
tyftyftyf | 0:21840c01d3d7 | 104 | } |
tyftyftyf | 0:21840c01d3d7 | 105 | |
tyftyftyf | 0:21840c01d3d7 | 106 | |
tyftyftyf | 0:21840c01d3d7 | 107 | /** |
tyftyftyf | 0:21840c01d3d7 | 108 | * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration |
tyftyftyf | 0:21840c01d3d7 | 109 | */ |
tyftyftyf | 0:21840c01d3d7 | 110 | |
tyftyftyf | 0:21840c01d3d7 | 111 | void FreeIMU::init(int accgyro_addr, bool fastmode) { |
tyftyftyf | 0:21840c01d3d7 | 112 | |
tyftyftyf | 0:21840c01d3d7 | 113 | wait_ms(5); |
tyftyftyf | 0:21840c01d3d7 | 114 | /* |
tyftyftyf | 0:21840c01d3d7 | 115 | // disable internal pullups of the ATMEGA which Wire enable by default |
tyftyftyf | 0:21840c01d3d7 | 116 | #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) |
tyftyftyf | 0:21840c01d3d7 | 117 | // deactivate internal pull-ups for twi |
tyftyftyf | 0:21840c01d3d7 | 118 | // as per note from atmega8 manual pg167 |
tyftyftyf | 0:21840c01d3d7 | 119 | cbi(PORTC, 4); |
tyftyftyf | 0:21840c01d3d7 | 120 | cbi(PORTC, 5); |
tyftyftyf | 0:21840c01d3d7 | 121 | #else |
tyftyftyf | 0:21840c01d3d7 | 122 | // deactivate internal pull-ups for twi |
tyftyftyf | 0:21840c01d3d7 | 123 | // as per note from atmega128 manual pg204 |
tyftyftyf | 0:21840c01d3d7 | 124 | cbi(PORTD, 0); |
tyftyftyf | 0:21840c01d3d7 | 125 | cbi(PORTD, 1); |
tyftyftyf | 0:21840c01d3d7 | 126 | #endif |
tyftyftyf | 0:21840c01d3d7 | 127 | */ |
tyftyftyf | 0:21840c01d3d7 | 128 | |
tyftyftyf | 0:21840c01d3d7 | 129 | /* |
tyftyftyf | 0:21840c01d3d7 | 130 | if(fastmode) { // switch to 400KHz I2C - eheheh |
tyftyftyf | 0:21840c01d3d7 | 131 | TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c |
tyftyftyf | 0:21840c01d3d7 | 132 | } |
tyftyftyf | 0:21840c01d3d7 | 133 | */ |
tyftyftyf | 0:21840c01d3d7 | 134 | //accgyro = MPU6050(false, accgyro_addr); |
tyftyftyf | 0:21840c01d3d7 | 135 | accgyro = MPU6050(0x68); |
tyftyftyf | 0:21840c01d3d7 | 136 | accgyro.initialize(); |
tyftyftyf | 0:21840c01d3d7 | 137 | accgyro.setI2CMasterModeEnabled(0); |
tyftyftyf | 0:21840c01d3d7 | 138 | accgyro.setI2CBypassEnabled(1); |
tyftyftyf | 0:21840c01d3d7 | 139 | accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); |
tyftyftyf | 0:21840c01d3d7 | 140 | wait_ms(5); |
tyftyftyf | 0:21840c01d3d7 | 141 | |
tyftyftyf | 0:21840c01d3d7 | 142 | |
tyftyftyf | 0:21840c01d3d7 | 143 | // init HMC5843 |
tyftyftyf | 0:21840c01d3d7 | 144 | magn.init(false); // Don't set mode yet, we'll do that later on. |
tyftyftyf | 0:21840c01d3d7 | 145 | // Calibrate HMC using self test, not recommended to change the gain after calibration. |
tyftyftyf | 0:21840c01d3d7 | 146 | magn.calibrate(1); // Use gain 1=default, valid 0-7, 7 not recommended. |
tyftyftyf | 0:21840c01d3d7 | 147 | // Single mode conversion was used in calibration, now set continuous mode |
tyftyftyf | 0:21840c01d3d7 | 148 | magn.setMode(0); |
tyftyftyf | 0:21840c01d3d7 | 149 | wait_ms(10); |
tyftyftyf | 0:21840c01d3d7 | 150 | magn.setDOR(6); |
tyftyftyf | 0:21840c01d3d7 | 151 | |
tyftyftyf | 0:21840c01d3d7 | 152 | baro.init(FIMU_BARO_ADDR); |
tyftyftyf | 0:21840c01d3d7 | 153 | |
tyftyftyf | 0:21840c01d3d7 | 154 | // zero gyro |
tyftyftyf | 0:21840c01d3d7 | 155 | zeroGyro(); |
tyftyftyf | 0:21840c01d3d7 | 156 | |
tyftyftyf | 0:21840c01d3d7 | 157 | #ifndef CALIBRATION_H |
tyftyftyf | 0:21840c01d3d7 | 158 | // load calibration from eeprom |
tyftyftyf | 0:21840c01d3d7 | 159 | calLoad(); |
tyftyftyf | 0:21840c01d3d7 | 160 | #endif |
tyftyftyf | 0:21840c01d3d7 | 161 | getQ_simple(NULL); |
tyftyftyf | 0:21840c01d3d7 | 162 | } |
tyftyftyf | 0:21840c01d3d7 | 163 | |
tyftyftyf | 0:21840c01d3d7 | 164 | void FreeIMU::getQ_simple(float* q) |
tyftyftyf | 0:21840c01d3d7 | 165 | { |
tyftyftyf | 0:21840c01d3d7 | 166 | float values[9]; |
tyftyftyf | 0:21840c01d3d7 | 167 | getValues(values); |
tyftyftyf | 0:21840c01d3d7 | 168 | |
tyftyftyf | 0:21840c01d3d7 | 169 | float pitch = atan2(values[0], sqrt(values[1]*values[1]+values[2]*values[2])); |
tyftyftyf | 0:21840c01d3d7 | 170 | float roll = -atan2(values[1], sqrt(values[0]*values[0]+values[2]*values[2])); |
tyftyftyf | 0:21840c01d3d7 | 171 | |
tyftyftyf | 0:21840c01d3d7 | 172 | float xh = values[6]*cos(pitch)+values[7]*sin(roll)*sin(pitch)-values[8]*cos(roll)*sin(pitch); |
tyftyftyf | 0:21840c01d3d7 | 173 | float yh = values[7]*cos(roll)+values[8]*sin(roll); |
tyftyftyf | 0:21840c01d3d7 | 174 | float yaw = atan2(yh, xh); |
tyftyftyf | 0:21840c01d3d7 | 175 | |
tyftyftyf | 0:21840c01d3d7 | 176 | float rollOver2 = roll * 0.5f; |
tyftyftyf | 0:21840c01d3d7 | 177 | float sinRollOver2 = (float)sin(rollOver2); |
tyftyftyf | 0:21840c01d3d7 | 178 | float cosRollOver2 = (float)cos(rollOver2); |
tyftyftyf | 0:21840c01d3d7 | 179 | float pitchOver2 = pitch * 0.5f; |
tyftyftyf | 0:21840c01d3d7 | 180 | float sinPitchOver2 = (float)sin(pitchOver2); |
tyftyftyf | 0:21840c01d3d7 | 181 | float cosPitchOver2 = (float)cos(pitchOver2); |
tyftyftyf | 0:21840c01d3d7 | 182 | float yawOver2 = yaw * 0.5f; |
tyftyftyf | 0:21840c01d3d7 | 183 | float sinYawOver2 = (float)sin(yawOver2); |
tyftyftyf | 0:21840c01d3d7 | 184 | float cosYawOver2 = (float)cos(yawOver2); |
tyftyftyf | 0:21840c01d3d7 | 185 | |
tyftyftyf | 0:21840c01d3d7 | 186 | q0 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; |
tyftyftyf | 0:21840c01d3d7 | 187 | q1 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; |
tyftyftyf | 0:21840c01d3d7 | 188 | q2 = - cosYawOver2 * cosPitchOver2 * cosRollOver2 - sinYawOver2 * sinPitchOver2 * sinRollOver2; |
tyftyftyf | 0:21840c01d3d7 | 189 | q3 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; |
tyftyftyf | 0:21840c01d3d7 | 190 | |
tyftyftyf | 0:21840c01d3d7 | 191 | if (q!=NULL){ |
tyftyftyf | 0:21840c01d3d7 | 192 | q[0] = q0; |
tyftyftyf | 0:21840c01d3d7 | 193 | q[1] = q1; |
tyftyftyf | 0:21840c01d3d7 | 194 | q[2] = q2; |
tyftyftyf | 0:21840c01d3d7 | 195 | q[3] = q3; |
tyftyftyf | 0:21840c01d3d7 | 196 | } |
tyftyftyf | 0:21840c01d3d7 | 197 | } |
tyftyftyf | 0:21840c01d3d7 | 198 | /* |
tyftyftyf | 0:21840c01d3d7 | 199 | #ifndef CALIBRATION_H |
tyftyftyf | 0:21840c01d3d7 | 200 | |
tyftyftyf | 0:21840c01d3d7 | 201 | static uint8_t location; // assuming ordered reads |
tyftyftyf | 0:21840c01d3d7 | 202 | |
tyftyftyf | 0:21840c01d3d7 | 203 | void eeprom_read_var(uint8_t size, byte * var) { |
tyftyftyf | 0:21840c01d3d7 | 204 | for(uint8_t i = 0; i<size; i++) { |
tyftyftyf | 0:21840c01d3d7 | 205 | var[i] = EEPROM.read(location + i); |
tyftyftyf | 0:21840c01d3d7 | 206 | } |
tyftyftyf | 0:21840c01d3d7 | 207 | location += size; |
tyftyftyf | 0:21840c01d3d7 | 208 | } |
tyftyftyf | 0:21840c01d3d7 | 209 | */ |
tyftyftyf | 0:21840c01d3d7 | 210 | |
tyftyftyf | 0:21840c01d3d7 | 211 | |
tyftyftyf | 0:21840c01d3d7 | 212 | /** |
tyftyftyf | 0:21840c01d3d7 | 213 | * Populates raw_values with the raw_values from the sensors |
tyftyftyf | 0:21840c01d3d7 | 214 | */ |
tyftyftyf | 0:21840c01d3d7 | 215 | void FreeIMU::getRawValues(int16_t * raw_values) { |
tyftyftyf | 0:21840c01d3d7 | 216 | |
tyftyftyf | 0:21840c01d3d7 | 217 | accgyro.getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]); |
tyftyftyf | 0:21840c01d3d7 | 218 | magn.getValues(&raw_values[6], &raw_values[7], &raw_values[8]); |
tyftyftyf | 0:21840c01d3d7 | 219 | |
tyftyftyf | 0:21840c01d3d7 | 220 | int temp, press; |
tyftyftyf | 0:21840c01d3d7 | 221 | //TODO: possible loss of precision |
tyftyftyf | 0:21840c01d3d7 | 222 | temp = baro.rawTemperature(MS561101BA_OSR_4096); |
tyftyftyf | 0:21840c01d3d7 | 223 | raw_values[9] = temp; |
tyftyftyf | 0:21840c01d3d7 | 224 | press = baro.rawPressure(MS561101BA_OSR_4096); |
tyftyftyf | 0:21840c01d3d7 | 225 | raw_values[10] = press; |
tyftyftyf | 0:21840c01d3d7 | 226 | } |
tyftyftyf | 0:21840c01d3d7 | 227 | |
tyftyftyf | 0:21840c01d3d7 | 228 | |
tyftyftyf | 0:21840c01d3d7 | 229 | /** |
tyftyftyf | 0:21840c01d3d7 | 230 | * Populates values with calibrated readings from the sensors |
tyftyftyf | 0:21840c01d3d7 | 231 | */ |
tyftyftyf | 0:21840c01d3d7 | 232 | void FreeIMU::getValues(float * values) { |
tyftyftyf | 0:21840c01d3d7 | 233 | |
tyftyftyf | 0:21840c01d3d7 | 234 | // MPU6050 |
tyftyftyf | 0:21840c01d3d7 | 235 | int16_t accgyroval[6]; |
tyftyftyf | 0:21840c01d3d7 | 236 | accgyro.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]); |
tyftyftyf | 0:21840c01d3d7 | 237 | |
tyftyftyf | 0:21840c01d3d7 | 238 | // remove offsets from the gyroscope |
tyftyftyf | 0:21840c01d3d7 | 239 | accgyroval[3] = accgyroval[3] - gyro_off_x; |
tyftyftyf | 0:21840c01d3d7 | 240 | accgyroval[4] = accgyroval[4] - gyro_off_y; |
tyftyftyf | 0:21840c01d3d7 | 241 | accgyroval[5] = accgyroval[5] - gyro_off_z; |
tyftyftyf | 0:21840c01d3d7 | 242 | |
tyftyftyf | 0:21840c01d3d7 | 243 | for(int i = 0; i<6; i++) { |
tyftyftyf | 0:21840c01d3d7 | 244 | if(i < 3) { |
tyftyftyf | 0:21840c01d3d7 | 245 | values[i] = (float) accgyroval[i]; |
tyftyftyf | 0:21840c01d3d7 | 246 | } |
tyftyftyf | 0:21840c01d3d7 | 247 | else { |
tyftyftyf | 0:21840c01d3d7 | 248 | values[i] = ((float) accgyroval[i]) / 16.4f; // NOTE: this depends on the sensitivity chosen |
tyftyftyf | 0:21840c01d3d7 | 249 | } |
tyftyftyf | 0:21840c01d3d7 | 250 | } |
tyftyftyf | 0:21840c01d3d7 | 251 | |
tyftyftyf | 0:21840c01d3d7 | 252 | |
tyftyftyf | 0:21840c01d3d7 | 253 | |
tyftyftyf | 0:21840c01d3d7 | 254 | #warning Accelerometer calibration active: have you calibrated your device? |
tyftyftyf | 0:21840c01d3d7 | 255 | // remove offsets and scale accelerometer (calibration) |
tyftyftyf | 0:21840c01d3d7 | 256 | values[0] = (values[0] - acc_off_x) / acc_scale_x; |
tyftyftyf | 0:21840c01d3d7 | 257 | values[1] = (values[1] - acc_off_y) / acc_scale_y; |
tyftyftyf | 0:21840c01d3d7 | 258 | values[2] = (values[2] - acc_off_z) / acc_scale_z; |
tyftyftyf | 0:21840c01d3d7 | 259 | |
tyftyftyf | 0:21840c01d3d7 | 260 | magn.getValues(&values[6]); |
tyftyftyf | 0:21840c01d3d7 | 261 | // calibration |
tyftyftyf | 0:21840c01d3d7 | 262 | #warning Magnetometer calibration active: have you calibrated your device? |
tyftyftyf | 0:21840c01d3d7 | 263 | values[6] = (values[6] - magn_off_x) / magn_scale_x; |
tyftyftyf | 0:21840c01d3d7 | 264 | values[7] = (values[7] - magn_off_y) / magn_scale_y; |
tyftyftyf | 0:21840c01d3d7 | 265 | values[8] = (values[8] - magn_off_z) / magn_scale_z; |
tyftyftyf | 0:21840c01d3d7 | 266 | |
tyftyftyf | 0:21840c01d3d7 | 267 | } |
tyftyftyf | 0:21840c01d3d7 | 268 | |
tyftyftyf | 0:21840c01d3d7 | 269 | |
tyftyftyf | 0:21840c01d3d7 | 270 | /** |
tyftyftyf | 0:21840c01d3d7 | 271 | * Computes gyro offsets |
tyftyftyf | 0:21840c01d3d7 | 272 | */ |
tyftyftyf | 0:21840c01d3d7 | 273 | void FreeIMU::zeroGyro() { |
tyftyftyf | 0:21840c01d3d7 | 274 | const int totSamples = 3; |
tyftyftyf | 0:21840c01d3d7 | 275 | int16_t raw[11]; |
tyftyftyf | 0:21840c01d3d7 | 276 | float tmpOffsets[] = {0,0,0}; |
tyftyftyf | 0:21840c01d3d7 | 277 | |
tyftyftyf | 0:21840c01d3d7 | 278 | for (int i = 0; i < totSamples; i++){ |
tyftyftyf | 0:21840c01d3d7 | 279 | getRawValues(raw); |
tyftyftyf | 0:21840c01d3d7 | 280 | tmpOffsets[0] += raw[3]; |
tyftyftyf | 0:21840c01d3d7 | 281 | tmpOffsets[1] += raw[4]; |
tyftyftyf | 0:21840c01d3d7 | 282 | tmpOffsets[2] += raw[5]; |
tyftyftyf | 0:21840c01d3d7 | 283 | } |
tyftyftyf | 0:21840c01d3d7 | 284 | |
tyftyftyf | 0:21840c01d3d7 | 285 | gyro_off_x = tmpOffsets[0] / totSamples; |
tyftyftyf | 0:21840c01d3d7 | 286 | gyro_off_y = tmpOffsets[1] / totSamples; |
tyftyftyf | 0:21840c01d3d7 | 287 | gyro_off_z = tmpOffsets[2] / totSamples; |
tyftyftyf | 0:21840c01d3d7 | 288 | } |
tyftyftyf | 0:21840c01d3d7 | 289 | |
tyftyftyf | 0:21840c01d3d7 | 290 | |
tyftyftyf | 0:21840c01d3d7 | 291 | /** |
tyftyftyf | 0:21840c01d3d7 | 292 | * Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion |
tyftyftyf | 0:21840c01d3d7 | 293 | * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference |
tyftyftyf | 0:21840c01d3d7 | 294 | * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw |
tyftyftyf | 0:21840c01d3d7 | 295 | * axis only. |
tyftyftyf | 0:21840c01d3d7 | 296 | * |
tyftyftyf | 0:21840c01d3d7 | 297 | * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms |
tyftyftyf | 0:21840c01d3d7 | 298 | */ |
tyftyftyf | 0:21840c01d3d7 | 299 | |
tyftyftyf | 0:21840c01d3d7 | 300 | void FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { |
tyftyftyf | 0:21840c01d3d7 | 301 | |
tyftyftyf | 0:21840c01d3d7 | 302 | float recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 303 | float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; |
tyftyftyf | 0:21840c01d3d7 | 304 | float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; |
tyftyftyf | 0:21840c01d3d7 | 305 | float qa, qb, qc; |
tyftyftyf | 0:21840c01d3d7 | 306 | |
tyftyftyf | 0:21840c01d3d7 | 307 | // Auxiliary variables to avoid repeated arithmetic |
tyftyftyf | 0:21840c01d3d7 | 308 | q0q0 = q0 * q0; |
tyftyftyf | 0:21840c01d3d7 | 309 | q0q1 = q0 * q1; |
tyftyftyf | 0:21840c01d3d7 | 310 | q0q2 = q0 * q2; |
tyftyftyf | 0:21840c01d3d7 | 311 | q0q3 = q0 * q3; |
tyftyftyf | 0:21840c01d3d7 | 312 | q1q1 = q1 * q1; |
tyftyftyf | 0:21840c01d3d7 | 313 | q1q2 = q1 * q2; |
tyftyftyf | 0:21840c01d3d7 | 314 | q1q3 = q1 * q3; |
tyftyftyf | 0:21840c01d3d7 | 315 | q2q2 = q2 * q2; |
tyftyftyf | 0:21840c01d3d7 | 316 | q2q3 = q2 * q3; |
tyftyftyf | 0:21840c01d3d7 | 317 | q3q3 = q3 * q3; |
tyftyftyf | 0:21840c01d3d7 | 318 | |
tyftyftyf | 0:21840c01d3d7 | 319 | // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation) |
tyftyftyf | 0:21840c01d3d7 | 320 | if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) { |
tyftyftyf | 0:21840c01d3d7 | 321 | float hx, hy, bx, bz; |
tyftyftyf | 0:21840c01d3d7 | 322 | float halfwx, halfwy, halfwz; |
tyftyftyf | 0:21840c01d3d7 | 323 | |
tyftyftyf | 0:21840c01d3d7 | 324 | // Normalise magnetometer measurement |
tyftyftyf | 0:21840c01d3d7 | 325 | recipNorm = invSqrt(mx * mx + my * my + mz * mz); |
tyftyftyf | 0:21840c01d3d7 | 326 | mx *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 327 | my *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 328 | mz *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 329 | |
tyftyftyf | 0:21840c01d3d7 | 330 | // Reference direction of Earth's magnetic field |
tyftyftyf | 0:21840c01d3d7 | 331 | hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); |
tyftyftyf | 0:21840c01d3d7 | 332 | hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); |
tyftyftyf | 0:21840c01d3d7 | 333 | bx = sqrt(hx * hx + hy * hy); |
tyftyftyf | 0:21840c01d3d7 | 334 | bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); |
tyftyftyf | 0:21840c01d3d7 | 335 | |
tyftyftyf | 0:21840c01d3d7 | 336 | // Estimated direction of magnetic field |
tyftyftyf | 0:21840c01d3d7 | 337 | halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); |
tyftyftyf | 0:21840c01d3d7 | 338 | halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); |
tyftyftyf | 0:21840c01d3d7 | 339 | halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); |
tyftyftyf | 0:21840c01d3d7 | 340 | |
tyftyftyf | 0:21840c01d3d7 | 341 | // Error is sum of cross product between estimated direction and measured direction of field vectors |
tyftyftyf | 0:21840c01d3d7 | 342 | halfex = (my * halfwz - mz * halfwy); |
tyftyftyf | 0:21840c01d3d7 | 343 | halfey = (mz * halfwx - mx * halfwz); |
tyftyftyf | 0:21840c01d3d7 | 344 | halfez = (mx * halfwy - my * halfwx); |
tyftyftyf | 0:21840c01d3d7 | 345 | } |
tyftyftyf | 0:21840c01d3d7 | 346 | |
tyftyftyf | 0:21840c01d3d7 | 347 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) |
tyftyftyf | 0:21840c01d3d7 | 348 | if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) { |
tyftyftyf | 0:21840c01d3d7 | 349 | float halfvx, halfvy, halfvz; |
tyftyftyf | 0:21840c01d3d7 | 350 | |
tyftyftyf | 0:21840c01d3d7 | 351 | // Normalise accelerometer measurement |
tyftyftyf | 0:21840c01d3d7 | 352 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); |
tyftyftyf | 0:21840c01d3d7 | 353 | ax *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 354 | ay *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 355 | az *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 356 | |
tyftyftyf | 0:21840c01d3d7 | 357 | // Estimated direction of gravity |
tyftyftyf | 0:21840c01d3d7 | 358 | halfvx = q1q3 - q0q2; |
tyftyftyf | 0:21840c01d3d7 | 359 | halfvy = q0q1 + q2q3; |
tyftyftyf | 0:21840c01d3d7 | 360 | halfvz = q0q0 - 0.5f + q3q3; |
tyftyftyf | 0:21840c01d3d7 | 361 | |
tyftyftyf | 0:21840c01d3d7 | 362 | // Error is sum of cross product between estimated direction and measured direction of field vectors |
tyftyftyf | 0:21840c01d3d7 | 363 | halfex += (ay * halfvz - az * halfvy); |
tyftyftyf | 0:21840c01d3d7 | 364 | halfey += (az * halfvx - ax * halfvz); |
tyftyftyf | 0:21840c01d3d7 | 365 | halfez += (ax * halfvy - ay * halfvx); |
tyftyftyf | 0:21840c01d3d7 | 366 | } |
tyftyftyf | 0:21840c01d3d7 | 367 | |
tyftyftyf | 0:21840c01d3d7 | 368 | // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer |
tyftyftyf | 0:21840c01d3d7 | 369 | if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { |
tyftyftyf | 0:21840c01d3d7 | 370 | // Compute and apply integral feedback if enabled |
tyftyftyf | 0:21840c01d3d7 | 371 | if(twoKi > 0.0f) { |
tyftyftyf | 0:21840c01d3d7 | 372 | integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki |
tyftyftyf | 0:21840c01d3d7 | 373 | integralFBy += twoKi * halfey * (1.0f / sampleFreq); |
tyftyftyf | 0:21840c01d3d7 | 374 | integralFBz += twoKi * halfez * (1.0f / sampleFreq); |
tyftyftyf | 0:21840c01d3d7 | 375 | gx += integralFBx; // apply integral feedback |
tyftyftyf | 0:21840c01d3d7 | 376 | gy += integralFBy; |
tyftyftyf | 0:21840c01d3d7 | 377 | gz += integralFBz; |
tyftyftyf | 0:21840c01d3d7 | 378 | } |
tyftyftyf | 0:21840c01d3d7 | 379 | else { |
tyftyftyf | 0:21840c01d3d7 | 380 | integralFBx = 0.0f; // prevent integral windup |
tyftyftyf | 0:21840c01d3d7 | 381 | integralFBy = 0.0f; |
tyftyftyf | 0:21840c01d3d7 | 382 | integralFBz = 0.0f; |
tyftyftyf | 0:21840c01d3d7 | 383 | } |
tyftyftyf | 0:21840c01d3d7 | 384 | |
tyftyftyf | 0:21840c01d3d7 | 385 | // Apply proportional feedback |
tyftyftyf | 0:21840c01d3d7 | 386 | gx += twoKp * halfex; |
tyftyftyf | 0:21840c01d3d7 | 387 | gy += twoKp * halfey; |
tyftyftyf | 0:21840c01d3d7 | 388 | gz += twoKp * halfez; |
tyftyftyf | 0:21840c01d3d7 | 389 | } |
tyftyftyf | 0:21840c01d3d7 | 390 | |
tyftyftyf | 0:21840c01d3d7 | 391 | // Integrate rate of change of quaternion |
tyftyftyf | 0:21840c01d3d7 | 392 | gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors |
tyftyftyf | 0:21840c01d3d7 | 393 | gy *= (0.5f * (1.0f / sampleFreq)); |
tyftyftyf | 0:21840c01d3d7 | 394 | gz *= (0.5f * (1.0f / sampleFreq)); |
tyftyftyf | 0:21840c01d3d7 | 395 | qa = q0; |
tyftyftyf | 0:21840c01d3d7 | 396 | qb = q1; |
tyftyftyf | 0:21840c01d3d7 | 397 | qc = q2; |
tyftyftyf | 0:21840c01d3d7 | 398 | q0 += (-qb * gx - qc * gy - q3 * gz); |
tyftyftyf | 0:21840c01d3d7 | 399 | q1 += (qa * gx + qc * gz - q3 * gy); |
tyftyftyf | 0:21840c01d3d7 | 400 | q2 += (qa * gy - qb * gz + q3 * gx); |
tyftyftyf | 0:21840c01d3d7 | 401 | q3 += (qa * gz + qb * gy - qc * gx); |
tyftyftyf | 0:21840c01d3d7 | 402 | |
tyftyftyf | 0:21840c01d3d7 | 403 | // Normalise quaternion |
tyftyftyf | 0:21840c01d3d7 | 404 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); |
tyftyftyf | 0:21840c01d3d7 | 405 | q0 *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 406 | q1 *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 407 | q2 *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 408 | q3 *= recipNorm; |
tyftyftyf | 0:21840c01d3d7 | 409 | } |
tyftyftyf | 0:21840c01d3d7 | 410 | |
tyftyftyf | 0:21840c01d3d7 | 411 | |
tyftyftyf | 0:21840c01d3d7 | 412 | /** |
tyftyftyf | 0:21840c01d3d7 | 413 | * Populates array q with a quaternion representing the IMU orientation with respect to the Earth |
tyftyftyf | 0:21840c01d3d7 | 414 | * |
tyftyftyf | 0:21840c01d3d7 | 415 | * @param q the quaternion to populate |
tyftyftyf | 0:21840c01d3d7 | 416 | */ |
tyftyftyf | 0:21840c01d3d7 | 417 | void FreeIMU::getQ(float * q) { |
tyftyftyf | 0:21840c01d3d7 | 418 | float val[9]; |
tyftyftyf | 0:21840c01d3d7 | 419 | getValues(val); |
tyftyftyf | 0:21840c01d3d7 | 420 | |
tyftyftyf | 0:21840c01d3d7 | 421 | DEBUG_PRINT(val[3] * M_PI/180); |
tyftyftyf | 0:21840c01d3d7 | 422 | DEBUG_PRINT(val[4] * M_PI/180); |
tyftyftyf | 0:21840c01d3d7 | 423 | DEBUG_PRINT(val[5] * M_PI/180); |
tyftyftyf | 0:21840c01d3d7 | 424 | DEBUG_PRINT(val[0]); |
tyftyftyf | 0:21840c01d3d7 | 425 | DEBUG_PRINT(val[1]); |
tyftyftyf | 0:21840c01d3d7 | 426 | DEBUG_PRINT(val[2]); |
tyftyftyf | 0:21840c01d3d7 | 427 | DEBUG_PRINT(val[6]); |
tyftyftyf | 0:21840c01d3d7 | 428 | DEBUG_PRINT(val[7]); |
tyftyftyf | 0:21840c01d3d7 | 429 | DEBUG_PRINT(val[8]); |
tyftyftyf | 0:21840c01d3d7 | 430 | |
tyftyftyf | 0:21840c01d3d7 | 431 | //now = micros(); |
tyftyftyf | 0:21840c01d3d7 | 432 | dt_us=update.read_us(); |
tyftyftyf | 0:21840c01d3d7 | 433 | sampleFreq = 1.0 / ((dt_us) / 1000000.0); |
tyftyftyf | 0:21840c01d3d7 | 434 | update.reset(); |
tyftyftyf | 0:21840c01d3d7 | 435 | // lastUpdate = now; |
tyftyftyf | 0:21840c01d3d7 | 436 | // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec |
tyftyftyf | 0:21840c01d3d7 | 437 | |
tyftyftyf | 0:21840c01d3d7 | 438 | AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], val[6], val[7], val[8]); |
tyftyftyf | 0:21840c01d3d7 | 439 | |
tyftyftyf | 0:21840c01d3d7 | 440 | if (q!=NULL){ |
tyftyftyf | 0:21840c01d3d7 | 441 | q[0] = q0; |
tyftyftyf | 0:21840c01d3d7 | 442 | q[1] = q1; |
tyftyftyf | 0:21840c01d3d7 | 443 | q[2] = q2; |
tyftyftyf | 0:21840c01d3d7 | 444 | q[3] = q3; |
tyftyftyf | 0:21840c01d3d7 | 445 | } |
tyftyftyf | 0:21840c01d3d7 | 446 | } |
tyftyftyf | 0:21840c01d3d7 | 447 | |
tyftyftyf | 0:21840c01d3d7 | 448 | |
tyftyftyf | 0:21840c01d3d7 | 449 | const float def_sea_press = 1013.25; |
tyftyftyf | 0:21840c01d3d7 | 450 | |
tyftyftyf | 0:21840c01d3d7 | 451 | /** |
tyftyftyf | 0:21840c01d3d7 | 452 | * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure |
tyftyftyf | 0:21840c01d3d7 | 453 | */ |
tyftyftyf | 0:21840c01d3d7 | 454 | float FreeIMU::getBaroAlt(float sea_press) { |
tyftyftyf | 0:21840c01d3d7 | 455 | float temp = baro.getTemperature(MS561101BA_OSR_4096); |
tyftyftyf | 0:21840c01d3d7 | 456 | float press = baro.getPressure(MS561101BA_OSR_4096); |
tyftyftyf | 0:21840c01d3d7 | 457 | return ((pow((float)(sea_press / press), 1.0f/5.257f) - 1.0f) * (temp + 273.15f)) / 0.0065f; |
tyftyftyf | 0:21840c01d3d7 | 458 | } |
tyftyftyf | 0:21840c01d3d7 | 459 | |
tyftyftyf | 0:21840c01d3d7 | 460 | /** |
tyftyftyf | 0:21840c01d3d7 | 461 | * Returns an altitude estimate from baromether readings only using a default sea level pressure |
tyftyftyf | 0:21840c01d3d7 | 462 | */ |
tyftyftyf | 0:21840c01d3d7 | 463 | float FreeIMU::getBaroAlt() { |
tyftyftyf | 0:21840c01d3d7 | 464 | return getBaroAlt(def_sea_press); |
tyftyftyf | 0:21840c01d3d7 | 465 | } |
tyftyftyf | 0:21840c01d3d7 | 466 | |
tyftyftyf | 0:21840c01d3d7 | 467 | float FreeIMU::getRawPressure() { |
tyftyftyf | 0:21840c01d3d7 | 468 | return baro.getPressure(MS561101BA_OSR_4096); |
tyftyftyf | 0:21840c01d3d7 | 469 | } |
tyftyftyf | 0:21840c01d3d7 | 470 | |
tyftyftyf | 0:21840c01d3d7 | 471 | |
tyftyftyf | 0:21840c01d3d7 | 472 | /** |
tyftyftyf | 0:21840c01d3d7 | 473 | * Compensates the accelerometer readings in the 3D vector acc expressed in the sensor frame for gravity |
tyftyftyf | 0:21840c01d3d7 | 474 | * @param acc the accelerometer readings to compensate for gravity |
tyftyftyf | 0:21840c01d3d7 | 475 | * @param q the quaternion orientation of the sensor board with respect to the world |
tyftyftyf | 0:21840c01d3d7 | 476 | */ |
tyftyftyf | 0:21840c01d3d7 | 477 | void FreeIMU::gravityCompensateAcc(float * acc, float * q) { |
tyftyftyf | 0:21840c01d3d7 | 478 | float g[3]; |
tyftyftyf | 0:21840c01d3d7 | 479 | |
tyftyftyf | 0:21840c01d3d7 | 480 | // get expected direction of gravity in the sensor frame |
tyftyftyf | 0:21840c01d3d7 | 481 | g[0] = 2 * (q[1] * q[3] - q[0] * q[2]); |
tyftyftyf | 0:21840c01d3d7 | 482 | g[1] = 2 * (q[0] * q[1] + q[2] * q[3]); |
tyftyftyf | 0:21840c01d3d7 | 483 | g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]; |
tyftyftyf | 0:21840c01d3d7 | 484 | |
tyftyftyf | 0:21840c01d3d7 | 485 | // compensate accelerometer readings with the expected direction of gravity |
tyftyftyf | 0:21840c01d3d7 | 486 | acc[0] = acc[0] - g[0]; |
tyftyftyf | 0:21840c01d3d7 | 487 | acc[1] = acc[1] - g[1]; |
tyftyftyf | 0:21840c01d3d7 | 488 | acc[2] = acc[2] - g[2]; |
tyftyftyf | 0:21840c01d3d7 | 489 | } |
tyftyftyf | 0:21840c01d3d7 | 490 | |
tyftyftyf | 0:21840c01d3d7 | 491 | |
tyftyftyf | 0:21840c01d3d7 | 492 | /** |
tyftyftyf | 0:21840c01d3d7 | 493 | * Returns the Euler angles in radians defined in the Aerospace sequence. |
tyftyftyf | 0:21840c01d3d7 | 494 | * See Sebastian O.H. Madwick report "An efficient orientation filter for |
tyftyftyf | 0:21840c01d3d7 | 495 | * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation |
tyftyftyf | 0:21840c01d3d7 | 496 | * |
tyftyftyf | 0:21840c01d3d7 | 497 | * @param angles three floats array which will be populated by the Euler angles in radians |
tyftyftyf | 0:21840c01d3d7 | 498 | */ |
tyftyftyf | 0:21840c01d3d7 | 499 | void FreeIMU::getEulerRad(float * angles) { |
tyftyftyf | 0:21840c01d3d7 | 500 | float q[4]; // quaternion |
tyftyftyf | 0:21840c01d3d7 | 501 | getQ(q); |
tyftyftyf | 0:21840c01d3d7 | 502 | angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi |
tyftyftyf | 0:21840c01d3d7 | 503 | angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta |
tyftyftyf | 0:21840c01d3d7 | 504 | angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi |
tyftyftyf | 0:21840c01d3d7 | 505 | } |
tyftyftyf | 0:21840c01d3d7 | 506 | |
tyftyftyf | 0:21840c01d3d7 | 507 | |
tyftyftyf | 0:21840c01d3d7 | 508 | /** |
tyftyftyf | 0:21840c01d3d7 | 509 | * Returns the Euler angles in degrees defined with the Aerospace sequence. |
tyftyftyf | 0:21840c01d3d7 | 510 | * See Sebastian O.H. Madwick report "An efficient orientation filter for |
tyftyftyf | 0:21840c01d3d7 | 511 | * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation |
tyftyftyf | 0:21840c01d3d7 | 512 | * |
tyftyftyf | 0:21840c01d3d7 | 513 | * @param angles three floats array which will be populated by the Euler angles in degrees |
tyftyftyf | 0:21840c01d3d7 | 514 | */ |
tyftyftyf | 0:21840c01d3d7 | 515 | void FreeIMU::getEuler(float * angles) { |
tyftyftyf | 0:21840c01d3d7 | 516 | getEulerRad(angles); |
tyftyftyf | 0:21840c01d3d7 | 517 | arr3_rad_to_deg(angles); |
tyftyftyf | 0:21840c01d3d7 | 518 | } |
tyftyftyf | 0:21840c01d3d7 | 519 | |
tyftyftyf | 0:21840c01d3d7 | 520 | |
tyftyftyf | 0:21840c01d3d7 | 521 | /** |
tyftyftyf | 0:21840c01d3d7 | 522 | * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between |
tyftyftyf | 0:21840c01d3d7 | 523 | * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch) |
tyftyftyf | 0:21840c01d3d7 | 524 | * and the Earth ground plane and the IMU Y axis. |
tyftyftyf | 0:21840c01d3d7 | 525 | * |
tyftyftyf | 0:21840c01d3d7 | 526 | * @note This is not an Euler representation: the rotations aren't consecutive rotations but only |
tyftyftyf | 0:21840c01d3d7 | 527 | * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler |
tyftyftyf | 0:21840c01d3d7 | 528 | * |
tyftyftyf | 0:21840c01d3d7 | 529 | * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians |
tyftyftyf | 0:21840c01d3d7 | 530 | */ |
tyftyftyf | 0:21840c01d3d7 | 531 | void FreeIMU::getYawPitchRollRad(float * ypr) { |
tyftyftyf | 0:21840c01d3d7 | 532 | float q[4]; // quaternion |
tyftyftyf | 0:21840c01d3d7 | 533 | float gx, gy, gz; // estimated gravity direction |
tyftyftyf | 0:21840c01d3d7 | 534 | getQ(q); |
tyftyftyf | 0:21840c01d3d7 | 535 | |
tyftyftyf | 0:21840c01d3d7 | 536 | gx = 2 * (q[1]*q[3] - q[0]*q[2]); |
tyftyftyf | 0:21840c01d3d7 | 537 | gy = 2 * (q[0]*q[1] + q[2]*q[3]); |
tyftyftyf | 0:21840c01d3d7 | 538 | gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; |
tyftyftyf | 0:21840c01d3d7 | 539 | |
tyftyftyf | 0:21840c01d3d7 | 540 | ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); |
tyftyftyf | 0:21840c01d3d7 | 541 | ypr[1] = atan(gx / sqrt(gy*gy + gz*gz)); |
tyftyftyf | 0:21840c01d3d7 | 542 | ypr[2] = atan(gy / sqrt(gx*gx + gz*gz)); |
tyftyftyf | 0:21840c01d3d7 | 543 | } |
tyftyftyf | 0:21840c01d3d7 | 544 | |
tyftyftyf | 0:21840c01d3d7 | 545 | |
tyftyftyf | 0:21840c01d3d7 | 546 | /** |
tyftyftyf | 0:21840c01d3d7 | 547 | * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between |
tyftyftyf | 0:21840c01d3d7 | 548 | * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch) |
tyftyftyf | 0:21840c01d3d7 | 549 | * and the Earth ground plane and the IMU Y axis. |
tyftyftyf | 0:21840c01d3d7 | 550 | * |
tyftyftyf | 0:21840c01d3d7 | 551 | * @note This is not an Euler representation: the rotations aren't consecutive rotations but only |
tyftyftyf | 0:21840c01d3d7 | 552 | * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler |
tyftyftyf | 0:21840c01d3d7 | 553 | * |
tyftyftyf | 0:21840c01d3d7 | 554 | * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees |
tyftyftyf | 0:21840c01d3d7 | 555 | */ |
tyftyftyf | 0:21840c01d3d7 | 556 | void FreeIMU::getYawPitchRoll(float * ypr) { |
tyftyftyf | 0:21840c01d3d7 | 557 | getYawPitchRollRad(ypr); |
tyftyftyf | 0:21840c01d3d7 | 558 | arr3_rad_to_deg(ypr); |
tyftyftyf | 0:21840c01d3d7 | 559 | } |
tyftyftyf | 0:21840c01d3d7 | 560 | |
tyftyftyf | 0:21840c01d3d7 | 561 | |
tyftyftyf | 0:21840c01d3d7 | 562 | /** |
tyftyftyf | 0:21840c01d3d7 | 563 | * Converts a 3 elements array arr of angles expressed in radians into degrees |
tyftyftyf | 0:21840c01d3d7 | 564 | */ |
tyftyftyf | 0:21840c01d3d7 | 565 | void arr3_rad_to_deg(float * arr) { |
tyftyftyf | 0:21840c01d3d7 | 566 | arr[0] *= 180/M_PI; |
tyftyftyf | 0:21840c01d3d7 | 567 | arr[1] *= 180/M_PI; |
tyftyftyf | 0:21840c01d3d7 | 568 | arr[2] *= 180/M_PI; |
tyftyftyf | 0:21840c01d3d7 | 569 | } |
tyftyftyf | 0:21840c01d3d7 | 570 | |
tyftyftyf | 0:21840c01d3d7 | 571 | |
tyftyftyf | 0:21840c01d3d7 | 572 | /** |
tyftyftyf | 0:21840c01d3d7 | 573 | * Fast inverse square root implementation |
tyftyftyf | 0:21840c01d3d7 | 574 | * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root |
tyftyftyf | 0:21840c01d3d7 | 575 | */ |
tyftyftyf | 0:21840c01d3d7 | 576 | float invSqrt(float number) { |
tyftyftyf | 0:21840c01d3d7 | 577 | volatile long i; |
tyftyftyf | 0:21840c01d3d7 | 578 | volatile float x, y; |
tyftyftyf | 0:21840c01d3d7 | 579 | volatile const float f = 1.5F; |
tyftyftyf | 0:21840c01d3d7 | 580 | |
tyftyftyf | 0:21840c01d3d7 | 581 | x = number * 0.5F; |
tyftyftyf | 0:21840c01d3d7 | 582 | y = number; |
tyftyftyf | 0:21840c01d3d7 | 583 | i = * ( long * ) &y; |
tyftyftyf | 0:21840c01d3d7 | 584 | i = 0x5f375a86 - ( i >> 1 ); |
tyftyftyf | 0:21840c01d3d7 | 585 | y = * ( float * ) &i; |
tyftyftyf | 0:21840c01d3d7 | 586 | y = y * ( f - ( x * y * y ) ); |
tyftyftyf | 0:21840c01d3d7 | 587 | return y; |
tyftyftyf | 0:21840c01d3d7 | 588 | } |
tyftyftyf | 0:21840c01d3d7 | 589 | |
tyftyftyf | 0:21840c01d3d7 | 590 |