Subversion Repositories FlightCtrl

/*

FreeIMU.h - 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/>.

*/

// Uncomment the appropriated version of FreeIMU you are using

// #define FREEIMU_VER 1 // For FreeIMU v0.1

// #define FREEIMU_VER 2 // For FreeIMU v0.2

//#define FREEIMU_VER 3 // For FreeIMU v0.3 and v0.3.1

#define FREEIMU_VER 4 // For FreeIMU v0.3.5

// #define FREEIMU_VER 5 // For FreeIMU v0.3.5_MS

// #define FREEIMU_VER 6 // For FreeIMU v0.3.5_BMP

#include <Wire.h>

#include "WProgram.h"

#if FREEIMU_VER <= 3

  #include <ADXL345.h>

  // default I2C 7-bit addresses of the sensors

  #define FIMU_ACC_ADDR ADXL345_ADDR_ALT_LOW // SDO connected to GND

  //#define FIMU_ADXL345_DEF_ADDR ADXL345_ADDR_ALT_HIGH // SDO connected to GND

#else

  #include <bma180.h>

    #define FIMU_ACC_ADDR BMA180_ADDRESS_SDO_LOW 

    //#define FIMU_ACC_ADDR BMA180_ADDRESS_SDO_HIGH

#endif

#include <HMC58X3.h>

#include <ITG3200.h>

#ifndef FreeIMU_h

#define FreeIMU_h

#define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW

#define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND

// HMC5843 address is fixed so don't bother to define it

/*

// ITG3200 constants

#define FIMU_ITG3200_SMPLRT_DIV 0x15

#define FIMU_ITG3200_DLPF_FS 0x16

#define FIMU_ITG3200_INT_CFG 0x17

#define FIMU_ITG3200_PWR_MGM 0x3E

*/

#ifndef cbi

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))

#endif

class FreeIMU

{

  public:

    FreeIMU();

    void init();

    void init(bool fastmode);

    void init(int acc_addr, int gyro_addr, bool fastmode);

    void getRawValues(int * raw_values);

    void getValues(float * values);

    void getQ(float * q);

    void getEuler(float * angles);

    void getYawPitchRoll(float * ypr);

    // we make them public so that users can interact directly with device classes

    #if FREEIMU_VER <= 3

      ADXL345 acc;

    #else

      BMA180 acc;

    #endif

    HMC58X3 magn;

    ITG3200 gyro;

  private:

    int* raw_acc, raw_gyro, raw_magn;

    void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);

    float q0, q1, q2, q3; // quaternion elements representing the estimated orientation

    float iq0, iq1, iq2, iq3;

    float exInt, eyInt, ezInt;  // scaled integral error

    int lastUpdate, now; // sample period expressed in milliseconds

    float halfT; // half the sample period expressed in seconds

    int startLoopTime;

};

float invSqrt(float number);

#endif // FreeIMU_h