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