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:
Fri Jan 10 05:41:40 2014 +0000
Revision:
9:a79af1283446
Parent:
8:cd43764b9623
Child:
13:21b275eeeda2
Tuning parameters

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