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