File content as of revision 29:484a501b8674:

/*
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 "FreeIMU.h"
#include "mbed.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 / 6.0;
    twoKpz = twoKpDef * 12.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();

#if HAS_MPU6050()
    magn = new HMC58X3();
#endif

    Thread::wait(10);

    accgyro->initialize();
    accgyro->setI2CMasterModeEnabled(0);
    accgyro->setI2CBypassEnabled(1);
    Thread::wait(20);
    
#if HAS_MPU9250()
    accgyro->initialize9250();
    accgyro->initialize9250MasterMode();
    Thread::wait(50);
#endif

    accgyro->setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    // both accelerometer and gyroscope will output at 1khz
    accgyro->setDLPFMode(1);
    accgyro->setRate(0);
    Thread::wait(20);

    accgyro->start_sampling();

    Thread::wait(20);

#if HAS_MPU6050()
    // 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();
#endif
    
    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)
{

#if HAS_MPU6050()
    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]);
#endif

#if HAS_MPU9250()
    accgyro->getMotion9(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5], &raw_values[6], &raw_values[7], &raw_values[8]);
#endif

    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)
{
#if HAS_MPU6050()
    // MPU6050
    int16_t accgyroval[6];
    accgyro->getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
#endif

#if HAS_MPU9250()
    // MPU6250
    int16_t accgyroval[9];
    accgyro->getMotion9(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5], &accgyroval[6], &accgyroval[7], &accgyroval[8]);
    values[6] = accgyroval[6];
    values[7] = accgyroval[7];
    values[8] = accgyroval[8];
#endif

    // 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]) / 16.4f; // 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;

#if HAS_MPU6050()
    magn->getValues(&values[6]);
#endif

    // 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;
}

#if HAS_MPU6050()
extern volatile bool magn_valid;
#endif

#if HAS_MPU9250()
// need to automatically turn this to true at 100Hz. This is done within the MPU9250 lib.
extern volatile bool magn_valid_9250;
#endif


/**
 * 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);

#if HAS_MPU6050()
        magn_valid = false;
#elif HAS_MPU9250()
        magn_valid_9250 = false;
#endif
    }

    // 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

    bool _magn_valid;
#if HAS_MPU6050()
    _magn_valid = magn_valid;
#elif HAS_MPU9250()
    _magn_valid = magn_valid_9250;
#endif
    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;
}