A Team / Mbed 2 deprecated Polulu_IMU_Kalman

IMU Kalman Filter

Dependencies:   mbed

Fork of frdm_serial_K64F by Mechatronics

IMU.h@8:7a22b8294c5d, 2014-12-08 (annotated)

Committer:
Pawel_13
Date:
Mon Dec 08 13:21:33 2014 +0000
Revision:
8:7a22b8294c5d
Quadrocopter - Freescale k64F IMU and serial to NI LabView

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pawel_13 8:7a22b8294c5d 1 #ifndef IMU_H
Pawel_13 8:7a22b8294c5d 2 #define IMU_H
Pawel_13 8:7a22b8294c5d 3
Pawel_13 8:7a22b8294c5d 4 #include "mbed.h"
Pawel_13 8:7a22b8294c5d 5
Pawel_13 8:7a22b8294c5d 6 // definitions for the gyro sensor
Pawel_13 8:7a22b8294c5d 7 #define L3GD20_ADDR 0xD6
Pawel_13 8:7a22b8294c5d 8 #define L3GD20_CTRL_REG1 0x20
Pawel_13 8:7a22b8294c5d 9 #define L3GD20_CTRL_REG4 0x23
Pawel_13 8:7a22b8294c5d 10 #define L3GD20_STATUS_REG 0x27
Pawel_13 8:7a22b8294c5d 11 #define L3GD20_OUT_X_L 0x28
Pawel_13 8:7a22b8294c5d 12 #define L3GD20_RANGE_250DPS 0x00 // Measurement range selected by CTRL_REG4
Pawel_13 8:7a22b8294c5d 13 #define L3GD20_RANGE_500DPS 0x01 // Default range = 250 Degree-per-Second = 0.7 rev/second
Pawel_13 8:7a22b8294c5d 14 #define L3GD20_RANGE_2000DPS 0x02 // Range determines Sensitivity
Pawel_13 8:7a22b8294c5d 15 #define L3GD20_SENSITIVITY_250DPS 0.00875 // Roughly 22/256 for fixed point match
Pawel_13 8:7a22b8294c5d 16 #define L3GD20_SENSITIVITY_500DPS 0.0175 // Roughly 45/256
Pawel_13 8:7a22b8294c5d 17 #define L3GD20_SENSITIVITY_2000DPS 0.070 // Roughly 18/256
Pawel_13 8:7a22b8294c5d 18 #define L3GD20_DPS_TO_RADS 0.017453293 // = pi/180 degrees/s to rad/s multiplier
Pawel_13 8:7a22b8294c5d 19
Pawel_13 8:7a22b8294c5d 20
Pawel_13 8:7a22b8294c5d 21 // definitions for the accelerometer
Pawel_13 8:7a22b8294c5d 22 #define LSM303_A_ADDR 0x32 // address for writing. +1 for reading, see manual p. 20/42.
Pawel_13 8:7a22b8294c5d 23 #define LSM303_A_CTRL_REG1 0x20
Pawel_13 8:7a22b8294c5d 24 #define LSM303_A_CTRL_REG4 0x23
Pawel_13 8:7a22b8294c5d 25 #define LSM303_A_OUT_X_L 0x28
Pawel_13 8:7a22b8294c5d 26 #define LSM303_A_FS_2G 0x00 // Full Scale measuremetn range - selected by CTRL_REG4
Pawel_13 8:7a22b8294c5d 27 #define LSM303_A_Sensitivity 0.001 // Linear acceleration sensitivity
Pawel_13 8:7a22b8294c5d 28 #define LSM303_A_GRAVITY_EARTH 9.80665 // Earth's gravity in m/s^2 upon calibration of sensor
Pawel_13 8:7a22b8294c5d 29
Pawel_13 8:7a22b8294c5d 30 // definitions for the magnetic sensor
Pawel_13 8:7a22b8294c5d 31 #define LSM303_M_ADDR 0x3C // address for writing. +1 for reading, see datasheet p. 21/42.
Pawel_13 8:7a22b8294c5d 32 #define LSM303_M_CRA_REG 0x00
Pawel_13 8:7a22b8294c5d 33 #define LSM303_M_CRB_REG 0x01
Pawel_13 8:7a22b8294c5d 34 #define LSM303_M_MR_REG 0x02
Pawel_13 8:7a22b8294c5d 35 #define LSM303_M_OUT_X_H 0x03 // Watch out: order of H and L reversed
Pawel_13 8:7a22b8294c5d 36 #define LSM303_M_FS_13G 0x01 // Full Scale measuremetn range - selected by CRB_REG
Pawel_13 8:7a22b8294c5d 37 #define LSM303_M_Sensitivity 1100 // Corresponding sensitivity = 1100 Bits/Gauss
Pawel_13 8:7a22b8294c5d 38
Pawel_13 8:7a22b8294c5d 39
Pawel_13 8:7a22b8294c5d 40 class IMU
Pawel_13 8:7a22b8294c5d 41 {
Pawel_13 8:7a22b8294c5d 42 public:
Pawel_13 8:7a22b8294c5d 43 /* constructor */
Pawel_13 8:7a22b8294c5d 44 IMU(PinName sda, PinName scl);
Pawel_13 8:7a22b8294c5d 45
Pawel_13 8:7a22b8294c5d 46 /* accessible functions */
Pawel_13 8:7a22b8294c5d 47 char init(void);
Pawel_13 8:7a22b8294c5d 48 char readData(float *);
Pawel_13 8:7a22b8294c5d 49 void filterData(float *, double *);
Pawel_13 8:7a22b8294c5d 50
Pawel_13 8:7a22b8294c5d 51 private:
Pawel_13 8:7a22b8294c5d 52 I2C _i2c;
Pawel_13 8:7a22b8294c5d 53 char address;
Pawel_13 8:7a22b8294c5d 54 int16_t L3GD20_biasX; /* digit counts */
Pawel_13 8:7a22b8294c5d 55 int16_t L3GD20_biasY;
Pawel_13 8:7a22b8294c5d 56 int16_t L3GD20_biasZ;
Pawel_13 8:7a22b8294c5d 57 double g_0;
Pawel_13 8:7a22b8294c5d 58 double FF[3];
Pawel_13 8:7a22b8294c5d 59 double FD[6][9];
Pawel_13 8:7a22b8294c5d 60 };
Pawel_13 8:7a22b8294c5d 61
Pawel_13 8:7a22b8294c5d 62 #endif