IMU 10dof MEMS from DR Robot adapted from HK10DOF Changed gyro to ITG3200
Fork of HK10DOF by
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.
Diff: IMU10DOF.cpp
- Revision:
- 3:450acaa4f52d
- Parent:
- 0:9a1682a09c50
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU10DOF.cpp Tue Nov 18 06:25:57 2014 +0000 @@ -0,0 +1,558 @@ +/* +FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino +Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net> +ported for HobbyKing's 10DOF stick on a mbed platform by Aloïs Wolff(wolffalois@gmail.com) + +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/>. + + Now ported to support DFRobot.com 10DOF MEMS IMU + by Larry Littlefield svkatielee@yahoo.com + +*/ + + + +//#define DEBUG + + +#include "IMU10DOF.h" +#include <math.h> +#include "helper_3dmath.h" + + +// #include "WireUtils.h" +//#include "DebugUtils.h" +//#include "vector_math.h" + +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif + +IMU10DOF::IMU10DOF(PinName sda, PinName scl) : acc(sda,scl), magn(sda,scl),gyro(sda,scl), baro(sda,scl),pc(USBTX,USBRX) +{ + + pc.baud(115200); + + // 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; + integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; + + +#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 +} + + + + +/** + * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration +*/ + +void IMU10DOF::init(bool fastmode) +{ + + + //Sensors Initialization + pc.printf("Initializing sensors...\n\r"); + magn.init(); + baro.getCalibrationData(); + acc.setPowerControl(0x00); + wait_ms(10); + acc.setDataFormatControl(0x0B); + wait_ms(10); + acc.setPowerControl(MeasurementMode); + wait_ms(10); + gyro.init(); + pc.printf("Sensors OK!\n\r"); + + update.start(); + int dt_us=0; + pc.printf("ZeroGyro\n\r"); + // zero gyro + //zeroGyro(); + gyro.zeroCalibrate(128,5); + pc.printf("ZeroGyro OK, CalLoad...\n\r"); + // load calibration from eeprom + calLoad(); + +} + +void IMU10DOF::calLoad() +{ + + + acc_off_x = 0; + acc_off_y = 0; + acc_off_z = 0; + acc_scale_x = 1; + acc_scale_y = 1; + acc_scale_z = 1; + + magn_off_x = 0.21; + magn_off_y = -0.175; + magn_off_z = 0; + magn_scale_x = 1; + magn_scale_y = 1; + magn_scale_z = 1; +} + +void IMU10DOF::getRawValues(int16_t * raw_values) +{ + int16_t raw3[3]; + int raw[3]; + + pc.printf("GetRaw IN \n\r"); + + acc.getOutput(&raw_values[0], &raw_values[1], &raw_values[2]); + pc.printf("GotACC\n\r"); + + gyro.read(raw); + + raw_values[3]=(int16_t)raw[0]; + raw_values[4]=(int16_t)raw[1]; + raw_values[5]=(int16_t)raw[2]; + pc.printf("GotGYRO\n\r"); + + + magn.getXYZ(raw3); + raw_values[6]=raw3[0]; + raw_values[7]=raw3[1]; + raw_values[8]=raw3[2]; + pc.printf("GotMAG\n\r"); + + + + baro.readSensor(); + raw_values[9]=(int16_t)baro.temp(); + raw_values[10]=(int16_t)baro.press(); + pc.printf("GotBARO\n\r"); + + +} + + +/** + * Populates values with calibrated readings from the sensors +*/ +void IMU10DOF::getValues(float * values) +{ + + int16_t accval[3]; + float gyrval[3]; + acc.getOutput(accval); + values[0] = (float) accval[0]; + values[1] = (float) accval[1]; + values[2] = (float) accval[2]; + + gyro.readFin(gyrval); + values[3] = gyrval[0]; + values[4] = gyrval[1]; + values[5] = gyrval[2]; + + magn.getXYZ(accval); + values[6]=(float)accval[0]; + values[7]=(float)accval[1]; + values[8]=(float)accval[2]; + + +#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; + + // 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 IMU10DOF::zeroGyro() +{ + const int totSamples = 3; + 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]; + } + + gyro_off_x = tmpOffsets[0] / totSamples; + gyro_off_y = tmpOffsets[1] / totSamples; + gyro_off_z = tmpOffsets[2] / totSamples; +} + + +/** + * 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 +*/ +//#if IS_9DOM() +void IMU10DOF::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) +{ + + 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)) { + 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); + } + + + // 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 += twoKi * 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 += twoKp * 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 +*/ +void IMU10DOF::getQ(float * q) +{ + float val[9]; + getValues(val); + /* + DEBUG_PRINT(val[3] * M_PI/180); + DEBUG_PRINT(val[4] * M_PI/180); + DEBUG_PRINT(val[5] * M_PI/180); + DEBUG_PRINT(val[0]); + DEBUG_PRINT(val[1]); + DEBUG_PRINT(val[2]); + DEBUG_PRINT(val[6]); + DEBUG_PRINT(val[7]); + DEBUG_PRINT(val[8]); + */ + + dt_us=update.read_us(); + sampleFreq = 1.0 / ((dt_us) / 1000000.0); + update.reset(); + + + + // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec + 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]); + + + + q[0] = q0; + q[1] = q1; + q[2] = q2; + q[3] = q3; +} + + + +const float def_sea_press = 1013.25; + +/** + * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure +*/ +float IMU10DOF::getBaroAlt(float sea_press) +{ + //float temp,press,res; + baro.readSensor(); + //temp=(float)baro.temp(); + //press=(float)baro.press(); + + + + return baro.altitud(); + //return(temp,press); +} + +/** + * Returns an altitude estimate from baromether readings only using a default sea level pressure +*/ +float IMU10DOF::getBaroAlt() +{ + return getBaroAlt(def_sea_press); +} + + +/** + * 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 IMU10DOF::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 IMU10DOF::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 IMU10DOF::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 IMU10DOF::getEuler + * + * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians +*/ +void IMU10DOF::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 IMU10DOF::getEuler + * + * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees +*/ +void IMU10DOF::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; +} + + +