Joseph Roberts / FreeIMU_external_magnetometer

Added external magnetometer functionality

Dependencies:   HMC58X31 MODI2C MPU6050 MS561101BA

Dependents:   Quadcopter_mk2

Fork of FreeIMU by Yifei Teng

FreeIMU.cpp@19:ab5318ef31c4, 2015-04-24 (annotated)

Committer:
joe4465
Date:
Fri Apr 24 16:49:14 2015 +0000
Revision:
19:ab5318ef31c4
Parent:
17:48a0eae27bf1 
24.04.15

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);
joe4465 17:48a0eae27bf1 118 accgyro->setI2CBypassEnabled(0);
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.
joe4465 17:48a0eae27bf1 130 magn->setGain(1);
tyftyftyf 3:f9b100a9aa65 131 // Calibrate HMC using self test, not recommended to change the gain after calibration.
joe4465 17:48a0eae27bf1 132 magn->calibrate(1, 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
joe4465 17:48a0eae27bf1 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