Added external magnetometer functionality
Dependencies: HMC58X31 MODI2C MPU6050 MS561101BA
Fork of FreeIMU by
FreeIMU.cpp
- Committer:
- joe4465
- Date:
- 2014-09-18
- Revision:
- 15:ea86489d606b
- Parent:
- 13:21b275eeeda2
- Child:
- 17:48a0eae27bf1
File content as of revision 15:ea86489d606b:
/* FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net> Development of this code has been supported by the Department of Computer Science, Universita' degli Studi di Torino, Italy within the Piemonte Project http://www.piemonte.di.unito.it/ This program is free software: you can redistribute it and/or modify it under the terms of the version 3 GNU General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. 02/20/2013 - Modified by Aloïs Wolff for MBED with MPU6050 only (wolffalois@gmail.com) */ //#include <inttypes.h> //#include <stdint.h> //#define DEBUG #include "MODI2C.h" #include "FreeIMU.h" #include "rtos.h" #define M_PI 3.1415926535897932384626433832795 FreeIMU::FreeIMU() { // initialize quaternion q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; exInt = 0.0; eyInt = 0.0; ezInt = 0.0; twoKp = twoKpDef; twoKi = twoKiDef; twoKiz = twoKiDef / 4.0; twoKpz = twoKpDef * 6.0; integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; update.start(); dt_us=0; /* lastUpdate = 0; now = 0; */ #ifndef CALIBRATION_H // initialize scale factors to neutral values acc_scale_x = 1; acc_scale_y = 1; acc_scale_z = 1; magn_scale_x = 1; magn_scale_y = 1; magn_scale_z = 1; #else // get values from global variables of same name defined in calibration.h acc_off_x = ::acc_off_x; acc_off_y = ::acc_off_y; acc_off_z = ::acc_off_z; acc_scale_x = ::acc_scale_x; acc_scale_y = ::acc_scale_y; acc_scale_z = ::acc_scale_z; magn_off_x = ::magn_off_x; magn_off_y = ::magn_off_y; magn_off_z = ::magn_off_z; magn_scale_x = ::magn_scale_x; magn_scale_y = ::magn_scale_y; magn_scale_z = ::magn_scale_z; #endif } void FreeIMU::init() { init(FIMU_ACCGYRO_ADDR, false); } void FreeIMU::init(bool fastmode) { init(FIMU_ACCGYRO_ADDR, fastmode); } void FreeIMU::sample(bool sampling) { accgyro->sample(sampling); } /** * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration */ void FreeIMU::init(int accgyro_addr, bool fastmode) { accgyro = new MPU6050(); Thread::wait(10); baro = new MS561101BA(); magn = new HMC58X3(); Thread::wait(10); accgyro->initialize(); accgyro->setI2CMasterModeEnabled(0); accgyro->setI2CBypassEnabled(1); accgyro->setFullScaleGyroRange(MPU6050_GYRO_FS_1000); accgyro->setDLPFMode(0); accgyro->setRate(0); Thread::wait(20); accgyro->start_sampling(); Thread::wait(10); // init HMC5843 magn->init(false); // Don't set mode yet, we'll do that later on. magn->setGain(0); // Calibrate HMC using self test, not recommended to change the gain after calibration. magn->calibrate(0, 8); // Use gain 1=default, valid 0-7, 7 not recommended. Thread::wait(30); magn->setDOR(6); Thread::wait(30); magn->start_sampling(); Thread::wait(30); baro->init(FIMU_BARO_ADDR); // zero gyro zeroGyro(); #ifndef CALIBRATION_H // load calibration from eeprom calLoad(); #endif Thread::wait(30); getQ_simple(NULL); baro->start_sampling(MS561101BA_OSR_4096); } void FreeIMU::getQ_simple(float* q) { float values[9]; getValues(values); float pitch = atan2(values[0], sqrt(values[1]*values[1]+values[2]*values[2])); float roll = -atan2(values[1], sqrt(values[0]*values[0]+values[2]*values[2])); float xh = values[6]*cos(pitch)+values[7]*sin(roll)*sin(pitch)-values[8]*cos(roll)*sin(pitch); float yh = values[7]*cos(roll)+values[8]*sin(roll); float yaw = -atan2(yh, xh); float rollOver2 = (roll + M_PI) * 0.5f; float sinRollOver2 = (float)sin(rollOver2); float cosRollOver2 = (float)cos(rollOver2); float pitchOver2 = pitch * 0.5f; float sinPitchOver2 = (float)sin(pitchOver2); float cosPitchOver2 = (float)cos(pitchOver2); float yawOver2 = yaw * 0.5f; float sinYawOver2 = (float)sin(yawOver2); float cosYawOver2 = (float)cos(yawOver2); q0 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; q1 = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2; q2 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; q3 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; if (q!=NULL) { q[0] = q0; q[1] = q1; q[2] = q2; q[3] = q3; } } /** * Populates raw_values with the raw_values from the sensors */ void FreeIMU::getRawValues(int16_t * raw_values) { accgyro->getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]); magn->getValues(&raw_values[6], &raw_values[7], &raw_values[8]); int temp, press; //TODO: possible loss of precision temp = baro->rawTemperature(); raw_values[9] = temp; press = baro->rawPressure(); raw_values[10] = press; } /** * Populates values with calibrated readings from the sensors */ void FreeIMU::getValues(float * values) { // MPU6050 int16_t accgyroval[6]; accgyro->getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]); // remove offsets from the gyroscope accgyroval[3] = accgyroval[3] - gyro_off_x; accgyroval[4] = accgyroval[4] - gyro_off_y; accgyroval[5] = accgyroval[5] - gyro_off_z; for(int i = 0; i<6; i++) { if(i < 3) { values[i] = (float) accgyroval[i]; } else { values[i] = ((float) accgyroval[i]) / 32.8f; // NOTE: this depends on the sensitivity chosen } } #warning Accelerometer calibration active: have you calibrated your device? // remove offsets and scale accelerometer (calibration) values[0] = (values[0] - acc_off_x) / acc_scale_x; values[1] = (values[1] - acc_off_y) / acc_scale_y; values[2] = (values[2] - acc_off_z) / acc_scale_z; magn->getValues(&values[6]); // calibration #warning Magnetometer calibration active: have you calibrated your device? values[6] = (values[6] - magn_off_x) / magn_scale_x; values[7] = (values[7] - magn_off_y) / magn_scale_y; values[8] = (values[8] - magn_off_z) / magn_scale_z; } /** * Computes gyro offsets */ void FreeIMU::zeroGyro() { const int totSamples = 64; int16_t raw[11]; float tmpOffsets[] = {0,0,0}; for (int i = 0; i < totSamples; i++) { getRawValues(raw); tmpOffsets[0] += raw[3]; tmpOffsets[1] += raw[4]; tmpOffsets[2] += raw[5]; Thread::wait(3); } gyro_off_x = tmpOffsets[0] / totSamples; gyro_off_y = tmpOffsets[1] / totSamples; gyro_off_z = tmpOffsets[2] / totSamples; } extern volatile bool magn_valid; /** * Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw * axis only. * * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms */ void FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, bool _magn_valid) { float recipNorm; float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; float qa, qb, qc; // Auxiliary variables to avoid repeated arithmetic q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3; // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation) if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f) && _magn_valid) { float hx, hy, bx, bz; float halfwx, halfwy, halfwz; // Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // Reference direction of Earth's magnetic field hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); bx = sqrt(hx * hx + hy * hy); bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); // Estimated direction of magnetic field halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); // Error is sum of cross product between estimated direction and measured direction of field vectors halfex = (my * halfwz - mz * halfwy); halfey = (mz * halfwx - mx * halfwz); halfez = (mx * halfwy - my * halfwx); magn_valid = false; } // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) { float halfvx, halfvy, halfvz; // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Estimated direction of gravity halfvx = q1q3 - q0q2; halfvy = q0q1 + q2q3; halfvz = q0q0 - 0.5f + q3q3; // Error is sum of cross product between estimated direction and measured direction of field vectors halfex += (ay * halfvz - az * halfvy); halfey += (az * halfvx - ax * halfvz); halfez += (ax * halfvy - ay * halfvx); } // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki integralFBy += twoKi * halfey * (1.0f / sampleFreq); integralFBz += twoKiz * halfez * (1.0f / sampleFreq); gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Apply proportional feedback gx += twoKp * halfex; gy += twoKp * halfey; gz += twoKpz * halfez; } // Integrate rate of change of quaternion gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors gy *= (0.5f * (1.0f / sampleFreq)); gz *= (0.5f * (1.0f / sampleFreq)); qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; } /** * Populates array q with a quaternion representing the IMU orientation with respect to the Earth * * @param q the quaternion to populate */ float val[9]; void FreeIMU::getQ(float * q) { getValues(val); dt_us=update.read_us(); sampleFreq = 1.0 / ((dt_us) / 1000000.0); update.reset(); // lastUpdate = now; // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec 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); if (q!=NULL) { q[0] = q0; q[1] = q1; q[2] = q2; q[3] = q3; } } void FreeIMU::getRate(float * r) { r[0] = val[3]; r[1] = val[4]; r[2] = val[5]; } const float def_sea_press = 1013.25; /** * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure */ float FreeIMU::getBaroAlt(float sea_press) { float temp = baro->getTemperature(); float press = baro->getPressure(); return ((pow((float)(sea_press / press), 1.0f/5.257f) - 1.0f) * (temp + 273.15f)) / 0.0065f; } /** * Returns an altitude estimate from baromether readings only using a default sea level pressure */ float FreeIMU::getBaroAlt() { return getBaroAlt(def_sea_press); } float FreeIMU::getRawPressure() { return baro->getPressure(); } /** * Compensates the accelerometer readings in the 3D vector acc expressed in the sensor frame for gravity * @param acc the accelerometer readings to compensate for gravity * @param q the quaternion orientation of the sensor board with respect to the world */ void FreeIMU::gravityCompensateAcc(float * acc, float * q) { float g[3]; // get expected direction of gravity in the sensor frame g[0] = 2 * (q[1] * q[3] - q[0] * q[2]); g[1] = 2 * (q[0] * q[1] + q[2] * q[3]); g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]; // compensate accelerometer readings with the expected direction of gravity acc[0] = acc[0] - g[0]; acc[1] = acc[1] - g[1]; acc[2] = acc[2] - g[2]; } /** * Returns the Euler angles in radians defined in the Aerospace sequence. * See Sebastian O.H. Madwick report "An efficient orientation filter for * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation * * @param angles three floats array which will be populated by the Euler angles in radians */ void FreeIMU::getEulerRad(float * angles) { float q[4]; // quaternion getQ(q); 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 angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta 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 } /** * Returns the Euler angles in degrees defined with the Aerospace sequence. * See Sebastian O.H. Madwick report "An efficient orientation filter for * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation * * @param angles three floats array which will be populated by the Euler angles in degrees */ void FreeIMU::getEuler(float * angles) { getEulerRad(angles); arr3_rad_to_deg(angles); } /** * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch) * and the Earth ground plane and the IMU Y axis. * * @note This is not an Euler representation: the rotations aren't consecutive rotations but only * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler * * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians */ void FreeIMU::getYawPitchRollRad(float * ypr) { float q[4]; // quaternion float gx, gy, gz; // estimated gravity direction getQ(q); gx = 2 * (q[1]*q[3] - q[0]*q[2]); gy = 2 * (q[0]*q[1] + q[2]*q[3]); gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); ypr[1] = atan(gx / sqrt(gy*gy + gz*gz)); ypr[2] = atan(gy / sqrt(gx*gx + gz*gz)); } /** * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch) * and the Earth ground plane and the IMU Y axis. * * @note This is not an Euler representation: the rotations aren't consecutive rotations but only * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler * * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees */ void FreeIMU::getYawPitchRoll(float * ypr) { getYawPitchRollRad(ypr); arr3_rad_to_deg(ypr); } /** * Converts a 3 elements array arr of angles expressed in radians into degrees */ void arr3_rad_to_deg(float * arr) { arr[0] *= 180/M_PI; arr[1] *= 180/M_PI; arr[2] *= 180/M_PI; } /** * Fast inverse square root implementation * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root */ float invSqrt(float number) { volatile long i; volatile float x, y; volatile const float f = 1.5F; x = number * 0.5F; y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); y = * ( float * ) &i; y = y * ( f - ( x * y * y ) ); return y; }