| 0,0 → 1,327 |
|---|
| /* |
| FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino |
| Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net> |
| |
| |
| 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/>. |
| |
| */ |
| |
| #include <inttypes.h> |
| //#define DEBUG |
| #include "WProgram.h" |
| #include "FreeIMU.h" |
| // #include "WireUtils.h" |
| #include "DebugUtils.h" |
| |
| //---------------------------------------------------------------------------------------------------- |
| // Definitions |
| |
| #define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer |
| #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases |
| //#define halfT 0.02f // half the sample period |
| |
| |
| FreeIMU::FreeIMU() { |
| #if FREEIMU_VER <= 3 |
| acc = ADXL345(); |
| #else |
| acc = BMA180(); |
| #endif |
| gyro = ITG3200(); |
| magn = HMC58X3(); |
| |
| // initialize quaternion |
| q0 = 1.0; |
| q1 = 0.0; |
| q2 = 0.0; |
| q3 = 0.0; |
| exInt = 0.0; |
| eyInt = 0.0; |
| ezInt = 0.0; |
| lastUpdate = 0; |
| now = 0; |
| } |
| |
| void FreeIMU::init() { |
| init(FIMU_ACC_ADDR, FIMU_ITG3200_DEF_ADDR, false); |
| } |
| |
| void FreeIMU::init(bool fastmode) { |
| init(FIMU_ACC_ADDR, FIMU_ITG3200_DEF_ADDR, fastmode); |
| } |
| |
| void FreeIMU::init(int acc_addr, int gyro_addr, bool fastmode) { |
| delay(5); |
| |
| // disable internal pullups of the ATMEGA which Wire enable by default |
| #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) |
| // deactivate internal pull-ups for twi |
| // as per note from atmega8 manual pg167 |
| cbi(PORTC, 4); |
| cbi(PORTC, 5); |
| #else |
| // deactivate internal pull-ups for twi |
| // as per note from atmega128 manual pg204 |
| cbi(PORTD, 0); |
| cbi(PORTD, 1); |
| #endif |
| |
| if(fastmode) { // switch to 400KHz I2C - eheheh |
| TWBR = ((16000000L / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c |
| // TODO: make the above usable also for 8MHz arduinos.. |
| } |
| |
| #if FREEIMU_VER <= 3 |
| // init ADXL345 |
| acc.init(acc_addr); |
| #else |
| // init BMA180 |
| acc.setAddress(acc_addr); |
| acc.SoftReset(); |
| acc.enableWrite(); |
| acc.SetFilter(acc.F10HZ); |
| acc.setGSensitivty(acc.G15); |
| acc.SetSMPSkip(); |
| acc.SetISRMode(); |
| acc.disableWrite(); |
| #endif |
| |
| // init ITG3200 |
| gyro.init(gyro_addr); |
| // calibrate the ITG3200 |
| gyro.zeroCalibrate(64,5); |
| |
| // init HMC5843 |
| magn.init(false); // Don't set mode yet, we'll do that later on. |
| // Calibrate HMC using self test, not recommended to change the gain after calibration. |
| magn.calibrate(1); // Use gain 1=default, valid 0-7, 7 not recommended. |
| // Single mode conversion was used in calibration, now set continuous mode |
| magn.setMode(0); |
| delay(10); |
| magn.setDOR(B110); |
| } |
| |
| |
| void FreeIMU::getRawValues(int * raw_values) { |
| acc.readAccel(&raw_values[0], &raw_values[1], &raw_values[2]); |
| gyro.readGyroRaw(&raw_values[3], &raw_values[4], &raw_values[5]); |
| magn.getValues(&raw_values[6], &raw_values[7], &raw_values[8]); |
| } |
| |
| |
| void FreeIMU::getValues(float * values) { |
| int accval[3]; |
| acc.readAccel(&accval[0], &accval[1], &accval[2]); |
| values[0] = ((float) accval[0]); |
| values[1] = ((float) accval[1]); |
| values[2] = ((float) accval[2]); |
| |
| gyro.readGyro(&values[3]); |
| |
| magn.getValues(&values[6]); |
| } |
| |
| |
| |
| //===================================================================================================== |
| // AHRS.c |
| // S.O.H. Madgwick |
| // 25th August 2010 |
| //===================================================================================================== |
| // Description: |
| // |
| // Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion |
| // compensation algorithms from my filter [Madgwick] 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. |
| // |
| // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'. |
| // |
| // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated |
| // orientation. See my report for an overview of the use of quaternions in this application. |
| // |
| // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'), |
| // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are |
| // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised. |
| // |
| //===================================================================================================== |
| void FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { |
| float norm; |
| float hx, hy, hz, bx, bz; |
| float vx, vy, vz, wx, wy, wz; |
| float ex, ey, ez; |
| |
| // auxiliary variables to reduce number of repeated operations |
| float q0q0 = q0*q0; |
| float q0q1 = q0*q1; |
| float q0q2 = q0*q2; |
| float q0q3 = q0*q3; |
| float q1q1 = q1*q1; |
| float q1q2 = q1*q2; |
| float q1q3 = q1*q3; |
| float q2q2 = q2*q2; |
| float q2q3 = q2*q3; |
| float q3q3 = q3*q3; |
| |
| // normalise the measurements |
| |
| now = millis(); |
| halfT = (now - lastUpdate) / 2000.0; |
| lastUpdate = now; |
| |
| norm = sqrt(ax*ax + ay*ay + az*az); |
| ax = ax / norm; |
| ay = ay / norm; |
| az = az / norm; |
| |
| /* |
| norm = invSqrt(ax*ax + ay*ay + az*az); |
| ax = ax * norm; |
| ay = ay * norm; |
| az = az * norm; |
| */ |
| |
| norm = sqrt(mx*mx + my*my + mz*mz); |
| mx = mx / norm; |
| my = my / norm; |
| mz = mz / norm; |
| |
| /* |
| norm = invSqrt(mx*mx + my*my + mz*mz); |
| mx = mx * norm; |
| my = my * norm; |
| mz = mz * norm; |
| */ |
| |
| // compute reference direction of flux |
| hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2); |
| hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1); |
| hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2); |
| bx = sqrt((hx*hx) + (hy*hy)); |
| bz = hz; |
| |
| // estimated direction of gravity and flux (v and w) |
| vx = 2*(q1q3 - q0q2); |
| vy = 2*(q0q1 + q2q3); |
| vz = q0q0 - q1q1 - q2q2 + q3q3; |
| wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2); |
| wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3); |
| wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); |
| |
| // error is sum of cross product between reference direction of fields and direction measured by sensors |
| ex = (ay*vz - az*vy) + (my*wz - mz*wy); |
| ey = (az*vx - ax*vz) + (mz*wx - mx*wz); |
| ez = (ax*vy - ay*vx) + (mx*wy - my*wx); |
| |
| // integral error scaled integral gain |
| exInt = exInt + ex*Ki; |
| eyInt = eyInt + ey*Ki; |
| ezInt = ezInt + ez*Ki; |
| |
| // adjusted gyroscope measurements |
| gx = gx + Kp*ex + exInt; |
| gy = gy + Kp*ey + eyInt; |
| gz = gz + Kp*ez + ezInt; |
| |
| // integrate quaternion rate and normalise |
| iq0 = (-q1*gx - q2*gy - q3*gz)*halfT; |
| iq1 = (q0*gx + q2*gz - q3*gy)*halfT; |
| iq2 = (q0*gy - q1*gz + q3*gx)*halfT; |
| iq3 = (q0*gz + q1*gy - q2*gx)*halfT; |
| |
| q0 += iq0; |
| q1 += iq1; |
| q2 += iq2; |
| q3 += iq3; |
| |
| // normalise quaternion |
| |
| norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); |
| q0 = q0 / norm; |
| q1 = q1 / norm; |
| q2 = q2 / norm; |
| q3 = q3 / norm; |
| |
| /* |
| norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); |
| q0 = q0 * norm; |
| q1 = q1 * norm; |
| q2 = q2 * norm; |
| q3 = q3 * norm; |
| */ |
| } |
| |
| void FreeIMU::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]); |
| |
| // 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; |
| } |
| |
| // Returns the Euler angles in radians 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 |
| void FreeIMU::getEuler(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) * 180/M_PI; // psi |
| angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]) * 180/M_PI; // 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) * 180/M_PI; // phi |
| } |
| |
| |
| |
| void FreeIMU::getYawPitchRoll(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) * 180/M_PI; |
| ypr[1] = atan(gx / sqrt(gy*gy + gz*gz)) * 180/M_PI; |
| ypr[2] = atan(gy / sqrt(gx*gx + gz*gz)) * 180/M_PI; |
| } |
| |
| |
| 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; |
| } |