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:
Tue Nov 05 11:31:39 2013 +0000
Revision:
1:794e9cdbc2a0
Parent:
0:21840c01d3d7
Child:
2:5c419926dcd7

        

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