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