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@9:a79af1283446, 2014-01-10 (annotated)

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