Larry Littlefield / IMU10DOF

IMU 10dof MEMS from DR Robot adapted from HK10DOF Changed gyro to ITG3200

Fork of HK10DOF by Aloïs Wolff

WARNING: This project is not complete, but this library seems ok so far.

I have the module DFRobotics.com 10DOF MEMS IMU. I wanted a concise module for resolving direction and movement.

I found HK10DOF library (http://developer.mbed.org/users/pommzorz/code/HK10DOF/) with quaternions. But it used a different gyro. So I modified that code to use the same higher level calls but use the ITG3200 low level calls.

IMU10DOF.cpp@4:c4db4e2ffdd7, 2014-11-18 (annotated)

Committer:
svkatielee
Date:
Tue Nov 18 06:28:56 2014 +0000
Revision:
4:c4db4e2ffdd7
Parent:
3:450acaa4f52d 
Changed gyro sensor to ITG3200

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pommzorz 0:9a1682a09c50 1 /*
pommzorz 0:9a1682a09c50 2 FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino
pommzorz 0:9a1682a09c50 3 Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net>
pommzorz 0:9a1682a09c50 4 ported for HobbyKing's 10DOF stick on a mbed platform by Aloïs Wolff(wolffalois@gmail.com)
pommzorz 0:9a1682a09c50 5
pommzorz 0:9a1682a09c50 6 Development of this code has been supported by the Department of Computer Science,
pommzorz 0:9a1682a09c50 7 Universita' degli Studi di Torino, Italy within the Piemonte Project
pommzorz 0:9a1682a09c50 8 http://www.piemonte.di.unito.it/
pommzorz 0:9a1682a09c50 9
pommzorz 0:9a1682a09c50 10
pommzorz 0:9a1682a09c50 11 This program is free software: you can redistribute it and/or modify
pommzorz 0:9a1682a09c50 12 it under the terms of the version 3 GNU General Public License as
pommzorz 0:9a1682a09c50 13 published by the Free Software Foundation.
pommzorz 0:9a1682a09c50 14
pommzorz 0:9a1682a09c50 15 This program is distributed in the hope that it will be useful,
pommzorz 0:9a1682a09c50 16 but WITHOUT ANY WARRANTY; without even the implied warranty of
pommzorz 0:9a1682a09c50 17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
pommzorz 0:9a1682a09c50 18 GNU General Public License for more details.
pommzorz 0:9a1682a09c50 19
pommzorz 0:9a1682a09c50 20 You should have received a copy of the GNU General Public License
pommzorz 0:9a1682a09c50 21 along with this program. If not, see <http://www.gnu.org/licenses/>.
pommzorz 0:9a1682a09c50 22
svkatielee 3:450acaa4f52d 23 Now ported to support DFRobot.com 10DOF MEMS IMU
svkatielee 3:450acaa4f52d 24 by Larry Littlefield svkatielee@yahoo.com
svkatielee 3:450acaa4f52d 25
pommzorz 0:9a1682a09c50 26 */
pommzorz 0:9a1682a09c50 27
pommzorz 0:9a1682a09c50 28
svkatielee 3:450acaa4f52d 29
pommzorz 0:9a1682a09c50 30 //#define DEBUG
pommzorz 0:9a1682a09c50 31
pommzorz 0:9a1682a09c50 32
svkatielee 3:450acaa4f52d 33 #include "IMU10DOF.h"
svkatielee 3:450acaa4f52d 34 #include <math.h>
pommzorz 0:9a1682a09c50 35 #include "helper_3dmath.h"
svkatielee 3:450acaa4f52d 36
pommzorz 0:9a1682a09c50 37
pommzorz 0:9a1682a09c50 38 // #include "WireUtils.h"
pommzorz 0:9a1682a09c50 39 //#include "DebugUtils.h"
pommzorz 0:9a1682a09c50 40 //#include "vector_math.h"
pommzorz 0:9a1682a09c50 41
svkatielee 3:450acaa4f52d 42 #ifndef M_PI
pommzorz 0:9a1682a09c50 43 #define M_PI 3.1415926535897932384626433832795
svkatielee 3:450acaa4f52d 44 #endif
pommzorz 0:9a1682a09c50 45
svkatielee 3:450acaa4f52d 46 IMU10DOF::IMU10DOF(PinName sda, PinName scl) : acc(sda,scl), magn(sda,scl),gyro(sda,scl), baro(sda,scl),pc(USBTX,USBRX)
pommzorz 0:9a1682a09c50 47 {
pommzorz 0:9a1682a09c50 48
svkatielee 3:450acaa4f52d 49 pc.baud(115200);
pommzorz 0:9a1682a09c50 50
pommzorz 0:9a1682a09c50 51 // initialize quaternion
pommzorz 0:9a1682a09c50 52 q0 = 1.0f;
pommzorz 0:9a1682a09c50 53 q1 = 0.0f;
pommzorz 0:9a1682a09c50 54 q2 = 0.0f;
pommzorz 0:9a1682a09c50 55 q3 = 0.0f;
pommzorz 0:9a1682a09c50 56 exInt = 0.0;
pommzorz 0:9a1682a09c50 57 eyInt = 0.0;
pommzorz 0:9a1682a09c50 58 ezInt = 0.0;
pommzorz 0:9a1682a09c50 59 twoKp = twoKpDef;
pommzorz 0:9a1682a09c50 60 twoKi = twoKiDef;
pommzorz 0:9a1682a09c50 61 integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
pommzorz 0:9a1682a09c50 62
pommzorz 0:9a1682a09c50 63
pommzorz 0:9a1682a09c50 64 #ifndef CALIBRATION_H
pommzorz 0:9a1682a09c50 65 // initialize scale factors to neutral values
pommzorz 0:9a1682a09c50 66 acc_scale_x = 1;
pommzorz 0:9a1682a09c50 67 acc_scale_y = 1;
pommzorz 0:9a1682a09c50 68 acc_scale_z = 1;
pommzorz 0:9a1682a09c50 69 magn_scale_x = 1;
pommzorz 0:9a1682a09c50 70 magn_scale_y = 1;
pommzorz 0:9a1682a09c50 71 magn_scale_z = 1;
pommzorz 0:9a1682a09c50 72 #else
pommzorz 0:9a1682a09c50 73 // get values from global variables of same name defined in calibration.h
pommzorz 0:9a1682a09c50 74 acc_off_x = ::acc_off_x;
pommzorz 0:9a1682a09c50 75 acc_off_y = ::acc_off_y;
pommzorz 0:9a1682a09c50 76 acc_off_z = ::acc_off_z;
pommzorz 0:9a1682a09c50 77 acc_scale_x = ::acc_scale_x;
pommzorz 0:9a1682a09c50 78 acc_scale_y = ::acc_scale_y;
pommzorz 0:9a1682a09c50 79 acc_scale_z = ::acc_scale_z;
pommzorz 0:9a1682a09c50 80 magn_off_x = ::magn_off_x;
pommzorz 0:9a1682a09c50 81 magn_off_y = ::magn_off_y;
pommzorz 0:9a1682a09c50 82 magn_off_z = ::magn_off_z;
pommzorz 0:9a1682a09c50 83 magn_scale_x = ::magn_scale_x;
pommzorz 0:9a1682a09c50 84 magn_scale_y = ::magn_scale_y;
pommzorz 0:9a1682a09c50 85 magn_scale_z = ::magn_scale_z;
pommzorz 0:9a1682a09c50 86 #endif
pommzorz 0:9a1682a09c50 87 }
pommzorz 0:9a1682a09c50 88
pommzorz 0:9a1682a09c50 89
pommzorz 0:9a1682a09c50 90
pommzorz 0:9a1682a09c50 91
pommzorz 0:9a1682a09c50 92 /**
pommzorz 0:9a1682a09c50 93 * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
pommzorz 0:9a1682a09c50 94 */
pommzorz 0:9a1682a09c50 95
svkatielee 3:450acaa4f52d 96 void IMU10DOF::init(bool fastmode)
pommzorz 0:9a1682a09c50 97 {
pommzorz 0:9a1682a09c50 98
pommzorz 0:9a1682a09c50 99
pommzorz 0:9a1682a09c50 100 //Sensors Initialization
pommzorz 0:9a1682a09c50 101 pc.printf("Initializing sensors...\n\r");
pommzorz 0:9a1682a09c50 102 magn.init();
pommzorz 0:9a1682a09c50 103 baro.getCalibrationData();
pommzorz 0:9a1682a09c50 104 acc.setPowerControl(0x00);
pommzorz 0:9a1682a09c50 105 wait_ms(10);
pommzorz 0:9a1682a09c50 106 acc.setDataFormatControl(0x0B);
pommzorz 0:9a1682a09c50 107 wait_ms(10);
pommzorz 0:9a1682a09c50 108 acc.setPowerControl(MeasurementMode);
pommzorz 0:9a1682a09c50 109 wait_ms(10);
svkatielee 3:450acaa4f52d 110 gyro.init();
pommzorz 0:9a1682a09c50 111 pc.printf("Sensors OK!\n\r");
pommzorz 0:9a1682a09c50 112
pommzorz 0:9a1682a09c50 113 update.start();
pommzorz 0:9a1682a09c50 114 int dt_us=0;
pommzorz 0:9a1682a09c50 115 pc.printf("ZeroGyro\n\r");
pommzorz 0:9a1682a09c50 116 // zero gyro
pommzorz 0:9a1682a09c50 117 //zeroGyro();
pommzorz 0:9a1682a09c50 118 gyro.zeroCalibrate(128,5);
pommzorz 0:9a1682a09c50 119 pc.printf("ZeroGyro OK, CalLoad...\n\r");
pommzorz 0:9a1682a09c50 120 // load calibration from eeprom
pommzorz 0:9a1682a09c50 121 calLoad();
pommzorz 0:9a1682a09c50 122
pommzorz 0:9a1682a09c50 123 }
pommzorz 0:9a1682a09c50 124
svkatielee 3:450acaa4f52d 125 void IMU10DOF::calLoad()
pommzorz 0:9a1682a09c50 126 {
pommzorz 0:9a1682a09c50 127
pommzorz 0:9a1682a09c50 128
pommzorz 0:9a1682a09c50 129 acc_off_x = 0;
pommzorz 0:9a1682a09c50 130 acc_off_y = 0;
pommzorz 0:9a1682a09c50 131 acc_off_z = 0;
pommzorz 0:9a1682a09c50 132 acc_scale_x = 1;
pommzorz 0:9a1682a09c50 133 acc_scale_y = 1;
pommzorz 0:9a1682a09c50 134 acc_scale_z = 1;
pommzorz 0:9a1682a09c50 135
svkatielee 3:450acaa4f52d 136 magn_off_x = 0.21;
svkatielee 3:450acaa4f52d 137 magn_off_y = -0.175;
pommzorz 0:9a1682a09c50 138 magn_off_z = 0;
pommzorz 0:9a1682a09c50 139 magn_scale_x = 1;
pommzorz 0:9a1682a09c50 140 magn_scale_y = 1;
pommzorz 0:9a1682a09c50 141 magn_scale_z = 1;
pommzorz 0:9a1682a09c50 142 }
pommzorz 0:9a1682a09c50 143
svkatielee 3:450acaa4f52d 144 void IMU10DOF::getRawValues(int16_t * raw_values)
pommzorz 0:9a1682a09c50 145 {
pommzorz 0:9a1682a09c50 146 int16_t raw3[3];
pommzorz 0:9a1682a09c50 147 int raw[3];
pommzorz 0:9a1682a09c50 148
pommzorz 0:9a1682a09c50 149 pc.printf("GetRaw IN \n\r");
pommzorz 0:9a1682a09c50 150
pommzorz 0:9a1682a09c50 151 acc.getOutput(&raw_values[0], &raw_values[1], &raw_values[2]);
pommzorz 0:9a1682a09c50 152 pc.printf("GotACC\n\r");
pommzorz 0:9a1682a09c50 153
pommzorz 0:9a1682a09c50 154 gyro.read(raw);
pommzorz 0:9a1682a09c50 155
pommzorz 0:9a1682a09c50 156 raw_values[3]=(int16_t)raw[0];
pommzorz 0:9a1682a09c50 157 raw_values[4]=(int16_t)raw[1];
pommzorz 0:9a1682a09c50 158 raw_values[5]=(int16_t)raw[2];
pommzorz 0:9a1682a09c50 159 pc.printf("GotGYRO\n\r");
pommzorz 0:9a1682a09c50 160
pommzorz 0:9a1682a09c50 161
pommzorz 0:9a1682a09c50 162 magn.getXYZ(raw3);
pommzorz 0:9a1682a09c50 163 raw_values[6]=raw3[0];
pommzorz 0:9a1682a09c50 164 raw_values[7]=raw3[1];
pommzorz 0:9a1682a09c50 165 raw_values[8]=raw3[2];
pommzorz 0:9a1682a09c50 166 pc.printf("GotMAG\n\r");
pommzorz 0:9a1682a09c50 167
pommzorz 0:9a1682a09c50 168
pommzorz 0:9a1682a09c50 169
pommzorz 0:9a1682a09c50 170 baro.readSensor();
pommzorz 0:9a1682a09c50 171 raw_values[9]=(int16_t)baro.temp();
pommzorz 0:9a1682a09c50 172 raw_values[10]=(int16_t)baro.press();
pommzorz 0:9a1682a09c50 173 pc.printf("GotBARO\n\r");
pommzorz 0:9a1682a09c50 174
pommzorz 0:9a1682a09c50 175
pommzorz 0:9a1682a09c50 176 }
pommzorz 0:9a1682a09c50 177
pommzorz 0:9a1682a09c50 178
pommzorz 0:9a1682a09c50 179 /**
pommzorz 0:9a1682a09c50 180 * Populates values with calibrated readings from the sensors
pommzorz 0:9a1682a09c50 181 */
svkatielee 3:450acaa4f52d 182 void IMU10DOF::getValues(float * values)
pommzorz 0:9a1682a09c50 183 {
pommzorz 0:9a1682a09c50 184
pommzorz 0:9a1682a09c50 185 int16_t accval[3];
pommzorz 0:9a1682a09c50 186 float gyrval[3];
pommzorz 0:9a1682a09c50 187 acc.getOutput(accval);
pommzorz 0:9a1682a09c50 188 values[0] = (float) accval[0];
pommzorz 0:9a1682a09c50 189 values[1] = (float) accval[1];
pommzorz 0:9a1682a09c50 190 values[2] = (float) accval[2];
pommzorz 0:9a1682a09c50 191
pommzorz 0:9a1682a09c50 192 gyro.readFin(gyrval);
pommzorz 0:9a1682a09c50 193 values[3] = gyrval[0];
pommzorz 0:9a1682a09c50 194 values[4] = gyrval[1];
pommzorz 0:9a1682a09c50 195 values[5] = gyrval[2];
pommzorz 0:9a1682a09c50 196
pommzorz 0:9a1682a09c50 197 magn.getXYZ(accval);
svkatielee 3:450acaa4f52d 198 values[6]=(float)accval[0];
svkatielee 3:450acaa4f52d 199 values[7]=(float)accval[1];
svkatielee 3:450acaa4f52d 200 values[8]=(float)accval[2];
pommzorz 0:9a1682a09c50 201
pommzorz 0:9a1682a09c50 202
pommzorz 0:9a1682a09c50 203 #warning Accelerometer calibration active: have you calibrated your device?
pommzorz 0:9a1682a09c50 204 // remove offsets and scale accelerometer (calibration)
pommzorz 0:9a1682a09c50 205 values[0] = (values[0] - acc_off_x) / acc_scale_x;
pommzorz 0:9a1682a09c50 206 values[1] = (values[1] - acc_off_y) / acc_scale_y;
pommzorz 0:9a1682a09c50 207 values[2] = (values[2] - acc_off_z) / acc_scale_z;
pommzorz 0:9a1682a09c50 208
pommzorz 0:9a1682a09c50 209 // calibration
pommzorz 0:9a1682a09c50 210 #warning Magnetometer calibration active: have you calibrated your device?
pommzorz 0:9a1682a09c50 211 values[6] = (values[6] - magn_off_x) / magn_scale_x;
pommzorz 0:9a1682a09c50 212 values[7] = (values[7] - magn_off_y) / magn_scale_y;
pommzorz 0:9a1682a09c50 213 values[8] = (values[8] - magn_off_z) / magn_scale_z;
pommzorz 0:9a1682a09c50 214
pommzorz 0:9a1682a09c50 215
pommzorz 0:9a1682a09c50 216
pommzorz 0:9a1682a09c50 217 }
pommzorz 0:9a1682a09c50 218
pommzorz 0:9a1682a09c50 219
pommzorz 0:9a1682a09c50 220 /**
pommzorz 0:9a1682a09c50 221 * Computes gyro offsets
pommzorz 0:9a1682a09c50 222 */
svkatielee 3:450acaa4f52d 223 void IMU10DOF::zeroGyro()
pommzorz 0:9a1682a09c50 224 {
pommzorz 0:9a1682a09c50 225 const int totSamples = 3;
pommzorz 0:9a1682a09c50 226 int16_t raw[11];
pommzorz 0:9a1682a09c50 227 float tmpOffsets[] = {0,0,0};
pommzorz 0:9a1682a09c50 228
pommzorz 0:9a1682a09c50 229 for (int i = 0; i < totSamples; i++) {
pommzorz 0:9a1682a09c50 230 getRawValues(raw);
pommzorz 0:9a1682a09c50 231 tmpOffsets[0] += raw[3];
pommzorz 0:9a1682a09c50 232 tmpOffsets[1] += raw[4];
pommzorz 0:9a1682a09c50 233 tmpOffsets[2] += raw[5];
pommzorz 0:9a1682a09c50 234 }
pommzorz 0:9a1682a09c50 235
pommzorz 0:9a1682a09c50 236 gyro_off_x = tmpOffsets[0] / totSamples;
pommzorz 0:9a1682a09c50 237 gyro_off_y = tmpOffsets[1] / totSamples;
pommzorz 0:9a1682a09c50 238 gyro_off_z = tmpOffsets[2] / totSamples;
pommzorz 0:9a1682a09c50 239 }
pommzorz 0:9a1682a09c50 240
pommzorz 0:9a1682a09c50 241
pommzorz 0:9a1682a09c50 242 /**
pommzorz 0:9a1682a09c50 243 * Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion
pommzorz 0:9a1682a09c50 244 * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
pommzorz 0:9a1682a09c50 245 * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
pommzorz 0:9a1682a09c50 246 * axis only.
pommzorz 0:9a1682a09c50 247 *
pommzorz 0:9a1682a09c50 248 * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
pommzorz 0:9a1682a09c50 249 */
pommzorz 0:9a1682a09c50 250 //#if IS_9DOM()
svkatielee 3:450acaa4f52d 251 void IMU10DOF::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
pommzorz 0:9a1682a09c50 252 {
pommzorz 0:9a1682a09c50 253
pommzorz 0:9a1682a09c50 254 float recipNorm;
pommzorz 0:9a1682a09c50 255 float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
pommzorz 0:9a1682a09c50 256 float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
pommzorz 0:9a1682a09c50 257 float qa, qb, qc;
pommzorz 0:9a1682a09c50 258
pommzorz 0:9a1682a09c50 259 // Auxiliary variables to avoid repeated arithmetic
pommzorz 0:9a1682a09c50 260 q0q0 = q0 * q0;
pommzorz 0:9a1682a09c50 261 q0q1 = q0 * q1;
pommzorz 0:9a1682a09c50 262 q0q2 = q0 * q2;
pommzorz 0:9a1682a09c50 263 q0q3 = q0 * q3;
pommzorz 0:9a1682a09c50 264 q1q1 = q1 * q1;
pommzorz 0:9a1682a09c50 265 q1q2 = q1 * q2;
pommzorz 0:9a1682a09c50 266 q1q3 = q1 * q3;
pommzorz 0:9a1682a09c50 267 q2q2 = q2 * q2;
pommzorz 0:9a1682a09c50 268 q2q3 = q2 * q3;
pommzorz 0:9a1682a09c50 269 q3q3 = q3 * q3;
pommzorz 0:9a1682a09c50 270
pommzorz 0:9a1682a09c50 271 // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
pommzorz 0:9a1682a09c50 272 if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
pommzorz 0:9a1682a09c50 273 float hx, hy, bx, bz;
pommzorz 0:9a1682a09c50 274 float halfwx, halfwy, halfwz;
pommzorz 0:9a1682a09c50 275
pommzorz 0:9a1682a09c50 276 // Normalise magnetometer measurement
pommzorz 0:9a1682a09c50 277 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
pommzorz 0:9a1682a09c50 278 mx *= recipNorm;
pommzorz 0:9a1682a09c50 279 my *= recipNorm;
pommzorz 0:9a1682a09c50 280 mz *= recipNorm;
pommzorz 0:9a1682a09c50 281
pommzorz 0:9a1682a09c50 282 // Reference direction of Earth's magnetic field
pommzorz 0:9a1682a09c50 283 hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
pommzorz 0:9a1682a09c50 284 hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
pommzorz 0:9a1682a09c50 285 bx = sqrt(hx * hx + hy * hy);
pommzorz 0:9a1682a09c50 286 bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
pommzorz 0:9a1682a09c50 287
pommzorz 0:9a1682a09c50 288 // Estimated direction of magnetic field
pommzorz 0:9a1682a09c50 289 halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
pommzorz 0:9a1682a09c50 290 halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
pommzorz 0:9a1682a09c50 291 halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
pommzorz 0:9a1682a09c50 292
pommzorz 0:9a1682a09c50 293 // Error is sum of cross product between estimated direction and measured direction of field vectors
pommzorz 0:9a1682a09c50 294 halfex = (my * halfwz - mz * halfwy);
pommzorz 0:9a1682a09c50 295 halfey = (mz * halfwx - mx * halfwz);
pommzorz 0:9a1682a09c50 296 halfez = (mx * halfwy - my * halfwx);
pommzorz 0:9a1682a09c50 297 }
pommzorz 0:9a1682a09c50 298
pommzorz 0:9a1682a09c50 299
pommzorz 0:9a1682a09c50 300 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
pommzorz 0:9a1682a09c50 301 if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
pommzorz 0:9a1682a09c50 302 float halfvx, halfvy, halfvz;
pommzorz 0:9a1682a09c50 303
pommzorz 0:9a1682a09c50 304 // Normalise accelerometer measurement
pommzorz 0:9a1682a09c50 305 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
pommzorz 0:9a1682a09c50 306 ax *= recipNorm;
pommzorz 0:9a1682a09c50 307 ay *= recipNorm;
pommzorz 0:9a1682a09c50 308 az *= recipNorm;
pommzorz 0:9a1682a09c50 309
pommzorz 0:9a1682a09c50 310 // Estimated direction of gravity
pommzorz 0:9a1682a09c50 311 halfvx = q1q3 - q0q2;
pommzorz 0:9a1682a09c50 312 halfvy = q0q1 + q2q3;
pommzorz 0:9a1682a09c50 313 halfvz = q0q0 - 0.5f + q3q3;
pommzorz 0:9a1682a09c50 314
pommzorz 0:9a1682a09c50 315 // Error is sum of cross product between estimated direction and measured direction of field vectors
pommzorz 0:9a1682a09c50 316 halfex += (ay * halfvz - az * halfvy);
pommzorz 0:9a1682a09c50 317 halfey += (az * halfvx - ax * halfvz);
pommzorz 0:9a1682a09c50 318 halfez += (ax * halfvy - ay * halfvx);
pommzorz 0:9a1682a09c50 319 }
pommzorz 0:9a1682a09c50 320
pommzorz 0:9a1682a09c50 321 // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
pommzorz 0:9a1682a09c50 322 if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
pommzorz 0:9a1682a09c50 323 // Compute and apply integral feedback if enabled
pommzorz 0:9a1682a09c50 324 if(twoKi > 0.0f) {
pommzorz 0:9a1682a09c50 325 integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
pommzorz 0:9a1682a09c50 326 integralFBy += twoKi * halfey * (1.0f / sampleFreq);
pommzorz 0:9a1682a09c50 327 integralFBz += twoKi * halfez * (1.0f / sampleFreq);
pommzorz 0:9a1682a09c50 328 gx += integralFBx; // apply integral feedback
pommzorz 0:9a1682a09c50 329 gy += integralFBy;
pommzorz 0:9a1682a09c50 330 gz += integralFBz;
pommzorz 0:9a1682a09c50 331 } else {
pommzorz 0:9a1682a09c50 332 integralFBx = 0.0f; // prevent integral windup
pommzorz 0:9a1682a09c50 333 integralFBy = 0.0f;
pommzorz 0:9a1682a09c50 334 integralFBz = 0.0f;
pommzorz 0:9a1682a09c50 335 }
pommzorz 0:9a1682a09c50 336
pommzorz 0:9a1682a09c50 337 // Apply proportional feedback
pommzorz 0:9a1682a09c50 338 gx += twoKp * halfex;
pommzorz 0:9a1682a09c50 339 gy += twoKp * halfey;
pommzorz 0:9a1682a09c50 340 gz += twoKp * halfez;
pommzorz 0:9a1682a09c50 341 }
pommzorz 0:9a1682a09c50 342
pommzorz 0:9a1682a09c50 343 // Integrate rate of change of quaternion
pommzorz 0:9a1682a09c50 344 gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
pommzorz 0:9a1682a09c50 345 gy *= (0.5f * (1.0f / sampleFreq));
pommzorz 0:9a1682a09c50 346 gz *= (0.5f * (1.0f / sampleFreq));
pommzorz 0:9a1682a09c50 347 qa = q0;
pommzorz 0:9a1682a09c50 348 qb = q1;
pommzorz 0:9a1682a09c50 349 qc = q2;
pommzorz 0:9a1682a09c50 350 q0 += (-qb * gx - qc * gy - q3 * gz);
pommzorz 0:9a1682a09c50 351 q1 += (qa * gx + qc * gz - q3 * gy);
pommzorz 0:9a1682a09c50 352 q2 += (qa * gy - qb * gz + q3 * gx);
pommzorz 0:9a1682a09c50 353 q3 += (qa * gz + qb * gy - qc * gx);
pommzorz 0:9a1682a09c50 354
pommzorz 0:9a1682a09c50 355 // Normalise quaternion
pommzorz 0:9a1682a09c50 356 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
pommzorz 0:9a1682a09c50 357 q0 *= recipNorm;
pommzorz 0:9a1682a09c50 358 q1 *= recipNorm;
pommzorz 0:9a1682a09c50 359 q2 *= recipNorm;
pommzorz 0:9a1682a09c50 360 q3 *= recipNorm;
pommzorz 0:9a1682a09c50 361 }
pommzorz 0:9a1682a09c50 362
pommzorz 0:9a1682a09c50 363
pommzorz 0:9a1682a09c50 364 /**
pommzorz 0:9a1682a09c50 365 * Populates array q with a quaternion representing the IMU orientation with respect to the Earth
pommzorz 0:9a1682a09c50 366 *
pommzorz 0:9a1682a09c50 367 * @param q the quaternion to populate
pommzorz 0:9a1682a09c50 368 */
svkatielee 3:450acaa4f52d 369 void IMU10DOF::getQ(float * q)
pommzorz 0:9a1682a09c50 370 {
pommzorz 0:9a1682a09c50 371 float val[9];
pommzorz 0:9a1682a09c50 372 getValues(val);
pommzorz 0:9a1682a09c50 373 /*
pommzorz 0:9a1682a09c50 374 DEBUG_PRINT(val[3] * M_PI/180);
pommzorz 0:9a1682a09c50 375 DEBUG_PRINT(val[4] * M_PI/180);
pommzorz 0:9a1682a09c50 376 DEBUG_PRINT(val[5] * M_PI/180);
pommzorz 0:9a1682a09c50 377 DEBUG_PRINT(val[0]);
pommzorz 0:9a1682a09c50 378 DEBUG_PRINT(val[1]);
pommzorz 0:9a1682a09c50 379 DEBUG_PRINT(val[2]);
pommzorz 0:9a1682a09c50 380 DEBUG_PRINT(val[6]);
pommzorz 0:9a1682a09c50 381 DEBUG_PRINT(val[7]);
pommzorz 0:9a1682a09c50 382 DEBUG_PRINT(val[8]);
pommzorz 0:9a1682a09c50 383 */
pommzorz 0:9a1682a09c50 384
pommzorz 0:9a1682a09c50 385 dt_us=update.read_us();
pommzorz 0:9a1682a09c50 386 sampleFreq = 1.0 / ((dt_us) / 1000000.0);
pommzorz 0:9a1682a09c50 387 update.reset();
pommzorz 0:9a1682a09c50 388
pommzorz 0:9a1682a09c50 389
pommzorz 0:9a1682a09c50 390
pommzorz 0:9a1682a09c50 391 // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
pommzorz 0:9a1682a09c50 392 AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], val[6], val[7], val[8]);
pommzorz 0:9a1682a09c50 393
pommzorz 0:9a1682a09c50 394
pommzorz 0:9a1682a09c50 395
pommzorz 0:9a1682a09c50 396 q[0] = q0;
pommzorz 0:9a1682a09c50 397 q[1] = q1;
pommzorz 0:9a1682a09c50 398 q[2] = q2;
pommzorz 0:9a1682a09c50 399 q[3] = q3;
pommzorz 0:9a1682a09c50 400 }
pommzorz 0:9a1682a09c50 401
pommzorz 0:9a1682a09c50 402
pommzorz 0:9a1682a09c50 403
pommzorz 0:9a1682a09c50 404 const float def_sea_press = 1013.25;
pommzorz 0:9a1682a09c50 405
pommzorz 0:9a1682a09c50 406 /**
pommzorz 0:9a1682a09c50 407 * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure
pommzorz 0:9a1682a09c50 408 */
svkatielee 3:450acaa4f52d 409 float IMU10DOF::getBaroAlt(float sea_press)
pommzorz 0:9a1682a09c50 410 {
pommzorz 0:9a1682a09c50 411 //float temp,press,res;
pommzorz 0:9a1682a09c50 412 baro.readSensor();
pommzorz 0:9a1682a09c50 413 //temp=(float)baro.temp();
pommzorz 0:9a1682a09c50 414 //press=(float)baro.press();
pommzorz 0:9a1682a09c50 415
pommzorz 0:9a1682a09c50 416
pommzorz 0:9a1682a09c50 417
pommzorz 0:9a1682a09c50 418 return baro.altitud();
pommzorz 0:9a1682a09c50 419 //return(temp,press);
pommzorz 0:9a1682a09c50 420 }
pommzorz 0:9a1682a09c50 421
pommzorz 0:9a1682a09c50 422 /**
pommzorz 0:9a1682a09c50 423 * Returns an altitude estimate from baromether readings only using a default sea level pressure
pommzorz 0:9a1682a09c50 424 */
svkatielee 3:450acaa4f52d 425 float IMU10DOF::getBaroAlt()
pommzorz 0:9a1682a09c50 426 {
pommzorz 0:9a1682a09c50 427 return getBaroAlt(def_sea_press);
pommzorz 0:9a1682a09c50 428 }
pommzorz 0:9a1682a09c50 429
pommzorz 0:9a1682a09c50 430
pommzorz 0:9a1682a09c50 431 /**
pommzorz 0:9a1682a09c50 432 * Compensates the accelerometer readings in the 3D vector acc expressed in the sensor frame for gravity
pommzorz 0:9a1682a09c50 433 * @param acc the accelerometer readings to compensate for gravity
pommzorz 0:9a1682a09c50 434 * @param q the quaternion orientation of the sensor board with respect to the world
pommzorz 0:9a1682a09c50 435 */
svkatielee 3:450acaa4f52d 436 void IMU10DOF::gravityCompensateAcc(float * acc, float * q)
pommzorz 0:9a1682a09c50 437 {
pommzorz 0:9a1682a09c50 438 float g[3];
pommzorz 0:9a1682a09c50 439
pommzorz 0:9a1682a09c50 440 // get expected direction of gravity in the sensor frame
pommzorz 0:9a1682a09c50 441 g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
pommzorz 0:9a1682a09c50 442 g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
pommzorz 0:9a1682a09c50 443 g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
pommzorz 0:9a1682a09c50 444
pommzorz 0:9a1682a09c50 445 // compensate accelerometer readings with the expected direction of gravity
pommzorz 0:9a1682a09c50 446 acc[0] = acc[0] - g[0];
pommzorz 0:9a1682a09c50 447 acc[1] = acc[1] - g[1];
pommzorz 0:9a1682a09c50 448 acc[2] = acc[2] - g[2];
pommzorz 0:9a1682a09c50 449 }
pommzorz 0:9a1682a09c50 450
pommzorz 0:9a1682a09c50 451
pommzorz 0:9a1682a09c50 452
pommzorz 0:9a1682a09c50 453 /**
pommzorz 0:9a1682a09c50 454 * Returns the Euler angles in radians defined in the Aerospace sequence.
pommzorz 0:9a1682a09c50 455 * See Sebastian O.H. Madwick report "An efficient orientation filter for
pommzorz 0:9a1682a09c50 456 * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
pommzorz 0:9a1682a09c50 457 *
pommzorz 0:9a1682a09c50 458 * @param angles three floats array which will be populated by the Euler angles in radians
pommzorz 0:9a1682a09c50 459 */
svkatielee 3:450acaa4f52d 460 void IMU10DOF::getEulerRad(float * angles)
pommzorz 0:9a1682a09c50 461 {
pommzorz 0:9a1682a09c50 462 float q[4]; // quaternion
pommzorz 0:9a1682a09c50 463 getQ(q);
pommzorz 0:9a1682a09c50 464 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:9a1682a09c50 465 angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
pommzorz 0:9a1682a09c50 466 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:9a1682a09c50 467 }
pommzorz 0:9a1682a09c50 468
pommzorz 0:9a1682a09c50 469
pommzorz 0:9a1682a09c50 470 /**
pommzorz 0:9a1682a09c50 471 * Returns the Euler angles in degrees defined with the Aerospace sequence.
pommzorz 0:9a1682a09c50 472 * See Sebastian O.H. Madwick report "An efficient orientation filter for
pommzorz 0:9a1682a09c50 473 * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
pommzorz 0:9a1682a09c50 474 *
pommzorz 0:9a1682a09c50 475 * @param angles three floats array which will be populated by the Euler angles in degrees
pommzorz 0:9a1682a09c50 476 */
svkatielee 3:450acaa4f52d 477 void IMU10DOF::getEuler(float * angles)
pommzorz 0:9a1682a09c50 478 {
pommzorz 0:9a1682a09c50 479 getEulerRad(angles);
pommzorz 0:9a1682a09c50 480 arr3_rad_to_deg(angles);
pommzorz 0:9a1682a09c50 481 }
pommzorz 0:9a1682a09c50 482
pommzorz 0:9a1682a09c50 483
pommzorz 0:9a1682a09c50 484 /**
pommzorz 0:9a1682a09c50 485 * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
pommzorz 0:9a1682a09c50 486 * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
pommzorz 0:9a1682a09c50 487 * and the Earth ground plane and the IMU Y axis.
pommzorz 0:9a1682a09c50 488 *
pommzorz 0:9a1682a09c50 489 * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
svkatielee 3:450acaa4f52d 490 * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see IMU10DOF::getEuler
pommzorz 0:9a1682a09c50 491 *
pommzorz 0:9a1682a09c50 492 * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians
pommzorz 0:9a1682a09c50 493 */
svkatielee 3:450acaa4f52d 494 void IMU10DOF::getYawPitchRollRad(float * ypr)
pommzorz 0:9a1682a09c50 495 {
pommzorz 0:9a1682a09c50 496 float q[4]; // quaternion
pommzorz 0:9a1682a09c50 497 float gx, gy, gz; // estimated gravity direction
pommzorz 0:9a1682a09c50 498 getQ(q);
pommzorz 0:9a1682a09c50 499
pommzorz 0:9a1682a09c50 500 gx = 2 * (q[1]*q[3] - q[0]*q[2]);
pommzorz 0:9a1682a09c50 501 gy = 2 * (q[0]*q[1] + q[2]*q[3]);
pommzorz 0:9a1682a09c50 502 gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
pommzorz 0:9a1682a09c50 503
pommzorz 0:9a1682a09c50 504 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:9a1682a09c50 505 ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
pommzorz 0:9a1682a09c50 506 ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
pommzorz 0:9a1682a09c50 507 }
pommzorz 0:9a1682a09c50 508
pommzorz 0:9a1682a09c50 509
pommzorz 0:9a1682a09c50 510 /**
pommzorz 0:9a1682a09c50 511 * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between
pommzorz 0:9a1682a09c50 512 * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
pommzorz 0:9a1682a09c50 513 * and the Earth ground plane and the IMU Y axis.
pommzorz 0:9a1682a09c50 514 *
pommzorz 0:9a1682a09c50 515 * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
svkatielee 3:450acaa4f52d 516 * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see IMU10DOF::getEuler
pommzorz 0:9a1682a09c50 517 *
pommzorz 0:9a1682a09c50 518 * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees
pommzorz 0:9a1682a09c50 519 */
svkatielee 3:450acaa4f52d 520 void IMU10DOF::getYawPitchRoll(float * ypr)
pommzorz 0:9a1682a09c50 521 {
pommzorz 0:9a1682a09c50 522 getYawPitchRollRad(ypr);
pommzorz 0:9a1682a09c50 523 arr3_rad_to_deg(ypr);
pommzorz 0:9a1682a09c50 524 }
pommzorz 0:9a1682a09c50 525
pommzorz 0:9a1682a09c50 526
pommzorz 0:9a1682a09c50 527 /**
pommzorz 0:9a1682a09c50 528 * Converts a 3 elements array arr of angles expressed in radians into degrees
pommzorz 0:9a1682a09c50 529 */
pommzorz 0:9a1682a09c50 530 void arr3_rad_to_deg(float * arr)
pommzorz 0:9a1682a09c50 531 {
pommzorz 0:9a1682a09c50 532 arr[0] *= 180/M_PI;
pommzorz 0:9a1682a09c50 533 arr[1] *= 180/M_PI;
pommzorz 0:9a1682a09c50 534 arr[2] *= 180/M_PI;
pommzorz 0:9a1682a09c50 535 }
pommzorz 0:9a1682a09c50 536
pommzorz 0:9a1682a09c50 537
pommzorz 0:9a1682a09c50 538 /**
pommzorz 0:9a1682a09c50 539 * Fast inverse square root implementation
pommzorz 0:9a1682a09c50 540 * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root
pommzorz 0:9a1682a09c50 541 */
pommzorz 0:9a1682a09c50 542 float invSqrt(float number)
pommzorz 0:9a1682a09c50 543 {
pommzorz 0:9a1682a09c50 544 volatile long i;
pommzorz 0:9a1682a09c50 545 volatile float x, y;
pommzorz 0:9a1682a09c50 546 volatile const float f = 1.5F;
pommzorz 0:9a1682a09c50 547
pommzorz 0:9a1682a09c50 548 x = number * 0.5F;
pommzorz 0:9a1682a09c50 549 y = number;
pommzorz 0:9a1682a09c50 550 i = * ( long * ) &y;
pommzorz 0:9a1682a09c50 551 i = 0x5f375a86 - ( i >> 1 );
pommzorz 0:9a1682a09c50 552 y = * ( float * ) &i;
pommzorz 0:9a1682a09c50 553 y = y * ( f - ( x * y * y ) );
pommzorz 0:9a1682a09c50 554 return y;
pommzorz 0:9a1682a09c50 555 }
pommzorz 0:9a1682a09c50 556
pommzorz 0:9a1682a09c50 557
pommzorz 0:9a1682a09c50 558