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:
Sat Nov 02 17:25:51 2013 +0000
Revision:
0:21840c01d3d7
Child:
1:794e9cdbc2a0
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew 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