Library for setting and reading the Pololu minIMU 9 v2 sensor
Revision 0:7b70a3ed96c1, committed 2013-10-26
- Comitter:
- Euler
- Date:
- Sat Oct 26 11:52:29 2013 +0000
- Commit message:
- Library for setting and reading the Pololu MinIMU 9 v2 sensor.
Changed in this revision
IMU.cpp | Show annotated file Show diff for this revision Revisions of this file |
IMU.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r 7b70a3ed96c1 IMU.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU.cpp Sat Oct 26 11:52:29 2013 +0000 @@ -0,0 +1,167 @@ +/** + * @author Eric Van den Bulck + * + * @section LICENSE + * + * Copyright (c) 2010 ARM Limited + * + * @section DESCRIPTION + * + * Pololu MinIMU-9 v2 sensor: + * L3GD20 three-axis digital output gyroscope. + * LSM303 three-axis digital accelerometer and magnetometer + * + * Information to build this library: + * 1. The Arduino libraries for this sensor from the Pololu and Adafruit websites, available at gitbub. + * https://github.com/adafruit/Adafruit_L3GD20 + * https://github.com/pololu/L3G/tree/master/L3G + * 2. The Rasberry Pi code at https://github.com/idavidstory/goPiCopter/tree/master/io/sensors + * https://github.com/idavidstory/goPiCopter/tree/master/io/sensors + * 3. Information on how to write libraries: http://mbed.org/cookbook/Writing-a-Library + * 4. Information on I2C control: http://mbed.org/users/mbed_official/code/mbed/ + * 5. The Youtube videos from Brian Douglas (3 x 15') at http://www.youtube.com/playlist?list=PLUMWjy5jgHK30fkGrufluENJqZmLZkmqI + * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ + * Reading an IMU Without Kalman: The Complementary Filter: http://www.pieter-jan.com/node/11 + * setup info on the minIMU-9 v2 on http://forum.pololu.com/viewtopic.php?f=3&t=4801&start=30 + */ + +#include "mbed.h" +#include "IMU.h" + +IMU::IMU(PinName sda, PinName scl) : _i2c(sda, scl) { + _i2c.frequency(400000); /* 400kHz, fast mode. */ +} + +char IMU::init(void) /* returns error upon a non-zero return */ +{ + char ack, rx, tx[2]; + double pi, a, A; + +// 1. Initialize selected registers: 2c.read and i2c.write return 0 on success (ack) +// -------------------------------- +// +// 1.a Enable L3DG20 gyrosensor and set operational mode: +// CTRL_REG1: set to 0x1F = 0001-1111 --> enable sensor, DR= 95Hz, LPF-Cut-off-freq=25Hz. +// CTRL_REG1: set to 0x5F = 0101-1111 --> enable sensor, DR=190Hz, LPF-Cut-off-freq=25Hz. +// CTRL_REG4: left at default = 0x00 --> Full Scale = 250 degrees/second --> Sensitivity = 0.00875 dps/digit. + address = L3GD20_ADDR; + tx[0] = L3GD20_CTRL_REG1; // address contrl_register 1 + tx[1] = 0x1F; // 00-01-1-111 enable sensor and set operational mode. + ack = _i2c.write(address, tx, 2); + ack |= _i2c.write(address, tx, 1); + ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x1F) ack |= 1; +// +// 1.b Enable LSM303 accelerometer and set operational mode: +// CTRL_REG1: set to 0x37 = 0011 0111 --> DR = 25Hz & enable sensor +// CTRL_REG1: set to 0x47 = 0100 0111 --> DR = 50Hz & enable sensor +// CTRL_REG1: set to 0x57 = 0101 0111 --> DR = 100Hz & enable sensor +// CTRL_REG1: set to 0x77 = 0111 0111 --> DR = 200Hz & enable sensor +// CTRL_REG4: set to 0x08 = 0000 1000 --> Full Scale = +/- 2G & high resolution --> Sensitivity = 0.001G/digit. + address = LSM303_A_ADDR; + tx[0] = LSM303_A_CTRL_REG1; + tx[1] = 0x57; // --> 200 Hz Data rate speed - p.24/42 of datasheet + ack |= _i2c.write(address, tx, 2); + ack |= _i2c.write(address, tx, 1); + ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x57) ack |= 1; + tx[0] = LSM303_A_CTRL_REG4; + tx[1] = 0x08; // 0000 1000 enable high resolution mode + selects default 2G scale. p.26/42 + ack |= _i2c.write(address, tx ,2); + ack |= _i2c.write(address, tx, 1); + ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x08) ack |= 1; +// +// 1.c enable LSM303 magnetometer and set operational mode: +// CRA_REG is reset from 0x10 to 0x14 = 00010100 --> 30 Hz data output rate. +// CRA_REG is reset from 0x10 to 0x18 = 00011000 --> 75 Hz data output rate. +// CRA_REG is reset from 0x10 to 0x1C = 00011100 --> 220 Hz data output rate. +// CRB_REG is kept at default = 00100000 = 0x20 --> range +/- 1.3 Gauss, Gain = 1100/980(Z) LSB/Gauss. +// MR_REG is reset from 0x03 to 0x00 -> continuos conversion mode in stead of sleep mode. + address = LSM303_M_ADDR; + tx[0] = LSM303_M_CRA_REG; + tx[1] = 0x18; // --> 75 Hz minimum output rate - p.36/42 of datasheet + ack |= _i2c.write(address, tx, 2); + ack |= _i2c.write(address, tx, 1); + ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x18) ack |= 1; + tx[0] = LSM303_M_MR_REG; + tx[1] = 0x00; // 0000 0000 --> continuous-conversion mode 25 Hz Data rate speed - p.24/42 of datasheet + ack |= _i2c.write(address, tx, 2); + ack |= _i2c.write(address, tx, 1); + ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x00) ack |= 1; + +// 2. Initialize calibration constants with predetermined values. +// acceleration: +// My calibration values, vs. the website http://rwsarduino.blogspot.be/2013/01/inertial-orientation-sensing.html + +/* my predetermined static bias counts */ + L3GD20_biasX = (int16_t) 90; /* digit counts */ + L3GD20_biasY = (int16_t) -182; + L3GD20_biasZ = (int16_t) -10; + +/* reference gravity acceleration */ + g_0 = 9.815; + +/* filter parameters: assume 50 Hz sampling rare and 2nd orcer Butterworth filter with fc = 6Hz. */ + pi = 3.1415926536; + A = tan(pi*6/50); a = 1 + sqrt(2.0)*A + A*A; + FF[1] = 2*(A*A-1)/a; + FF[2] = (1-sqrt(2.0)*A+A*A)/a; + FF[0] = (1+FF[1]+FF[2])/4; + + return ack; +} + +char IMU::readData(float *d) +{ + char ack, reg, D[6]; + int16_t W[3]; + +// report the data in rad/s +// gyro data are 16 bit readings per axis, stored: X_l, X_h, Y_l, Y_h, Z_l, Z_h +// #define L3GD20_SENSITIVITY_250DPS 0.00875 --- #define L3GD20_DPS_TO_RADS 0.017453293 + address = L3GD20_ADDR; + reg = L3GD20_OUT_X_L | 0x80; // set address auto-increment bit + ack = _i2c.write(address,®,1); ack |= _i2c.read(address+1,D,6); + W[0] = (int16_t) (D[1] << 8 | D[0]); + W[1] = (int16_t) (D[3] << 8 | D[2]); + W[2] = (int16_t) (D[5] << 8 | D[4]); + *(d+0) = (float) 0.971*(W[0]-L3GD20_biasX)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS; + *(d+1) = (float) 0.998*(W[1]-L3GD20_biasY)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS; + *(d+2) = (float) 1.002*(W[2]-L3GD20_biasZ)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS; + +// Accelerometer data are stored as 12 bit readings, left justified per axis. +// The data needs to be shifted 4 digits to the right! This is not general, only for the A measurement. + address = LSM303_A_ADDR; + reg = LSM303_A_OUT_X_L | 0x80; // set address auto-increment bit + ack |= _i2c.write(address,®,1); ack |= _i2c.read(address+1,D,6); + W[0] = ((int16_t) (D[1] << 8 | D[0])) >> 4; + W[1] = ((int16_t) (D[3] << 8 | D[2])) >> 4; + W[2] = ((int16_t) (D[5] << 8 | D[4])) >> 4; + *(d+3) = (float) g_0*0.991*(W[0]+34)/1000; + *(d+4) = (float) g_0*0.970*(W[1]+2)/1000; + *(d+5) = (float) g_0*0.983*(W[2]+28)/1000; +// GN = 001 +// Magnetometer; are stored as 12 bit readings, right justified per axis. + address = LSM303_M_ADDR; + reg = LSM303_M_OUT_X_H | 0x80; // set address auto-increment bit + ack |= _i2c.write(address,®,1); ack |= _i2c.read(address+1,D,6); + W[0] = ((int16_t) (D[0] << 8 | D[1])); + W[1] = ((int16_t) (D[4] << 8 | D[5])); + W[2] = ((int16_t) (D[2] << 8 | D[3])); + *(d+6) = (float) 2.813*(W[0]-264)/1100; + *(d+7) = (float) 2.822*(W[1]- 98)/1100; + *(d+8) = (float) 2.880*(W[2]-305)/980; + + return ack; +} + +void IMU::filterData(float *d, double *D) +// 2nd order Butterworth filter. Filter coefficients FF computed in function init. +{ + for (int i=0; i<9; ++i) { +// *(FD+9*i+2) = *(FD+9*i+1); *(FD+9*i+1) = *(FD+9*i); *(FD+9*i) = (double) d[i]; + FD[2][i] = FD[1][i]; FD[1][i] = FD[0][i]; FD[0][i] = (double) d[i]; + FD[5][i] = FD[4][i]; FD[4][i] = FD[3][i]; + FD[3][i] = FF[0]*(FD[0][i] + 2*FD[1][i] + FD[2][i]) - FF[1]*FD[4][i] - FF[2]*FD[5][i]; + D[i] = FD[3][i]; + } +// D[0] = FD[0][2]; D[1] = FD[1][2]; D[2] = FD[2][2]; +}
diff -r 000000000000 -r 7b70a3ed96c1 IMU.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU.h Sat Oct 26 11:52:29 2013 +0000 @@ -0,0 +1,62 @@ +#ifndef IMU_H +#define IMU_H + +#include "mbed.h" + +// definitions for the gyro sensor +#define L3GD20_ADDR 0xD6 +#define L3GD20_CTRL_REG1 0x20 +#define L3GD20_CTRL_REG4 0x23 +#define L3GD20_STATUS_REG 0x27 +#define L3GD20_OUT_X_L 0x28 +#define L3GD20_RANGE_250DPS 0x00 // Measurement range selected by CTRL_REG4 +#define L3GD20_RANGE_500DPS 0x01 // Default range = 250 Degree-per-Second = 0.7 rev/second +#define L3GD20_RANGE_2000DPS 0x02 // Range determines Sensitivity +#define L3GD20_SENSITIVITY_250DPS 0.00875 // Roughly 22/256 for fixed point match +#define L3GD20_SENSITIVITY_500DPS 0.0175 // Roughly 45/256 +#define L3GD20_SENSITIVITY_2000DPS 0.070 // Roughly 18/256 +#define L3GD20_DPS_TO_RADS 0.017453293 // = pi/180 degrees/s to rad/s multiplier + + +// definitions for the accelerometer +#define LSM303_A_ADDR 0x32 // address for writing. +1 for reading, see manual p. 20/42. +#define LSM303_A_CTRL_REG1 0x20 +#define LSM303_A_CTRL_REG4 0x23 +#define LSM303_A_OUT_X_L 0x28 +#define LSM303_A_FS_2G 0x00 // Full Scale measuremetn range - selected by CTRL_REG4 +#define LSM303_A_Sensitivity 0.001 // Linear acceleration sensitivity +#define LSM303_A_GRAVITY_EARTH 9.80665 // Earth's gravity in m/s^2 upon calibration of sensor + +// definitions for the magnetic sensor +#define LSM303_M_ADDR 0x3C // address for writing. +1 for reading, see datasheet p. 21/42. +#define LSM303_M_CRA_REG 0x00 +#define LSM303_M_CRB_REG 0x01 +#define LSM303_M_MR_REG 0x02 +#define LSM303_M_OUT_X_H 0x03 // Watch out: order of H and L reversed +#define LSM303_M_FS_13G 0x01 // Full Scale measuremetn range - selected by CRB_REG +#define LSM303_M_Sensitivity 1100 // Corresponding sensitivity = 1100 Bits/Gauss + + +class IMU +{ + public: +/* constructor */ + IMU(PinName sda, PinName scl); + +/* accessible functions */ + char init(void); + char readData(float *); + void filterData(float *, double *); + + private: + I2C _i2c; + char address; + int16_t L3GD20_biasX; /* digit counts */ + int16_t L3GD20_biasY; + int16_t L3GD20_biasZ; + double g_0; + double FF[3]; + double FD[6][9]; +}; + +#endif