Aloïs Wolff / Mbed 2 deprecated MbedFreeIMU

quick and probably buggy port from the FreeIMU 0.4 library adapted for MBED and MPU6050 only...

Dependencies:   MPU6050_tmp mbed

FreeIMU.cpp@2:a79ea2f610a1, 2013-02-20 (annotated)

Committer:
pommzorz
Date:
Wed Feb 20 17:33:14 2013 +0000
Revision:
2:a79ea2f610a1
Parent:
0:c7a5b6fa0171 
added contact info

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pommzorz 0:c7a5b6fa0171 1 /*
pommzorz 0:c7a5b6fa0171 2 FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino
pommzorz 0:c7a5b6fa0171 3 Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net>
pommzorz 0:c7a5b6fa0171 4
pommzorz 0:c7a5b6fa0171 5 Development of this code has been supported by the Department of Computer Science,
pommzorz 0:c7a5b6fa0171 6 Universita' degli Studi di Torino, Italy within the Piemonte Project
pommzorz 0:c7a5b6fa0171 7 http://www.piemonte.di.unito.it/
pommzorz 0:c7a5b6fa0171 8
pommzorz 0:c7a5b6fa0171 9
pommzorz 0:c7a5b6fa0171 10 This program is free software: you can redistribute it and/or modify
pommzorz 0:c7a5b6fa0171 11 it under the terms of the version 3 GNU General Public License as
pommzorz 0:c7a5b6fa0171 12 published by the Free Software Foundation.
pommzorz 0:c7a5b6fa0171 13
pommzorz 0:c7a5b6fa0171 14 This program is distributed in the hope that it will be useful,
pommzorz 0:c7a5b6fa0171 15 but WITHOUT ANY WARRANTY; without even the implied warranty of
pommzorz 0:c7a5b6fa0171 16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
pommzorz 0:c7a5b6fa0171 17 GNU General Public License for more details.
pommzorz 0:c7a5b6fa0171 18
pommzorz 0:c7a5b6fa0171 19 You should have received a copy of the GNU General Public License
pommzorz 0:c7a5b6fa0171 20 along with this program. If not, see <http://www.gnu.org/licenses/>.
pommzorz 0:c7a5b6fa0171 21
pommzorz 2:a79ea2f610a1 22 02/20/2013 - Modified by Aloïs Wolff for MBED with MPU6050 only (wolffalois@gmail.com)
pommzorz 0:c7a5b6fa0171 23 */
pommzorz 0:c7a5b6fa0171 24
pommzorz 0:c7a5b6fa0171 25 //#include <inttypes.h>
pommzorz 0:c7a5b6fa0171 26 //#include <stdint.h>
pommzorz 0:c7a5b6fa0171 27 //#define DEBUG
pommzorz 0:c7a5b6fa0171 28 #include "FreeIMU.h"
pommzorz 0:c7a5b6fa0171 29 #define M_PI 3.1415926535897932384626433832795
pommzorz 0:c7a5b6fa0171 30
pommzorz 0:c7a5b6fa0171 31 #ifdef DEBUG
pommzorz 0:c7a5b6fa0171 32 #define DEBUG_PRINT(x) Serial.println(x)
pommzorz 0:c7a5b6fa0171 33 #else
pommzorz 0:c7a5b6fa0171 34 #define DEBUG_PRINT(x)
pommzorz 0:c7a5b6fa0171 35 #endif
pommzorz 0:c7a5b6fa0171 36 // #include "WireUtils.h"
pommzorz 0:c7a5b6fa0171 37 //#include "DebugUtils.h"
pommzorz 0:c7a5b6fa0171 38
pommzorz 0:c7a5b6fa0171 39 //#include "vector_math.h"
pommzorz 0:c7a5b6fa0171 40
pommzorz 0:c7a5b6fa0171 41 FreeIMU::FreeIMU() {
pommzorz 0:c7a5b6fa0171 42
pommzorz 0:c7a5b6fa0171 43 accgyro = MPU6050(0x69); // I2C
pommzorz 0:c7a5b6fa0171 44
pommzorz 0:c7a5b6fa0171 45 // initialize quaternion
pommzorz 0:c7a5b6fa0171 46 q0 = 1.0f;
pommzorz 0:c7a5b6fa0171 47 q1 = 0.0f;
pommzorz 0:c7a5b6fa0171 48 q2 = 0.0f;
pommzorz 0:c7a5b6fa0171 49 q3 = 0.0f;
pommzorz 0:c7a5b6fa0171 50 exInt = 0.0;
pommzorz 0:c7a5b6fa0171 51 eyInt = 0.0;
pommzorz 0:c7a5b6fa0171 52 ezInt = 0.0;
pommzorz 0:c7a5b6fa0171 53 twoKp = twoKpDef;
pommzorz 0:c7a5b6fa0171 54 twoKi = twoKiDef;
pommzorz 0:c7a5b6fa0171 55 integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
pommzorz 0:c7a5b6fa0171 56
pommzorz 0:c7a5b6fa0171 57
pommzorz 0:c7a5b6fa0171 58 update.start();
pommzorz 0:c7a5b6fa0171 59 int dt_us=0;
pommzorz 0:c7a5b6fa0171 60 /*
pommzorz 0:c7a5b6fa0171 61 lastUpdate = 0;
pommzorz 0:c7a5b6fa0171 62 now = 0;
pommzorz 0:c7a5b6fa0171 63 */
pommzorz 0:c7a5b6fa0171 64 #ifndef CALIBRATION_H
pommzorz 0:c7a5b6fa0171 65 // initialize scale factors to neutral values
pommzorz 0:c7a5b6fa0171 66 acc_scale_x = 1;
pommzorz 0:c7a5b6fa0171 67 acc_scale_y = 1;
pommzorz 0:c7a5b6fa0171 68 acc_scale_z = 1;
pommzorz 0:c7a5b6fa0171 69 magn_scale_x = 1;
pommzorz 0:c7a5b6fa0171 70 magn_scale_y = 1;
pommzorz 0:c7a5b6fa0171 71 magn_scale_z = 1;
pommzorz 0:c7a5b6fa0171 72 #else
pommzorz 0:c7a5b6fa0171 73 // get values from global variables of same name defined in calibration.h
pommzorz 0:c7a5b6fa0171 74 acc_off_x = ::acc_off_x;
pommzorz 0:c7a5b6fa0171 75 acc_off_y = ::acc_off_y;
pommzorz 0:c7a5b6fa0171 76 acc_off_z = ::acc_off_z;
pommzorz 0:c7a5b6fa0171 77 acc_scale_x = ::acc_scale_x;
pommzorz 0:c7a5b6fa0171 78 acc_scale_y = ::acc_scale_y;
pommzorz 0:c7a5b6fa0171 79 acc_scale_z = ::acc_scale_z;
pommzorz 0:c7a5b6fa0171 80 magn_off_x = ::magn_off_x;
pommzorz 0:c7a5b6fa0171 81 magn_off_y = ::magn_off_y;
pommzorz 0:c7a5b6fa0171 82 magn_off_z = ::magn_off_z;
pommzorz 0:c7a5b6fa0171 83 magn_scale_x = ::magn_scale_x;
pommzorz 0:c7a5b6fa0171 84 magn_scale_y = ::magn_scale_y;
pommzorz 0:c7a5b6fa0171 85 magn_scale_z = ::magn_scale_z;
pommzorz 0:c7a5b6fa0171 86 #endif
pommzorz 0:c7a5b6fa0171 87 }
pommzorz 0:c7a5b6fa0171 88
pommzorz 0:c7a5b6fa0171 89 void FreeIMU::init() {
pommzorz 0:c7a5b6fa0171 90
pommzorz 0:c7a5b6fa0171 91 init(FIMU_ACCGYRO_ADDR, false);
pommzorz 0:c7a5b6fa0171 92
pommzorz 0:c7a5b6fa0171 93 }
pommzorz 0:c7a5b6fa0171 94
pommzorz 0:c7a5b6fa0171 95 void FreeIMU::init(bool fastmode) {
pommzorz 0:c7a5b6fa0171 96
pommzorz 0:c7a5b6fa0171 97 init(FIMU_ACCGYRO_ADDR, fastmode);
pommzorz 0:c7a5b6fa0171 98
pommzorz 0:c7a5b6fa0171 99 }
pommzorz 0:c7a5b6fa0171 100
pommzorz 0:c7a5b6fa0171 101
pommzorz 0:c7a5b6fa0171 102 /**
pommzorz 0:c7a5b6fa0171 103 * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
pommzorz 0:c7a5b6fa0171 104 */
pommzorz 0:c7a5b6fa0171 105
pommzorz 0:c7a5b6fa0171 106 void FreeIMU::init(int accgyro_addr, bool fastmode) {
pommzorz 0:c7a5b6fa0171 107
pommzorz 0:c7a5b6fa0171 108 wait_ms(5);
pommzorz 0:c7a5b6fa0171 109 /*
pommzorz 0:c7a5b6fa0171 110 // disable internal pullups of the ATMEGA which Wire enable by default
pommzorz 0:c7a5b6fa0171 111 #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
pommzorz 0:c7a5b6fa0171 112 // deactivate internal pull-ups for twi
pommzorz 0:c7a5b6fa0171 113 // as per note from atmega8 manual pg167
pommzorz 0:c7a5b6fa0171 114 cbi(PORTC, 4);
pommzorz 0:c7a5b6fa0171 115 cbi(PORTC, 5);
pommzorz 0:c7a5b6fa0171 116 #else
pommzorz 0:c7a5b6fa0171 117 // deactivate internal pull-ups for twi
pommzorz 0:c7a5b6fa0171 118 // as per note from atmega128 manual pg204
pommzorz 0:c7a5b6fa0171 119 cbi(PORTD, 0);
pommzorz 0:c7a5b6fa0171 120 cbi(PORTD, 1);
pommzorz 0:c7a5b6fa0171 121 #endif
pommzorz 0:c7a5b6fa0171 122 */
pommzorz 0:c7a5b6fa0171 123
pommzorz 0:c7a5b6fa0171 124 /*
pommzorz 0:c7a5b6fa0171 125 if(fastmode) { // switch to 400KHz I2C - eheheh
pommzorz 0:c7a5b6fa0171 126 TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c
pommzorz 0:c7a5b6fa0171 127 }
pommzorz 0:c7a5b6fa0171 128 */
pommzorz 0:c7a5b6fa0171 129 //accgyro = MPU6050(false, accgyro_addr);
pommzorz 0:c7a5b6fa0171 130 accgyro = MPU6050(0x69);
pommzorz 0:c7a5b6fa0171 131 accgyro.initialize();
pommzorz 0:c7a5b6fa0171 132 accgyro.setI2CMasterModeEnabled(0);
pommzorz 0:c7a5b6fa0171 133 accgyro.setI2CBypassEnabled(1);
pommzorz 0:c7a5b6fa0171 134 accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
pommzorz 0:c7a5b6fa0171 135 wait_ms(5);
pommzorz 0:c7a5b6fa0171 136
pommzorz 0:c7a5b6fa0171 137
pommzorz 0:c7a5b6fa0171 138 // zero gyro
pommzorz 0:c7a5b6fa0171 139 zeroGyro();
pommzorz 0:c7a5b6fa0171 140
pommzorz 0:c7a5b6fa0171 141 #ifndef CALIBRATION_H
pommzorz 0:c7a5b6fa0171 142 // load calibration from eeprom
pommzorz 0:c7a5b6fa0171 143 calLoad();
pommzorz 0:c7a5b6fa0171 144 #endif
pommzorz 0:c7a5b6fa0171 145 }
pommzorz 0:c7a5b6fa0171 146 /*
pommzorz 0:c7a5b6fa0171 147 #ifndef CALIBRATION_H
pommzorz 0:c7a5b6fa0171 148
pommzorz 0:c7a5b6fa0171 149 static uint8_t location; // assuming ordered reads
pommzorz 0:c7a5b6fa0171 150
pommzorz 0:c7a5b6fa0171 151 void eeprom_read_var(uint8_t size, byte * var) {
pommzorz 0:c7a5b6fa0171 152 for(uint8_t i = 0; i<size; i++) {
pommzorz 0:c7a5b6fa0171 153 var[i] = EEPROM.read(location + i);
pommzorz 0:c7a5b6fa0171 154 }
pommzorz 0:c7a5b6fa0171 155 location += size;
pommzorz 0:c7a5b6fa0171 156 }
pommzorz 0:c7a5b6fa0171 157 */
pommzorz 0:c7a5b6fa0171 158 void FreeIMU::calLoad() {
pommzorz 0:c7a5b6fa0171 159 /*
pommzorz 0:c7a5b6fa0171 160 if(EEPROM.read(FREEIMU_EEPROM_BASE) == FREEIMU_EEPROM_SIGNATURE) { // check if signature is ok so we have good data
pommzorz 0:c7a5b6fa0171 161 location = FREEIMU_EEPROM_BASE + 1; // reset location
pommzorz 0:c7a5b6fa0171 162
pommzorz 0:c7a5b6fa0171 163 eeprom_read_var(sizeof(acc_off_x), (byte *) &acc_off_x);
pommzorz 0:c7a5b6fa0171 164 eeprom_read_var(sizeof(acc_off_y), (byte *) &acc_off_y);
pommzorz 0:c7a5b6fa0171 165 eeprom_read_var(sizeof(acc_off_z), (byte *) &acc_off_z);
pommzorz 0:c7a5b6fa0171 166
pommzorz 0:c7a5b6fa0171 167 eeprom_read_var(sizeof(magn_off_x), (byte *) &magn_off_x);
pommzorz 0:c7a5b6fa0171 168 eeprom_read_var(sizeof(magn_off_y), (byte *) &magn_off_y);
pommzorz 0:c7a5b6fa0171 169 eeprom_read_var(sizeof(magn_off_z), (byte *) &magn_off_z);
pommzorz 0:c7a5b6fa0171 170
pommzorz 0:c7a5b6fa0171 171 eeprom_read_var(sizeof(acc_scale_x), (byte *) &acc_scale_x);
pommzorz 0:c7a5b6fa0171 172 eeprom_read_var(sizeof(acc_scale_y), (byte *) &acc_scale_y);
pommzorz 0:c7a5b6fa0171 173 eeprom_read_var(sizeof(acc_scale_z), (byte *) &acc_scale_z);
pommzorz 0:c7a5b6fa0171 174
pommzorz 0:c7a5b6fa0171 175 eeprom_read_var(sizeof(magn_scale_x), (byte *) &magn_scale_x);
pommzorz 0:c7a5b6fa0171 176 eeprom_read_var(sizeof(magn_scale_y), (byte *) &magn_scale_y);
pommzorz 0:c7a5b6fa0171 177 eeprom_read_var(sizeof(magn_scale_z), (byte *) &magn_scale_z);
pommzorz 0:c7a5b6fa0171 178 }
pommzorz 0:c7a5b6fa0171 179 else {
pommzorz 0:c7a5b6fa0171 180 */
pommzorz 0:c7a5b6fa0171 181 acc_off_x = 0;
pommzorz 0:c7a5b6fa0171 182 acc_off_y = 0;
pommzorz 0:c7a5b6fa0171 183 acc_off_z = 0;
pommzorz 0:c7a5b6fa0171 184 acc_scale_x = 1;
pommzorz 0:c7a5b6fa0171 185 acc_scale_y = 1;
pommzorz 0:c7a5b6fa0171 186 acc_scale_z = 1;
pommzorz 0:c7a5b6fa0171 187
pommzorz 0:c7a5b6fa0171 188 magn_off_x = 0;
pommzorz 0:c7a5b6fa0171 189 magn_off_y = 0;
pommzorz 0:c7a5b6fa0171 190 magn_off_z = 0;
pommzorz 0:c7a5b6fa0171 191 magn_scale_x = 1;
pommzorz 0:c7a5b6fa0171 192 magn_scale_y = 1;
pommzorz 0:c7a5b6fa0171 193 magn_scale_z = 1;
pommzorz 0:c7a5b6fa0171 194 // }
pommzorz 0:c7a5b6fa0171 195 }
pommzorz 0:c7a5b6fa0171 196 //#endif
pommzorz 0:c7a5b6fa0171 197
pommzorz 0:c7a5b6fa0171 198 /**
pommzorz 0:c7a5b6fa0171 199 * Populates raw_values with the raw_values from the sensors
pommzorz 0:c7a5b6fa0171 200 */
pommzorz 0:c7a5b6fa0171 201 void FreeIMU::getRawValues(int16_t * raw_values) {
pommzorz 0:c7a5b6fa0171 202
pommzorz 0:c7a5b6fa0171 203 accgyro.getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
pommzorz 0:c7a5b6fa0171 204
pommzorz 0:c7a5b6fa0171 205 }
pommzorz 0:c7a5b6fa0171 206
pommzorz 0:c7a5b6fa0171 207
pommzorz 0:c7a5b6fa0171 208 /**
pommzorz 0:c7a5b6fa0171 209 * Populates values with calibrated readings from the sensors
pommzorz 0:c7a5b6fa0171 210 */
pommzorz 0:c7a5b6fa0171 211 void FreeIMU::getValues(float * values) {
pommzorz 0:c7a5b6fa0171 212
pommzorz 0:c7a5b6fa0171 213 // MPU6050
pommzorz 0:c7a5b6fa0171 214 int16_t accgyroval[6];
pommzorz 0:c7a5b6fa0171 215 accgyro.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
pommzorz 0:c7a5b6fa0171 216
pommzorz 0:c7a5b6fa0171 217 // remove offsets from the gyroscope
pommzorz 0:c7a5b6fa0171 218 accgyroval[3] = accgyroval[3] - gyro_off_x;
pommzorz 0:c7a5b6fa0171 219 accgyroval[4] = accgyroval[4] - gyro_off_y;
pommzorz 0:c7a5b6fa0171 220 accgyroval[5] = accgyroval[5] - gyro_off_z;
pommzorz 0:c7a5b6fa0171 221
pommzorz 0:c7a5b6fa0171 222 for(int i = 0; i<6; i++) {
pommzorz 0:c7a5b6fa0171 223 if(i < 3) {
pommzorz 0:c7a5b6fa0171 224 values[i] = (float) accgyroval[i];
pommzorz 0:c7a5b6fa0171 225 }
pommzorz 0:c7a5b6fa0171 226 else {
pommzorz 0:c7a5b6fa0171 227 values[i] = ((float) accgyroval[i]) / 16.4f; // NOTE: this depends on the sensitivity chosen
pommzorz 0:c7a5b6fa0171 228 }
pommzorz 0:c7a5b6fa0171 229 }
pommzorz 0:c7a5b6fa0171 230
pommzorz 0:c7a5b6fa0171 231
pommzorz 0:c7a5b6fa0171 232
pommzorz 0:c7a5b6fa0171 233 #warning Accelerometer calibration active: have you calibrated your device?
pommzorz 0:c7a5b6fa0171 234 // remove offsets and scale accelerometer (calibration)
pommzorz 0:c7a5b6fa0171 235 values[0] = (values[0] - acc_off_x) / acc_scale_x;
pommzorz 0:c7a5b6fa0171 236 values[1] = (values[1] - acc_off_y) / acc_scale_y;
pommzorz 0:c7a5b6fa0171 237 values[2] = (values[2] - acc_off_z) / acc_scale_z;
pommzorz 0:c7a5b6fa0171 238
pommzorz 0:c7a5b6fa0171 239
pommzorz 0:c7a5b6fa0171 240
pommzorz 0:c7a5b6fa0171 241 }
pommzorz 0:c7a5b6fa0171 242
pommzorz 0:c7a5b6fa0171 243
pommzorz 0:c7a5b6fa0171 244 /**
pommzorz 0:c7a5b6fa0171 245 * Computes gyro offsets
pommzorz 0:c7a5b6fa0171 246 */
pommzorz 0:c7a5b6fa0171 247 void FreeIMU::zeroGyro() {
pommzorz 0:c7a5b6fa0171 248 const int totSamples = 3;
pommzorz 0:c7a5b6fa0171 249 int16_t raw[11];
pommzorz 0:c7a5b6fa0171 250 float tmpOffsets[] = {0,0,0};
pommzorz 0:c7a5b6fa0171 251
pommzorz 0:c7a5b6fa0171 252 for (int i = 0; i < totSamples; i++){
pommzorz 0:c7a5b6fa0171 253 getRawValues(raw);
pommzorz 0:c7a5b6fa0171 254 tmpOffsets[0] += raw[3];
pommzorz 0:c7a5b6fa0171 255 tmpOffsets[1] += raw[4];
pommzorz 0:c7a5b6fa0171 256 tmpOffsets[2] += raw[5];
pommzorz 0:c7a5b6fa0171 257 }
pommzorz 0:c7a5b6fa0171 258
pommzorz 0:c7a5b6fa0171 259 gyro_off_x = tmpOffsets[0] / totSamples;
pommzorz 0:c7a5b6fa0171 260 gyro_off_y = tmpOffsets[1] / totSamples;
pommzorz 0:c7a5b6fa0171 261 gyro_off_z = tmpOffsets[2] / totSamples;
pommzorz 0:c7a5b6fa0171 262 }
pommzorz 0:c7a5b6fa0171 263
pommzorz 0:c7a5b6fa0171 264
pommzorz 0:c7a5b6fa0171 265 /**
pommzorz 0:c7a5b6fa0171 266 * Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion
pommzorz 0:c7a5b6fa0171 267 * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
pommzorz 0:c7a5b6fa0171 268 * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
pommzorz 0:c7a5b6fa0171 269 * axis only.
pommzorz 0:c7a5b6fa0171 270 *
pommzorz 0:c7a5b6fa0171 271 * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
pommzorz 0:c7a5b6fa0171 272 */
pommzorz 0:c7a5b6fa0171 273
pommzorz 0:c7a5b6fa0171 274 void FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) {
pommzorz 0:c7a5b6fa0171 275
pommzorz 0:c7a5b6fa0171 276 float recipNorm;
pommzorz 0:c7a5b6fa0171 277 float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
pommzorz 0:c7a5b6fa0171 278 float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
pommzorz 0:c7a5b6fa0171 279 float qa, qb, qc;
pommzorz 0:c7a5b6fa0171 280
pommzorz 0:c7a5b6fa0171 281 // Auxiliary variables to avoid repeated arithmetic
pommzorz 0:c7a5b6fa0171 282 q0q0 = q0 * q0;
pommzorz 0:c7a5b6fa0171 283 q0q1 = q0 * q1;
pommzorz 0:c7a5b6fa0171 284 q0q2 = q0 * q2;
pommzorz 0:c7a5b6fa0171 285 q0q3 = q0 * q3;
pommzorz 0:c7a5b6fa0171 286 q1q1 = q1 * q1;
pommzorz 0:c7a5b6fa0171 287 q1q2 = q1 * q2;
pommzorz 0:c7a5b6fa0171 288 q1q3 = q1 * q3;
pommzorz 0:c7a5b6fa0171 289 q2q2 = q2 * q2;
pommzorz 0:c7a5b6fa0171 290 q2q3 = q2 * q3;
pommzorz 0:c7a5b6fa0171 291 q3q3 = q3 * q3;
pommzorz 0:c7a5b6fa0171 292
pommzorz 0:c7a5b6fa0171 293
pommzorz 0:c7a5b6fa0171 294
pommzorz 0:c7a5b6fa0171 295 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
pommzorz 0:c7a5b6fa0171 296 if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
pommzorz 0:c7a5b6fa0171 297 float halfvx, halfvy, halfvz;
pommzorz 0:c7a5b6fa0171 298
pommzorz 0:c7a5b6fa0171 299 // Normalise accelerometer measurement
pommzorz 0:c7a5b6fa0171 300 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
pommzorz 0:c7a5b6fa0171 301 ax *= recipNorm;
pommzorz 0:c7a5b6fa0171 302 ay *= recipNorm;
pommzorz 0:c7a5b6fa0171 303 az *= recipNorm;
pommzorz 0:c7a5b6fa0171 304
pommzorz 0:c7a5b6fa0171 305 // Estimated direction of gravity
pommzorz 0:c7a5b6fa0171 306 halfvx = q1q3 - q0q2;
pommzorz 0:c7a5b6fa0171 307 halfvy = q0q1 + q2q3;
pommzorz 0:c7a5b6fa0171 308 halfvz = q0q0 - 0.5f + q3q3;
pommzorz 0:c7a5b6fa0171 309
pommzorz 0:c7a5b6fa0171 310 // Error is sum of cross product between estimated direction and measured direction of field vectors
pommzorz 0:c7a5b6fa0171 311 halfex += (ay * halfvz - az * halfvy);
pommzorz 0:c7a5b6fa0171 312 halfey += (az * halfvx - ax * halfvz);
pommzorz 0:c7a5b6fa0171 313 halfez += (ax * halfvy - ay * halfvx);
pommzorz 0:c7a5b6fa0171 314 }
pommzorz 0:c7a5b6fa0171 315
pommzorz 0:c7a5b6fa0171 316 // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
pommzorz 0:c7a5b6fa0171 317 if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
pommzorz 0:c7a5b6fa0171 318 // Compute and apply integral feedback if enabled
pommzorz 0:c7a5b6fa0171 319 if(twoKi > 0.0f) {
pommzorz 0:c7a5b6fa0171 320 integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
pommzorz 0:c7a5b6fa0171 321 integralFBy += twoKi * halfey * (1.0f / sampleFreq);
pommzorz 0:c7a5b6fa0171 322 integralFBz += twoKi * halfez * (1.0f / sampleFreq);
pommzorz 0:c7a5b6fa0171 323 gx += integralFBx; // apply integral feedback
pommzorz 0:c7a5b6fa0171 324 gy += integralFBy;
pommzorz 0:c7a5b6fa0171 325 gz += integralFBz;
pommzorz 0:c7a5b6fa0171 326 }
pommzorz 0:c7a5b6fa0171 327 else {
pommzorz 0:c7a5b6fa0171 328 integralFBx = 0.0f; // prevent integral windup
pommzorz 0:c7a5b6fa0171 329 integralFBy = 0.0f;
pommzorz 0:c7a5b6fa0171 330 integralFBz = 0.0f;
pommzorz 0:c7a5b6fa0171 331 }
pommzorz 0:c7a5b6fa0171 332
pommzorz 0:c7a5b6fa0171 333 // Apply proportional feedback
pommzorz 0:c7a5b6fa0171 334 gx += twoKp * halfex;
pommzorz 0:c7a5b6fa0171 335 gy += twoKp * halfey;
pommzorz 0:c7a5b6fa0171 336 gz += twoKp * halfez;
pommzorz 0:c7a5b6fa0171 337 }
pommzorz 0:c7a5b6fa0171 338
pommzorz 0:c7a5b6fa0171 339 // Integrate rate of change of quaternion
pommzorz 0:c7a5b6fa0171 340 gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
pommzorz 0:c7a5b6fa0171 341 gy *= (0.5f * (1.0f / sampleFreq));
pommzorz 0:c7a5b6fa0171 342 gz *= (0.5f * (1.0f / sampleFreq));
pommzorz 0:c7a5b6fa0171 343 qa = q0;
pommzorz 0:c7a5b6fa0171 344 qb = q1;
pommzorz 0:c7a5b6fa0171 345 qc = q2;
pommzorz 0:c7a5b6fa0171 346 q0 += (-qb * gx - qc * gy - q3 * gz);
pommzorz 0:c7a5b6fa0171 347 q1 += (qa * gx + qc * gz - q3 * gy);
pommzorz 0:c7a5b6fa0171 348 q2 += (qa * gy - qb * gz + q3 * gx);
pommzorz 0:c7a5b6fa0171 349 q3 += (qa * gz + qb * gy - qc * gx);
pommzorz 0:c7a5b6fa0171 350
pommzorz 0:c7a5b6fa0171 351 // Normalise quaternion
pommzorz 0:c7a5b6fa0171 352 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
pommzorz 0:c7a5b6fa0171 353 q0 *= recipNorm;
pommzorz 0:c7a5b6fa0171 354 q1 *= recipNorm;
pommzorz 0:c7a5b6fa0171 355 q2 *= recipNorm;
pommzorz 0:c7a5b6fa0171 356 q3 *= recipNorm;
pommzorz 0:c7a5b6fa0171 357 }
pommzorz 0:c7a5b6fa0171 358
pommzorz 0:c7a5b6fa0171 359
pommzorz 0:c7a5b6fa0171 360 /**
pommzorz 0:c7a5b6fa0171 361 * Populates array q with a quaternion representing the IMU orientation with respect to the Earth
pommzorz 0:c7a5b6fa0171 362 *
pommzorz 0:c7a5b6fa0171 363 * @param q the quaternion to populate
pommzorz 0:c7a5b6fa0171 364 */
pommzorz 0:c7a5b6fa0171 365 void FreeIMU::getQ(float * q) {
pommzorz 0:c7a5b6fa0171 366 float val[9];
pommzorz 0:c7a5b6fa0171 367 getValues(val);
pommzorz 0:c7a5b6fa0171 368
pommzorz 0:c7a5b6fa0171 369 DEBUG_PRINT(val[3] * M_PI/180);
pommzorz 0:c7a5b6fa0171 370 DEBUG_PRINT(val[4] * M_PI/180);
pommzorz 0:c7a5b6fa0171 371 DEBUG_PRINT(val[5] * M_PI/180);
pommzorz 0:c7a5b6fa0171 372 DEBUG_PRINT(val[0]);
pommzorz 0:c7a5b6fa0171 373 DEBUG_PRINT(val[1]);
pommzorz 0:c7a5b6fa0171 374 DEBUG_PRINT(val[2]);
pommzorz 0:c7a5b6fa0171 375 DEBUG_PRINT(val[6]);
pommzorz 0:c7a5b6fa0171 376 DEBUG_PRINT(val[7]);
pommzorz 0:c7a5b6fa0171 377 DEBUG_PRINT(val[8]);
pommzorz 0:c7a5b6fa0171 378
pommzorz 0:c7a5b6fa0171 379 //now = micros();
pommzorz 0:c7a5b6fa0171 380 dt_us=update.read_us();
pommzorz 0:c7a5b6fa0171 381 sampleFreq = 1.0 / ((dt_us) / 1000000.0);
pommzorz 0:c7a5b6fa0171 382 update.reset();
pommzorz 0:c7a5b6fa0171 383 // lastUpdate = now;
pommzorz 0:c7a5b6fa0171 384 // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
pommzorz 0:c7a5b6fa0171 385
pommzorz 0:c7a5b6fa0171 386 AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);
pommzorz 0:c7a5b6fa0171 387
pommzorz 0:c7a5b6fa0171 388
pommzorz 0:c7a5b6fa0171 389 q[0] = q0;
pommzorz 0:c7a5b6fa0171 390 q[1] = q1;
pommzorz 0:c7a5b6fa0171 391 q[2] = q2;
pommzorz 0:c7a5b6fa0171 392 q[3] = q3;
pommzorz 0:c7a5b6fa0171 393 }
pommzorz 0:c7a5b6fa0171 394
pommzorz 0:c7a5b6fa0171 395
pommzorz 0:c7a5b6fa0171 396
pommzorz 0:c7a5b6fa0171 397 /**
pommzorz 0:c7a5b6fa0171 398 * Returns the Euler angles in radians defined in the Aerospace sequence.
pommzorz 0:c7a5b6fa0171 399 * See Sebastian O.H. Madwick report "An efficient orientation filter for
pommzorz 0:c7a5b6fa0171 400 * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
pommzorz 0:c7a5b6fa0171 401 *
pommzorz 0:c7a5b6fa0171 402 * @param angles three floats array which will be populated by the Euler angles in radians
pommzorz 0:c7a5b6fa0171 403 */
pommzorz 0:c7a5b6fa0171 404 void FreeIMU::getEulerRad(float * angles) {
pommzorz 0:c7a5b6fa0171 405 float q[4]; // quaternion
pommzorz 0:c7a5b6fa0171 406 getQ(q);
pommzorz 0:c7a5b6fa0171 407 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
pommzorz 0:c7a5b6fa0171 408 angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
pommzorz 0:c7a5b6fa0171 409 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
pommzorz 0:c7a5b6fa0171 410 }
pommzorz 0:c7a5b6fa0171 411
pommzorz 0:c7a5b6fa0171 412
pommzorz 0:c7a5b6fa0171 413 /**
pommzorz 0:c7a5b6fa0171 414 * Returns the Euler angles in degrees defined with the Aerospace sequence.
pommzorz 0:c7a5b6fa0171 415 * See Sebastian O.H. Madwick report "An efficient orientation filter for
pommzorz 0:c7a5b6fa0171 416 * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
pommzorz 0:c7a5b6fa0171 417 *
pommzorz 0:c7a5b6fa0171 418 * @param angles three floats array which will be populated by the Euler angles in degrees
pommzorz 0:c7a5b6fa0171 419 */
pommzorz 0:c7a5b6fa0171 420 void FreeIMU::getEuler(float * angles) {
pommzorz 0:c7a5b6fa0171 421 getEulerRad(angles);
pommzorz 0:c7a5b6fa0171 422 arr3_rad_to_deg(angles);
pommzorz 0:c7a5b6fa0171 423 }
pommzorz 0:c7a5b6fa0171 424
pommzorz 0:c7a5b6fa0171 425
pommzorz 0:c7a5b6fa0171 426 /**
pommzorz 0:c7a5b6fa0171 427 * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
pommzorz 0:c7a5b6fa0171 428 * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
pommzorz 0:c7a5b6fa0171 429 * and the Earth ground plane and the IMU Y axis.
pommzorz 0:c7a5b6fa0171 430 *
pommzorz 0:c7a5b6fa0171 431 * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
pommzorz 0:c7a5b6fa0171 432 * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
pommzorz 0:c7a5b6fa0171 433 *
pommzorz 0:c7a5b6fa0171 434 * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians
pommzorz 0:c7a5b6fa0171 435 */
pommzorz 0:c7a5b6fa0171 436 void FreeIMU::getYawPitchRollRad(float * ypr) {
pommzorz 0:c7a5b6fa0171 437 float q[4]; // quaternion
pommzorz 0:c7a5b6fa0171 438 float gx, gy, gz; // estimated gravity direction
pommzorz 0:c7a5b6fa0171 439 getQ(q);
pommzorz 0:c7a5b6fa0171 440
pommzorz 0:c7a5b6fa0171 441 gx = 2 * (q[1]*q[3] - q[0]*q[2]);
pommzorz 0:c7a5b6fa0171 442 gy = 2 * (q[0]*q[1] + q[2]*q[3]);
pommzorz 0:c7a5b6fa0171 443 gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
pommzorz 0:c7a5b6fa0171 444
pommzorz 0:c7a5b6fa0171 445 ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
pommzorz 0:c7a5b6fa0171 446 ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
pommzorz 0:c7a5b6fa0171 447 ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
pommzorz 0:c7a5b6fa0171 448 }
pommzorz 0:c7a5b6fa0171 449
pommzorz 0:c7a5b6fa0171 450
pommzorz 0:c7a5b6fa0171 451 /**
pommzorz 0:c7a5b6fa0171 452 * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between
pommzorz 0:c7a5b6fa0171 453 * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
pommzorz 0:c7a5b6fa0171 454 * and the Earth ground plane and the IMU Y axis.
pommzorz 0:c7a5b6fa0171 455 *
pommzorz 0:c7a5b6fa0171 456 * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
pommzorz 0:c7a5b6fa0171 457 * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
pommzorz 0:c7a5b6fa0171 458 *
pommzorz 0:c7a5b6fa0171 459 * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees
pommzorz 0:c7a5b6fa0171 460 */
pommzorz 0:c7a5b6fa0171 461 void FreeIMU::getYawPitchRoll(float * ypr) {
pommzorz 0:c7a5b6fa0171 462 getYawPitchRollRad(ypr);
pommzorz 0:c7a5b6fa0171 463 arr3_rad_to_deg(ypr);
pommzorz 0:c7a5b6fa0171 464 }
pommzorz 0:c7a5b6fa0171 465
pommzorz 0:c7a5b6fa0171 466
pommzorz 0:c7a5b6fa0171 467 /**
pommzorz 0:c7a5b6fa0171 468 * Converts a 3 elements array arr of angles expressed in radians into degrees
pommzorz 0:c7a5b6fa0171 469 */
pommzorz 0:c7a5b6fa0171 470 void arr3_rad_to_deg(float * arr) {
pommzorz 0:c7a5b6fa0171 471 arr[0] *= 180/M_PI;
pommzorz 0:c7a5b6fa0171 472 arr[1] *= 180/M_PI;
pommzorz 0:c7a5b6fa0171 473 arr[2] *= 180/M_PI;
pommzorz 0:c7a5b6fa0171 474 }
pommzorz 0:c7a5b6fa0171 475
pommzorz 0:c7a5b6fa0171 476
pommzorz 0:c7a5b6fa0171 477 /**
pommzorz 0:c7a5b6fa0171 478 * Fast inverse square root implementation
pommzorz 0:c7a5b6fa0171 479 * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root
pommzorz 0:c7a5b6fa0171 480 */
pommzorz 0:c7a5b6fa0171 481 float invSqrt(float number) {
pommzorz 0:c7a5b6fa0171 482 volatile long i;
pommzorz 0:c7a5b6fa0171 483 volatile float x, y;
pommzorz 0:c7a5b6fa0171 484 volatile const float f = 1.5F;
pommzorz 0:c7a5b6fa0171 485
pommzorz 0:c7a5b6fa0171 486 x = number * 0.5F;
pommzorz 0:c7a5b6fa0171 487 y = number;
pommzorz 0:c7a5b6fa0171 488 i = * ( long * ) &y;
pommzorz 0:c7a5b6fa0171 489 i = 0x5f375a86 - ( i >> 1 );
pommzorz 0:c7a5b6fa0171 490 y = * ( float * ) &i;
pommzorz 0:c7a5b6fa0171 491 y = y * ( f - ( x * y * y ) );
pommzorz 0:c7a5b6fa0171 492 return y;
pommzorz 0:c7a5b6fa0171 493 }
pommzorz 0:c7a5b6fa0171 494
pommzorz 0:c7a5b6fa0171 495
pommzorz 0:c7a5b6fa0171 496
pommzorz 0:c7a5b6fa0171 497