IMU 10dof MEMS from DR Robot adapted from HK10DOF Changed gyro to ITG3200
Fork of HK10DOF by
WARNING: This project is not complete, but this library seems ok so far.
I have the module DFRobotics.com 10DOF MEMS IMU. I wanted a concise module for resolving direction and movement.
I found HK10DOF library (http://developer.mbed.org/users/pommzorz/code/HK10DOF/) with quaternions. But it used a different gyro. So I modified that code to use the same higher level calls but use the ITG3200 low level calls.
IMU10DOF.cpp@4:c4db4e2ffdd7, 2014-11-18 (annotated)
- Committer:
- svkatielee
- Date:
- Tue Nov 18 06:28:56 2014 +0000
- Revision:
- 4:c4db4e2ffdd7
- Parent:
- 3:450acaa4f52d
Changed gyro sensor to ITG3200
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pommzorz | 0:9a1682a09c50 | 1 | /* |
pommzorz | 0:9a1682a09c50 | 2 | FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino |
pommzorz | 0:9a1682a09c50 | 3 | Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net> |
pommzorz | 0:9a1682a09c50 | 4 | ported for HobbyKing's 10DOF stick on a mbed platform by Aloïs Wolff(wolffalois@gmail.com) |
pommzorz | 0:9a1682a09c50 | 5 | |
pommzorz | 0:9a1682a09c50 | 6 | Development of this code has been supported by the Department of Computer Science, |
pommzorz | 0:9a1682a09c50 | 7 | Universita' degli Studi di Torino, Italy within the Piemonte Project |
pommzorz | 0:9a1682a09c50 | 8 | http://www.piemonte.di.unito.it/ |
pommzorz | 0:9a1682a09c50 | 9 | |
pommzorz | 0:9a1682a09c50 | 10 | |
pommzorz | 0:9a1682a09c50 | 11 | This program is free software: you can redistribute it and/or modify |
pommzorz | 0:9a1682a09c50 | 12 | it under the terms of the version 3 GNU General Public License as |
pommzorz | 0:9a1682a09c50 | 13 | published by the Free Software Foundation. |
pommzorz | 0:9a1682a09c50 | 14 | |
pommzorz | 0:9a1682a09c50 | 15 | This program is distributed in the hope that it will be useful, |
pommzorz | 0:9a1682a09c50 | 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
pommzorz | 0:9a1682a09c50 | 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
pommzorz | 0:9a1682a09c50 | 18 | GNU General Public License for more details. |
pommzorz | 0:9a1682a09c50 | 19 | |
pommzorz | 0:9a1682a09c50 | 20 | You should have received a copy of the GNU General Public License |
pommzorz | 0:9a1682a09c50 | 21 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
pommzorz | 0:9a1682a09c50 | 22 | |
svkatielee | 3:450acaa4f52d | 23 | Now ported to support DFRobot.com 10DOF MEMS IMU |
svkatielee | 3:450acaa4f52d | 24 | by Larry Littlefield svkatielee@yahoo.com |
svkatielee | 3:450acaa4f52d | 25 | |
pommzorz | 0:9a1682a09c50 | 26 | */ |
pommzorz | 0:9a1682a09c50 | 27 | |
pommzorz | 0:9a1682a09c50 | 28 | |
svkatielee | 3:450acaa4f52d | 29 | |
pommzorz | 0:9a1682a09c50 | 30 | //#define DEBUG |
pommzorz | 0:9a1682a09c50 | 31 | |
pommzorz | 0:9a1682a09c50 | 32 | |
svkatielee | 3:450acaa4f52d | 33 | #include "IMU10DOF.h" |
svkatielee | 3:450acaa4f52d | 34 | #include <math.h> |
pommzorz | 0:9a1682a09c50 | 35 | #include "helper_3dmath.h" |
svkatielee | 3:450acaa4f52d | 36 | |
pommzorz | 0:9a1682a09c50 | 37 | |
pommzorz | 0:9a1682a09c50 | 38 | // #include "WireUtils.h" |
pommzorz | 0:9a1682a09c50 | 39 | //#include "DebugUtils.h" |
pommzorz | 0:9a1682a09c50 | 40 | //#include "vector_math.h" |
pommzorz | 0:9a1682a09c50 | 41 | |
svkatielee | 3:450acaa4f52d | 42 | #ifndef M_PI |
pommzorz | 0:9a1682a09c50 | 43 | #define M_PI 3.1415926535897932384626433832795 |
svkatielee | 3:450acaa4f52d | 44 | #endif |
pommzorz | 0:9a1682a09c50 | 45 | |
svkatielee | 3:450acaa4f52d | 46 | IMU10DOF::IMU10DOF(PinName sda, PinName scl) : acc(sda,scl), magn(sda,scl),gyro(sda,scl), baro(sda,scl),pc(USBTX,USBRX) |
pommzorz | 0:9a1682a09c50 | 47 | { |
pommzorz | 0:9a1682a09c50 | 48 | |
svkatielee | 3:450acaa4f52d | 49 | pc.baud(115200); |
pommzorz | 0:9a1682a09c50 | 50 | |
pommzorz | 0:9a1682a09c50 | 51 | // initialize quaternion |
pommzorz | 0:9a1682a09c50 | 52 | q0 = 1.0f; |
pommzorz | 0:9a1682a09c50 | 53 | q1 = 0.0f; |
pommzorz | 0:9a1682a09c50 | 54 | q2 = 0.0f; |
pommzorz | 0:9a1682a09c50 | 55 | q3 = 0.0f; |
pommzorz | 0:9a1682a09c50 | 56 | exInt = 0.0; |
pommzorz | 0:9a1682a09c50 | 57 | eyInt = 0.0; |
pommzorz | 0:9a1682a09c50 | 58 | ezInt = 0.0; |
pommzorz | 0:9a1682a09c50 | 59 | twoKp = twoKpDef; |
pommzorz | 0:9a1682a09c50 | 60 | twoKi = twoKiDef; |
pommzorz | 0:9a1682a09c50 | 61 | integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; |
pommzorz | 0:9a1682a09c50 | 62 | |
pommzorz | 0:9a1682a09c50 | 63 | |
pommzorz | 0:9a1682a09c50 | 64 | #ifndef CALIBRATION_H |
pommzorz | 0:9a1682a09c50 | 65 | // initialize scale factors to neutral values |
pommzorz | 0:9a1682a09c50 | 66 | acc_scale_x = 1; |
pommzorz | 0:9a1682a09c50 | 67 | acc_scale_y = 1; |
pommzorz | 0:9a1682a09c50 | 68 | acc_scale_z = 1; |
pommzorz | 0:9a1682a09c50 | 69 | magn_scale_x = 1; |
pommzorz | 0:9a1682a09c50 | 70 | magn_scale_y = 1; |
pommzorz | 0:9a1682a09c50 | 71 | magn_scale_z = 1; |
pommzorz | 0:9a1682a09c50 | 72 | #else |
pommzorz | 0:9a1682a09c50 | 73 | // get values from global variables of same name defined in calibration.h |
pommzorz | 0:9a1682a09c50 | 74 | acc_off_x = ::acc_off_x; |
pommzorz | 0:9a1682a09c50 | 75 | acc_off_y = ::acc_off_y; |
pommzorz | 0:9a1682a09c50 | 76 | acc_off_z = ::acc_off_z; |
pommzorz | 0:9a1682a09c50 | 77 | acc_scale_x = ::acc_scale_x; |
pommzorz | 0:9a1682a09c50 | 78 | acc_scale_y = ::acc_scale_y; |
pommzorz | 0:9a1682a09c50 | 79 | acc_scale_z = ::acc_scale_z; |
pommzorz | 0:9a1682a09c50 | 80 | magn_off_x = ::magn_off_x; |
pommzorz | 0:9a1682a09c50 | 81 | magn_off_y = ::magn_off_y; |
pommzorz | 0:9a1682a09c50 | 82 | magn_off_z = ::magn_off_z; |
pommzorz | 0:9a1682a09c50 | 83 | magn_scale_x = ::magn_scale_x; |
pommzorz | 0:9a1682a09c50 | 84 | magn_scale_y = ::magn_scale_y; |
pommzorz | 0:9a1682a09c50 | 85 | magn_scale_z = ::magn_scale_z; |
pommzorz | 0:9a1682a09c50 | 86 | #endif |
pommzorz | 0:9a1682a09c50 | 87 | } |
pommzorz | 0:9a1682a09c50 | 88 | |
pommzorz | 0:9a1682a09c50 | 89 | |
pommzorz | 0:9a1682a09c50 | 90 | |
pommzorz | 0:9a1682a09c50 | 91 | |
pommzorz | 0:9a1682a09c50 | 92 | /** |
pommzorz | 0:9a1682a09c50 | 93 | * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration |
pommzorz | 0:9a1682a09c50 | 94 | */ |
pommzorz | 0:9a1682a09c50 | 95 | |
svkatielee | 3:450acaa4f52d | 96 | void IMU10DOF::init(bool fastmode) |
pommzorz | 0:9a1682a09c50 | 97 | { |
pommzorz | 0:9a1682a09c50 | 98 | |
pommzorz | 0:9a1682a09c50 | 99 | |
pommzorz | 0:9a1682a09c50 | 100 | //Sensors Initialization |
pommzorz | 0:9a1682a09c50 | 101 | pc.printf("Initializing sensors...\n\r"); |
pommzorz | 0:9a1682a09c50 | 102 | magn.init(); |
pommzorz | 0:9a1682a09c50 | 103 | baro.getCalibrationData(); |
pommzorz | 0:9a1682a09c50 | 104 | acc.setPowerControl(0x00); |
pommzorz | 0:9a1682a09c50 | 105 | wait_ms(10); |
pommzorz | 0:9a1682a09c50 | 106 | acc.setDataFormatControl(0x0B); |
pommzorz | 0:9a1682a09c50 | 107 | wait_ms(10); |
pommzorz | 0:9a1682a09c50 | 108 | acc.setPowerControl(MeasurementMode); |
pommzorz | 0:9a1682a09c50 | 109 | wait_ms(10); |
svkatielee | 3:450acaa4f52d | 110 | gyro.init(); |
pommzorz | 0:9a1682a09c50 | 111 | pc.printf("Sensors OK!\n\r"); |
pommzorz | 0:9a1682a09c50 | 112 | |
pommzorz | 0:9a1682a09c50 | 113 | update.start(); |
pommzorz | 0:9a1682a09c50 | 114 | int dt_us=0; |
pommzorz | 0:9a1682a09c50 | 115 | pc.printf("ZeroGyro\n\r"); |
pommzorz | 0:9a1682a09c50 | 116 | // zero gyro |
pommzorz | 0:9a1682a09c50 | 117 | //zeroGyro(); |
pommzorz | 0:9a1682a09c50 | 118 | gyro.zeroCalibrate(128,5); |
pommzorz | 0:9a1682a09c50 | 119 | pc.printf("ZeroGyro OK, CalLoad...\n\r"); |
pommzorz | 0:9a1682a09c50 | 120 | // load calibration from eeprom |
pommzorz | 0:9a1682a09c50 | 121 | calLoad(); |
pommzorz | 0:9a1682a09c50 | 122 | |
pommzorz | 0:9a1682a09c50 | 123 | } |
pommzorz | 0:9a1682a09c50 | 124 | |
svkatielee | 3:450acaa4f52d | 125 | void IMU10DOF::calLoad() |
pommzorz | 0:9a1682a09c50 | 126 | { |
pommzorz | 0:9a1682a09c50 | 127 | |
pommzorz | 0:9a1682a09c50 | 128 | |
pommzorz | 0:9a1682a09c50 | 129 | acc_off_x = 0; |
pommzorz | 0:9a1682a09c50 | 130 | acc_off_y = 0; |
pommzorz | 0:9a1682a09c50 | 131 | acc_off_z = 0; |
pommzorz | 0:9a1682a09c50 | 132 | acc_scale_x = 1; |
pommzorz | 0:9a1682a09c50 | 133 | acc_scale_y = 1; |
pommzorz | 0:9a1682a09c50 | 134 | acc_scale_z = 1; |
pommzorz | 0:9a1682a09c50 | 135 | |
svkatielee | 3:450acaa4f52d | 136 | magn_off_x = 0.21; |
svkatielee | 3:450acaa4f52d | 137 | magn_off_y = -0.175; |
pommzorz | 0:9a1682a09c50 | 138 | magn_off_z = 0; |
pommzorz | 0:9a1682a09c50 | 139 | magn_scale_x = 1; |
pommzorz | 0:9a1682a09c50 | 140 | magn_scale_y = 1; |
pommzorz | 0:9a1682a09c50 | 141 | magn_scale_z = 1; |
pommzorz | 0:9a1682a09c50 | 142 | } |
pommzorz | 0:9a1682a09c50 | 143 | |
svkatielee | 3:450acaa4f52d | 144 | void IMU10DOF::getRawValues(int16_t * raw_values) |
pommzorz | 0:9a1682a09c50 | 145 | { |
pommzorz | 0:9a1682a09c50 | 146 | int16_t raw3[3]; |
pommzorz | 0:9a1682a09c50 | 147 | int raw[3]; |
pommzorz | 0:9a1682a09c50 | 148 | |
pommzorz | 0:9a1682a09c50 | 149 | pc.printf("GetRaw IN \n\r"); |
pommzorz | 0:9a1682a09c50 | 150 | |
pommzorz | 0:9a1682a09c50 | 151 | acc.getOutput(&raw_values[0], &raw_values[1], &raw_values[2]); |
pommzorz | 0:9a1682a09c50 | 152 | pc.printf("GotACC\n\r"); |
pommzorz | 0:9a1682a09c50 | 153 | |
pommzorz | 0:9a1682a09c50 | 154 | gyro.read(raw); |
pommzorz | 0:9a1682a09c50 | 155 | |
pommzorz | 0:9a1682a09c50 | 156 | raw_values[3]=(int16_t)raw[0]; |
pommzorz | 0:9a1682a09c50 | 157 | raw_values[4]=(int16_t)raw[1]; |
pommzorz | 0:9a1682a09c50 | 158 | raw_values[5]=(int16_t)raw[2]; |
pommzorz | 0:9a1682a09c50 | 159 | pc.printf("GotGYRO\n\r"); |
pommzorz | 0:9a1682a09c50 | 160 | |
pommzorz | 0:9a1682a09c50 | 161 | |
pommzorz | 0:9a1682a09c50 | 162 | magn.getXYZ(raw3); |
pommzorz | 0:9a1682a09c50 | 163 | raw_values[6]=raw3[0]; |
pommzorz | 0:9a1682a09c50 | 164 | raw_values[7]=raw3[1]; |
pommzorz | 0:9a1682a09c50 | 165 | raw_values[8]=raw3[2]; |
pommzorz | 0:9a1682a09c50 | 166 | pc.printf("GotMAG\n\r"); |
pommzorz | 0:9a1682a09c50 | 167 | |
pommzorz | 0:9a1682a09c50 | 168 | |
pommzorz | 0:9a1682a09c50 | 169 | |
pommzorz | 0:9a1682a09c50 | 170 | baro.readSensor(); |
pommzorz | 0:9a1682a09c50 | 171 | raw_values[9]=(int16_t)baro.temp(); |
pommzorz | 0:9a1682a09c50 | 172 | raw_values[10]=(int16_t)baro.press(); |
pommzorz | 0:9a1682a09c50 | 173 | pc.printf("GotBARO\n\r"); |
pommzorz | 0:9a1682a09c50 | 174 | |
pommzorz | 0:9a1682a09c50 | 175 | |
pommzorz | 0:9a1682a09c50 | 176 | } |
pommzorz | 0:9a1682a09c50 | 177 | |
pommzorz | 0:9a1682a09c50 | 178 | |
pommzorz | 0:9a1682a09c50 | 179 | /** |
pommzorz | 0:9a1682a09c50 | 180 | * Populates values with calibrated readings from the sensors |
pommzorz | 0:9a1682a09c50 | 181 | */ |
svkatielee | 3:450acaa4f52d | 182 | void IMU10DOF::getValues(float * values) |
pommzorz | 0:9a1682a09c50 | 183 | { |
pommzorz | 0:9a1682a09c50 | 184 | |
pommzorz | 0:9a1682a09c50 | 185 | int16_t accval[3]; |
pommzorz | 0:9a1682a09c50 | 186 | float gyrval[3]; |
pommzorz | 0:9a1682a09c50 | 187 | acc.getOutput(accval); |
pommzorz | 0:9a1682a09c50 | 188 | values[0] = (float) accval[0]; |
pommzorz | 0:9a1682a09c50 | 189 | values[1] = (float) accval[1]; |
pommzorz | 0:9a1682a09c50 | 190 | values[2] = (float) accval[2]; |
pommzorz | 0:9a1682a09c50 | 191 | |
pommzorz | 0:9a1682a09c50 | 192 | gyro.readFin(gyrval); |
pommzorz | 0:9a1682a09c50 | 193 | values[3] = gyrval[0]; |
pommzorz | 0:9a1682a09c50 | 194 | values[4] = gyrval[1]; |
pommzorz | 0:9a1682a09c50 | 195 | values[5] = gyrval[2]; |
pommzorz | 0:9a1682a09c50 | 196 | |
pommzorz | 0:9a1682a09c50 | 197 | magn.getXYZ(accval); |
svkatielee | 3:450acaa4f52d | 198 | values[6]=(float)accval[0]; |
svkatielee | 3:450acaa4f52d | 199 | values[7]=(float)accval[1]; |
svkatielee | 3:450acaa4f52d | 200 | values[8]=(float)accval[2]; |
pommzorz | 0:9a1682a09c50 | 201 | |
pommzorz | 0:9a1682a09c50 | 202 | |
pommzorz | 0:9a1682a09c50 | 203 | #warning Accelerometer calibration active: have you calibrated your device? |
pommzorz | 0:9a1682a09c50 | 204 | // remove offsets and scale accelerometer (calibration) |
pommzorz | 0:9a1682a09c50 | 205 | values[0] = (values[0] - acc_off_x) / acc_scale_x; |
pommzorz | 0:9a1682a09c50 | 206 | values[1] = (values[1] - acc_off_y) / acc_scale_y; |
pommzorz | 0:9a1682a09c50 | 207 | values[2] = (values[2] - acc_off_z) / acc_scale_z; |
pommzorz | 0:9a1682a09c50 | 208 | |
pommzorz | 0:9a1682a09c50 | 209 | // calibration |
pommzorz | 0:9a1682a09c50 | 210 | #warning Magnetometer calibration active: have you calibrated your device? |
pommzorz | 0:9a1682a09c50 | 211 | values[6] = (values[6] - magn_off_x) / magn_scale_x; |
pommzorz | 0:9a1682a09c50 | 212 | values[7] = (values[7] - magn_off_y) / magn_scale_y; |
pommzorz | 0:9a1682a09c50 | 213 | values[8] = (values[8] - magn_off_z) / magn_scale_z; |
pommzorz | 0:9a1682a09c50 | 214 | |
pommzorz | 0:9a1682a09c50 | 215 | |
pommzorz | 0:9a1682a09c50 | 216 | |
pommzorz | 0:9a1682a09c50 | 217 | } |
pommzorz | 0:9a1682a09c50 | 218 | |
pommzorz | 0:9a1682a09c50 | 219 | |
pommzorz | 0:9a1682a09c50 | 220 | /** |
pommzorz | 0:9a1682a09c50 | 221 | * Computes gyro offsets |
pommzorz | 0:9a1682a09c50 | 222 | */ |
svkatielee | 3:450acaa4f52d | 223 | void IMU10DOF::zeroGyro() |
pommzorz | 0:9a1682a09c50 | 224 | { |
pommzorz | 0:9a1682a09c50 | 225 | const int totSamples = 3; |
pommzorz | 0:9a1682a09c50 | 226 | int16_t raw[11]; |
pommzorz | 0:9a1682a09c50 | 227 | float tmpOffsets[] = {0,0,0}; |
pommzorz | 0:9a1682a09c50 | 228 | |
pommzorz | 0:9a1682a09c50 | 229 | for (int i = 0; i < totSamples; i++) { |
pommzorz | 0:9a1682a09c50 | 230 | getRawValues(raw); |
pommzorz | 0:9a1682a09c50 | 231 | tmpOffsets[0] += raw[3]; |
pommzorz | 0:9a1682a09c50 | 232 | tmpOffsets[1] += raw[4]; |
pommzorz | 0:9a1682a09c50 | 233 | tmpOffsets[2] += raw[5]; |
pommzorz | 0:9a1682a09c50 | 234 | } |
pommzorz | 0:9a1682a09c50 | 235 | |
pommzorz | 0:9a1682a09c50 | 236 | gyro_off_x = tmpOffsets[0] / totSamples; |
pommzorz | 0:9a1682a09c50 | 237 | gyro_off_y = tmpOffsets[1] / totSamples; |
pommzorz | 0:9a1682a09c50 | 238 | gyro_off_z = tmpOffsets[2] / totSamples; |
pommzorz | 0:9a1682a09c50 | 239 | } |
pommzorz | 0:9a1682a09c50 | 240 | |
pommzorz | 0:9a1682a09c50 | 241 | |
pommzorz | 0:9a1682a09c50 | 242 | /** |
pommzorz | 0:9a1682a09c50 | 243 | * Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion |
pommzorz | 0:9a1682a09c50 | 244 | * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference |
pommzorz | 0:9a1682a09c50 | 245 | * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw |
pommzorz | 0:9a1682a09c50 | 246 | * axis only. |
pommzorz | 0:9a1682a09c50 | 247 | * |
pommzorz | 0:9a1682a09c50 | 248 | * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms |
pommzorz | 0:9a1682a09c50 | 249 | */ |
pommzorz | 0:9a1682a09c50 | 250 | //#if IS_9DOM() |
svkatielee | 3:450acaa4f52d | 251 | void IMU10DOF::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) |
pommzorz | 0:9a1682a09c50 | 252 | { |
pommzorz | 0:9a1682a09c50 | 253 | |
pommzorz | 0:9a1682a09c50 | 254 | float recipNorm; |
pommzorz | 0:9a1682a09c50 | 255 | float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; |
pommzorz | 0:9a1682a09c50 | 256 | float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; |
pommzorz | 0:9a1682a09c50 | 257 | float qa, qb, qc; |
pommzorz | 0:9a1682a09c50 | 258 | |
pommzorz | 0:9a1682a09c50 | 259 | // Auxiliary variables to avoid repeated arithmetic |
pommzorz | 0:9a1682a09c50 | 260 | q0q0 = q0 * q0; |
pommzorz | 0:9a1682a09c50 | 261 | q0q1 = q0 * q1; |
pommzorz | 0:9a1682a09c50 | 262 | q0q2 = q0 * q2; |
pommzorz | 0:9a1682a09c50 | 263 | q0q3 = q0 * q3; |
pommzorz | 0:9a1682a09c50 | 264 | q1q1 = q1 * q1; |
pommzorz | 0:9a1682a09c50 | 265 | q1q2 = q1 * q2; |
pommzorz | 0:9a1682a09c50 | 266 | q1q3 = q1 * q3; |
pommzorz | 0:9a1682a09c50 | 267 | q2q2 = q2 * q2; |
pommzorz | 0:9a1682a09c50 | 268 | q2q3 = q2 * q3; |
pommzorz | 0:9a1682a09c50 | 269 | q3q3 = q3 * q3; |
pommzorz | 0:9a1682a09c50 | 270 | |
pommzorz | 0:9a1682a09c50 | 271 | // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation) |
pommzorz | 0:9a1682a09c50 | 272 | if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) { |
pommzorz | 0:9a1682a09c50 | 273 | float hx, hy, bx, bz; |
pommzorz | 0:9a1682a09c50 | 274 | float halfwx, halfwy, halfwz; |
pommzorz | 0:9a1682a09c50 | 275 | |
pommzorz | 0:9a1682a09c50 | 276 | // Normalise magnetometer measurement |
pommzorz | 0:9a1682a09c50 | 277 | recipNorm = invSqrt(mx * mx + my * my + mz * mz); |
pommzorz | 0:9a1682a09c50 | 278 | mx *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 279 | my *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 280 | mz *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 281 | |
pommzorz | 0:9a1682a09c50 | 282 | // Reference direction of Earth's magnetic field |
pommzorz | 0:9a1682a09c50 | 283 | hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); |
pommzorz | 0:9a1682a09c50 | 284 | hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); |
pommzorz | 0:9a1682a09c50 | 285 | bx = sqrt(hx * hx + hy * hy); |
pommzorz | 0:9a1682a09c50 | 286 | bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); |
pommzorz | 0:9a1682a09c50 | 287 | |
pommzorz | 0:9a1682a09c50 | 288 | // Estimated direction of magnetic field |
pommzorz | 0:9a1682a09c50 | 289 | halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); |
pommzorz | 0:9a1682a09c50 | 290 | halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); |
pommzorz | 0:9a1682a09c50 | 291 | halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); |
pommzorz | 0:9a1682a09c50 | 292 | |
pommzorz | 0:9a1682a09c50 | 293 | // Error is sum of cross product between estimated direction and measured direction of field vectors |
pommzorz | 0:9a1682a09c50 | 294 | halfex = (my * halfwz - mz * halfwy); |
pommzorz | 0:9a1682a09c50 | 295 | halfey = (mz * halfwx - mx * halfwz); |
pommzorz | 0:9a1682a09c50 | 296 | halfez = (mx * halfwy - my * halfwx); |
pommzorz | 0:9a1682a09c50 | 297 | } |
pommzorz | 0:9a1682a09c50 | 298 | |
pommzorz | 0:9a1682a09c50 | 299 | |
pommzorz | 0:9a1682a09c50 | 300 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) |
pommzorz | 0:9a1682a09c50 | 301 | if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) { |
pommzorz | 0:9a1682a09c50 | 302 | float halfvx, halfvy, halfvz; |
pommzorz | 0:9a1682a09c50 | 303 | |
pommzorz | 0:9a1682a09c50 | 304 | // Normalise accelerometer measurement |
pommzorz | 0:9a1682a09c50 | 305 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); |
pommzorz | 0:9a1682a09c50 | 306 | ax *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 307 | ay *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 308 | az *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 309 | |
pommzorz | 0:9a1682a09c50 | 310 | // Estimated direction of gravity |
pommzorz | 0:9a1682a09c50 | 311 | halfvx = q1q3 - q0q2; |
pommzorz | 0:9a1682a09c50 | 312 | halfvy = q0q1 + q2q3; |
pommzorz | 0:9a1682a09c50 | 313 | halfvz = q0q0 - 0.5f + q3q3; |
pommzorz | 0:9a1682a09c50 | 314 | |
pommzorz | 0:9a1682a09c50 | 315 | // Error is sum of cross product between estimated direction and measured direction of field vectors |
pommzorz | 0:9a1682a09c50 | 316 | halfex += (ay * halfvz - az * halfvy); |
pommzorz | 0:9a1682a09c50 | 317 | halfey += (az * halfvx - ax * halfvz); |
pommzorz | 0:9a1682a09c50 | 318 | halfez += (ax * halfvy - ay * halfvx); |
pommzorz | 0:9a1682a09c50 | 319 | } |
pommzorz | 0:9a1682a09c50 | 320 | |
pommzorz | 0:9a1682a09c50 | 321 | // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer |
pommzorz | 0:9a1682a09c50 | 322 | if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { |
pommzorz | 0:9a1682a09c50 | 323 | // Compute and apply integral feedback if enabled |
pommzorz | 0:9a1682a09c50 | 324 | if(twoKi > 0.0f) { |
pommzorz | 0:9a1682a09c50 | 325 | integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki |
pommzorz | 0:9a1682a09c50 | 326 | integralFBy += twoKi * halfey * (1.0f / sampleFreq); |
pommzorz | 0:9a1682a09c50 | 327 | integralFBz += twoKi * halfez * (1.0f / sampleFreq); |
pommzorz | 0:9a1682a09c50 | 328 | gx += integralFBx; // apply integral feedback |
pommzorz | 0:9a1682a09c50 | 329 | gy += integralFBy; |
pommzorz | 0:9a1682a09c50 | 330 | gz += integralFBz; |
pommzorz | 0:9a1682a09c50 | 331 | } else { |
pommzorz | 0:9a1682a09c50 | 332 | integralFBx = 0.0f; // prevent integral windup |
pommzorz | 0:9a1682a09c50 | 333 | integralFBy = 0.0f; |
pommzorz | 0:9a1682a09c50 | 334 | integralFBz = 0.0f; |
pommzorz | 0:9a1682a09c50 | 335 | } |
pommzorz | 0:9a1682a09c50 | 336 | |
pommzorz | 0:9a1682a09c50 | 337 | // Apply proportional feedback |
pommzorz | 0:9a1682a09c50 | 338 | gx += twoKp * halfex; |
pommzorz | 0:9a1682a09c50 | 339 | gy += twoKp * halfey; |
pommzorz | 0:9a1682a09c50 | 340 | gz += twoKp * halfez; |
pommzorz | 0:9a1682a09c50 | 341 | } |
pommzorz | 0:9a1682a09c50 | 342 | |
pommzorz | 0:9a1682a09c50 | 343 | // Integrate rate of change of quaternion |
pommzorz | 0:9a1682a09c50 | 344 | gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors |
pommzorz | 0:9a1682a09c50 | 345 | gy *= (0.5f * (1.0f / sampleFreq)); |
pommzorz | 0:9a1682a09c50 | 346 | gz *= (0.5f * (1.0f / sampleFreq)); |
pommzorz | 0:9a1682a09c50 | 347 | qa = q0; |
pommzorz | 0:9a1682a09c50 | 348 | qb = q1; |
pommzorz | 0:9a1682a09c50 | 349 | qc = q2; |
pommzorz | 0:9a1682a09c50 | 350 | q0 += (-qb * gx - qc * gy - q3 * gz); |
pommzorz | 0:9a1682a09c50 | 351 | q1 += (qa * gx + qc * gz - q3 * gy); |
pommzorz | 0:9a1682a09c50 | 352 | q2 += (qa * gy - qb * gz + q3 * gx); |
pommzorz | 0:9a1682a09c50 | 353 | q3 += (qa * gz + qb * gy - qc * gx); |
pommzorz | 0:9a1682a09c50 | 354 | |
pommzorz | 0:9a1682a09c50 | 355 | // Normalise quaternion |
pommzorz | 0:9a1682a09c50 | 356 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); |
pommzorz | 0:9a1682a09c50 | 357 | q0 *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 358 | q1 *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 359 | q2 *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 360 | q3 *= recipNorm; |
pommzorz | 0:9a1682a09c50 | 361 | } |
pommzorz | 0:9a1682a09c50 | 362 | |
pommzorz | 0:9a1682a09c50 | 363 | |
pommzorz | 0:9a1682a09c50 | 364 | /** |
pommzorz | 0:9a1682a09c50 | 365 | * Populates array q with a quaternion representing the IMU orientation with respect to the Earth |
pommzorz | 0:9a1682a09c50 | 366 | * |
pommzorz | 0:9a1682a09c50 | 367 | * @param q the quaternion to populate |
pommzorz | 0:9a1682a09c50 | 368 | */ |
svkatielee | 3:450acaa4f52d | 369 | void IMU10DOF::getQ(float * q) |
pommzorz | 0:9a1682a09c50 | 370 | { |
pommzorz | 0:9a1682a09c50 | 371 | float val[9]; |
pommzorz | 0:9a1682a09c50 | 372 | getValues(val); |
pommzorz | 0:9a1682a09c50 | 373 | /* |
pommzorz | 0:9a1682a09c50 | 374 | DEBUG_PRINT(val[3] * M_PI/180); |
pommzorz | 0:9a1682a09c50 | 375 | DEBUG_PRINT(val[4] * M_PI/180); |
pommzorz | 0:9a1682a09c50 | 376 | DEBUG_PRINT(val[5] * M_PI/180); |
pommzorz | 0:9a1682a09c50 | 377 | DEBUG_PRINT(val[0]); |
pommzorz | 0:9a1682a09c50 | 378 | DEBUG_PRINT(val[1]); |
pommzorz | 0:9a1682a09c50 | 379 | DEBUG_PRINT(val[2]); |
pommzorz | 0:9a1682a09c50 | 380 | DEBUG_PRINT(val[6]); |
pommzorz | 0:9a1682a09c50 | 381 | DEBUG_PRINT(val[7]); |
pommzorz | 0:9a1682a09c50 | 382 | DEBUG_PRINT(val[8]); |
pommzorz | 0:9a1682a09c50 | 383 | */ |
pommzorz | 0:9a1682a09c50 | 384 | |
pommzorz | 0:9a1682a09c50 | 385 | dt_us=update.read_us(); |
pommzorz | 0:9a1682a09c50 | 386 | sampleFreq = 1.0 / ((dt_us) / 1000000.0); |
pommzorz | 0:9a1682a09c50 | 387 | update.reset(); |
pommzorz | 0:9a1682a09c50 | 388 | |
pommzorz | 0:9a1682a09c50 | 389 | |
pommzorz | 0:9a1682a09c50 | 390 | |
pommzorz | 0:9a1682a09c50 | 391 | // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec |
pommzorz | 0:9a1682a09c50 | 392 | 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]); |
pommzorz | 0:9a1682a09c50 | 393 | |
pommzorz | 0:9a1682a09c50 | 394 | |
pommzorz | 0:9a1682a09c50 | 395 | |
pommzorz | 0:9a1682a09c50 | 396 | q[0] = q0; |
pommzorz | 0:9a1682a09c50 | 397 | q[1] = q1; |
pommzorz | 0:9a1682a09c50 | 398 | q[2] = q2; |
pommzorz | 0:9a1682a09c50 | 399 | q[3] = q3; |
pommzorz | 0:9a1682a09c50 | 400 | } |
pommzorz | 0:9a1682a09c50 | 401 | |
pommzorz | 0:9a1682a09c50 | 402 | |
pommzorz | 0:9a1682a09c50 | 403 | |
pommzorz | 0:9a1682a09c50 | 404 | const float def_sea_press = 1013.25; |
pommzorz | 0:9a1682a09c50 | 405 | |
pommzorz | 0:9a1682a09c50 | 406 | /** |
pommzorz | 0:9a1682a09c50 | 407 | * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure |
pommzorz | 0:9a1682a09c50 | 408 | */ |
svkatielee | 3:450acaa4f52d | 409 | float IMU10DOF::getBaroAlt(float sea_press) |
pommzorz | 0:9a1682a09c50 | 410 | { |
pommzorz | 0:9a1682a09c50 | 411 | //float temp,press,res; |
pommzorz | 0:9a1682a09c50 | 412 | baro.readSensor(); |
pommzorz | 0:9a1682a09c50 | 413 | //temp=(float)baro.temp(); |
pommzorz | 0:9a1682a09c50 | 414 | //press=(float)baro.press(); |
pommzorz | 0:9a1682a09c50 | 415 | |
pommzorz | 0:9a1682a09c50 | 416 | |
pommzorz | 0:9a1682a09c50 | 417 | |
pommzorz | 0:9a1682a09c50 | 418 | return baro.altitud(); |
pommzorz | 0:9a1682a09c50 | 419 | //return(temp,press); |
pommzorz | 0:9a1682a09c50 | 420 | } |
pommzorz | 0:9a1682a09c50 | 421 | |
pommzorz | 0:9a1682a09c50 | 422 | /** |
pommzorz | 0:9a1682a09c50 | 423 | * Returns an altitude estimate from baromether readings only using a default sea level pressure |
pommzorz | 0:9a1682a09c50 | 424 | */ |
svkatielee | 3:450acaa4f52d | 425 | float IMU10DOF::getBaroAlt() |
pommzorz | 0:9a1682a09c50 | 426 | { |
pommzorz | 0:9a1682a09c50 | 427 | return getBaroAlt(def_sea_press); |
pommzorz | 0:9a1682a09c50 | 428 | } |
pommzorz | 0:9a1682a09c50 | 429 | |
pommzorz | 0:9a1682a09c50 | 430 | |
pommzorz | 0:9a1682a09c50 | 431 | /** |
pommzorz | 0:9a1682a09c50 | 432 | * Compensates the accelerometer readings in the 3D vector acc expressed in the sensor frame for gravity |
pommzorz | 0:9a1682a09c50 | 433 | * @param acc the accelerometer readings to compensate for gravity |
pommzorz | 0:9a1682a09c50 | 434 | * @param q the quaternion orientation of the sensor board with respect to the world |
pommzorz | 0:9a1682a09c50 | 435 | */ |
svkatielee | 3:450acaa4f52d | 436 | void IMU10DOF::gravityCompensateAcc(float * acc, float * q) |
pommzorz | 0:9a1682a09c50 | 437 | { |
pommzorz | 0:9a1682a09c50 | 438 | float g[3]; |
pommzorz | 0:9a1682a09c50 | 439 | |
pommzorz | 0:9a1682a09c50 | 440 | // get expected direction of gravity in the sensor frame |
pommzorz | 0:9a1682a09c50 | 441 | g[0] = 2 * (q[1] * q[3] - q[0] * q[2]); |
pommzorz | 0:9a1682a09c50 | 442 | g[1] = 2 * (q[0] * q[1] + q[2] * q[3]); |
pommzorz | 0:9a1682a09c50 | 443 | g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]; |
pommzorz | 0:9a1682a09c50 | 444 | |
pommzorz | 0:9a1682a09c50 | 445 | // compensate accelerometer readings with the expected direction of gravity |
pommzorz | 0:9a1682a09c50 | 446 | acc[0] = acc[0] - g[0]; |
pommzorz | 0:9a1682a09c50 | 447 | acc[1] = acc[1] - g[1]; |
pommzorz | 0:9a1682a09c50 | 448 | acc[2] = acc[2] - g[2]; |
pommzorz | 0:9a1682a09c50 | 449 | } |
pommzorz | 0:9a1682a09c50 | 450 | |
pommzorz | 0:9a1682a09c50 | 451 | |
pommzorz | 0:9a1682a09c50 | 452 | |
pommzorz | 0:9a1682a09c50 | 453 | /** |
pommzorz | 0:9a1682a09c50 | 454 | * Returns the Euler angles in radians defined in the Aerospace sequence. |
pommzorz | 0:9a1682a09c50 | 455 | * See Sebastian O.H. Madwick report "An efficient orientation filter for |
pommzorz | 0:9a1682a09c50 | 456 | * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation |
pommzorz | 0:9a1682a09c50 | 457 | * |
pommzorz | 0:9a1682a09c50 | 458 | * @param angles three floats array which will be populated by the Euler angles in radians |
pommzorz | 0:9a1682a09c50 | 459 | */ |
svkatielee | 3:450acaa4f52d | 460 | void IMU10DOF::getEulerRad(float * angles) |
pommzorz | 0:9a1682a09c50 | 461 | { |
pommzorz | 0:9a1682a09c50 | 462 | float q[4]; // quaternion |
pommzorz | 0:9a1682a09c50 | 463 | getQ(q); |
pommzorz | 0:9a1682a09c50 | 464 | 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 |
pommzorz | 0:9a1682a09c50 | 465 | angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta |
pommzorz | 0:9a1682a09c50 | 466 | 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 |
pommzorz | 0:9a1682a09c50 | 467 | } |
pommzorz | 0:9a1682a09c50 | 468 | |
pommzorz | 0:9a1682a09c50 | 469 | |
pommzorz | 0:9a1682a09c50 | 470 | /** |
pommzorz | 0:9a1682a09c50 | 471 | * Returns the Euler angles in degrees defined with the Aerospace sequence. |
pommzorz | 0:9a1682a09c50 | 472 | * See Sebastian O.H. Madwick report "An efficient orientation filter for |
pommzorz | 0:9a1682a09c50 | 473 | * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation |
pommzorz | 0:9a1682a09c50 | 474 | * |
pommzorz | 0:9a1682a09c50 | 475 | * @param angles three floats array which will be populated by the Euler angles in degrees |
pommzorz | 0:9a1682a09c50 | 476 | */ |
svkatielee | 3:450acaa4f52d | 477 | void IMU10DOF::getEuler(float * angles) |
pommzorz | 0:9a1682a09c50 | 478 | { |
pommzorz | 0:9a1682a09c50 | 479 | getEulerRad(angles); |
pommzorz | 0:9a1682a09c50 | 480 | arr3_rad_to_deg(angles); |
pommzorz | 0:9a1682a09c50 | 481 | } |
pommzorz | 0:9a1682a09c50 | 482 | |
pommzorz | 0:9a1682a09c50 | 483 | |
pommzorz | 0:9a1682a09c50 | 484 | /** |
pommzorz | 0:9a1682a09c50 | 485 | * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between |
pommzorz | 0:9a1682a09c50 | 486 | * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch) |
pommzorz | 0:9a1682a09c50 | 487 | * and the Earth ground plane and the IMU Y axis. |
pommzorz | 0:9a1682a09c50 | 488 | * |
pommzorz | 0:9a1682a09c50 | 489 | * @note This is not an Euler representation: the rotations aren't consecutive rotations but only |
svkatielee | 3:450acaa4f52d | 490 | * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see IMU10DOF::getEuler |
pommzorz | 0:9a1682a09c50 | 491 | * |
pommzorz | 0:9a1682a09c50 | 492 | * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians |
pommzorz | 0:9a1682a09c50 | 493 | */ |
svkatielee | 3:450acaa4f52d | 494 | void IMU10DOF::getYawPitchRollRad(float * ypr) |
pommzorz | 0:9a1682a09c50 | 495 | { |
pommzorz | 0:9a1682a09c50 | 496 | float q[4]; // quaternion |
pommzorz | 0:9a1682a09c50 | 497 | float gx, gy, gz; // estimated gravity direction |
pommzorz | 0:9a1682a09c50 | 498 | getQ(q); |
pommzorz | 0:9a1682a09c50 | 499 | |
pommzorz | 0:9a1682a09c50 | 500 | gx = 2 * (q[1]*q[3] - q[0]*q[2]); |
pommzorz | 0:9a1682a09c50 | 501 | gy = 2 * (q[0]*q[1] + q[2]*q[3]); |
pommzorz | 0:9a1682a09c50 | 502 | gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; |
pommzorz | 0:9a1682a09c50 | 503 | |
pommzorz | 0:9a1682a09c50 | 504 | ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); |
pommzorz | 0:9a1682a09c50 | 505 | ypr[1] = atan(gx / sqrt(gy*gy + gz*gz)); |
pommzorz | 0:9a1682a09c50 | 506 | ypr[2] = atan(gy / sqrt(gx*gx + gz*gz)); |
pommzorz | 0:9a1682a09c50 | 507 | } |
pommzorz | 0:9a1682a09c50 | 508 | |
pommzorz | 0:9a1682a09c50 | 509 | |
pommzorz | 0:9a1682a09c50 | 510 | /** |
pommzorz | 0:9a1682a09c50 | 511 | * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between |
pommzorz | 0:9a1682a09c50 | 512 | * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch) |
pommzorz | 0:9a1682a09c50 | 513 | * and the Earth ground plane and the IMU Y axis. |
pommzorz | 0:9a1682a09c50 | 514 | * |
pommzorz | 0:9a1682a09c50 | 515 | * @note This is not an Euler representation: the rotations aren't consecutive rotations but only |
svkatielee | 3:450acaa4f52d | 516 | * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see IMU10DOF::getEuler |
pommzorz | 0:9a1682a09c50 | 517 | * |
pommzorz | 0:9a1682a09c50 | 518 | * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees |
pommzorz | 0:9a1682a09c50 | 519 | */ |
svkatielee | 3:450acaa4f52d | 520 | void IMU10DOF::getYawPitchRoll(float * ypr) |
pommzorz | 0:9a1682a09c50 | 521 | { |
pommzorz | 0:9a1682a09c50 | 522 | getYawPitchRollRad(ypr); |
pommzorz | 0:9a1682a09c50 | 523 | arr3_rad_to_deg(ypr); |
pommzorz | 0:9a1682a09c50 | 524 | } |
pommzorz | 0:9a1682a09c50 | 525 | |
pommzorz | 0:9a1682a09c50 | 526 | |
pommzorz | 0:9a1682a09c50 | 527 | /** |
pommzorz | 0:9a1682a09c50 | 528 | * Converts a 3 elements array arr of angles expressed in radians into degrees |
pommzorz | 0:9a1682a09c50 | 529 | */ |
pommzorz | 0:9a1682a09c50 | 530 | void arr3_rad_to_deg(float * arr) |
pommzorz | 0:9a1682a09c50 | 531 | { |
pommzorz | 0:9a1682a09c50 | 532 | arr[0] *= 180/M_PI; |
pommzorz | 0:9a1682a09c50 | 533 | arr[1] *= 180/M_PI; |
pommzorz | 0:9a1682a09c50 | 534 | arr[2] *= 180/M_PI; |
pommzorz | 0:9a1682a09c50 | 535 | } |
pommzorz | 0:9a1682a09c50 | 536 | |
pommzorz | 0:9a1682a09c50 | 537 | |
pommzorz | 0:9a1682a09c50 | 538 | /** |
pommzorz | 0:9a1682a09c50 | 539 | * Fast inverse square root implementation |
pommzorz | 0:9a1682a09c50 | 540 | * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root |
pommzorz | 0:9a1682a09c50 | 541 | */ |
pommzorz | 0:9a1682a09c50 | 542 | float invSqrt(float number) |
pommzorz | 0:9a1682a09c50 | 543 | { |
pommzorz | 0:9a1682a09c50 | 544 | volatile long i; |
pommzorz | 0:9a1682a09c50 | 545 | volatile float x, y; |
pommzorz | 0:9a1682a09c50 | 546 | volatile const float f = 1.5F; |
pommzorz | 0:9a1682a09c50 | 547 | |
pommzorz | 0:9a1682a09c50 | 548 | x = number * 0.5F; |
pommzorz | 0:9a1682a09c50 | 549 | y = number; |
pommzorz | 0:9a1682a09c50 | 550 | i = * ( long * ) &y; |
pommzorz | 0:9a1682a09c50 | 551 | i = 0x5f375a86 - ( i >> 1 ); |
pommzorz | 0:9a1682a09c50 | 552 | y = * ( float * ) &i; |
pommzorz | 0:9a1682a09c50 | 553 | y = y * ( f - ( x * y * y ) ); |
pommzorz | 0:9a1682a09c50 | 554 | return y; |
pommzorz | 0:9a1682a09c50 | 555 | } |
pommzorz | 0:9a1682a09c50 | 556 | |
pommzorz | 0:9a1682a09c50 | 557 | |
pommzorz | 0:9a1682a09c50 | 558 |