Blame |
Last modification |
View Log
| RSS feed
/*
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;
}