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@6:6b1185b32814, 2013-12-23 (annotated)

Committer:
tyftyftyf
Date:
Mon Dec 23 08:35:22 2013 +0000
Revision:
6:6b1185b32814
Parent:
3:f9b100a9aa65
Child:
8:cd43764b9623
Bug fixes

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