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@3:f9b100a9aa65, 2013-11-09 (annotated)

Committer:
tyftyftyf
Date:
Sat Nov 09 08:53:25 2013 +0000
Revision:
3:f9b100a9aa65
Parent:
2:5c419926dcd7
Child:
6:6b1185b32814
Implemented async mode. 1/10 the previous round time. Capable of sampling at 1Khz. +/- 0.05 deg accuracy

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