10DOF FreeIMU port for FreeIMU v4 board and GY-86. This library was modified extensively to specifically suit the Mbed platform. Used threads and interrupts to achieve async mode.

Dependencies:   HMC58X3 AK8963 MS561101BA MODI2C MPU9250

Dependents:   MTQuadControl FreeIMU_serial FreeIMU_demo

Port of FreeIMU library from Arduino to Mbed

10DOF FreeIMU port for FreeIMU v4 board and GY-86. This library was modified extensively to specifically suit the Mbed platform. Maximum sampling rate of 500hz can be achieved using this library.

Improvements

Sensor fusion algorithm fast initialization

This library implements the ARHS hot start algorithm, meaning that you can get accurate readings seconds after the algorithm is started, much faster than the Arduino version, where outputs slowly converge to the correct value in about a minute.

Caching

Sensors are read at their maximum output rates. Read values are cached hence multiple consecutive queries will not cause multiple reads.

Fully async

Acc & Gyro reads are performed via timer interrupts. Magnetometer and barometer are read by RTOS thread. No interfering with main program logic.

Usage

Declare a global FreeIMU object like the one below. There should only be one FreeIMU instance existing at a time.

#include "mbed.h"
#include "FreeIMU.h"
FreeIMU imu;

int main(){
    imu.init(true);
}

Then, anywhere in the code, you may call imu.getQ(q) to get the quarternion, where q is an array of 4 floats representing the quarternion structure.

You are recommended to call getQ frequently to keep the filter updated. However, the frequency should not exceed 500hz to avoid redundant calculation. One way to do this is by using the RtosTimer:

void getIMUdata(void const *);     //method definition

//in main
RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)NULL);
IMUTimer.start(2);     //1 / 2ms = 500hz

//getIMUdata function
void getIMUdata(void const *dummy){
    imu.getQ(NULL);
}
Committer:
tyftyftyf
Date:
Thu Mar 29 22:07:11 2018 +0000
Revision:
27:d1042e848b07
Parent:
24:dc802ce22e75
Child:
28:de24fce0509a
wip

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