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