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:
joe4465
Date:
Thu Sep 18 15:47:30 2014 +0000
Revision:
15:ea86489d606b
Parent:
13:21b275eeeda2
Child:
18:3f4803a943d3
Moved _sampling into header file under private

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