0,0 → 1,103 |
/* |
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 |
|