Eric Van den Bulck / IMU

Library for setting and reading the Pololu minIMU 9 v2 sensor

IMU.cpp@0:7b70a3ed96c1, 2013-10-26 (annotated)

Committer:
Euler
Date:
Sat Oct 26 11:52:29 2013 +0000
Revision:
0:7b70a3ed96c1
Library for setting and reading the Pololu MinIMU 9 v2 sensor.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Euler 0:7b70a3ed96c1 1 /**
Euler 0:7b70a3ed96c1 2 * @author Eric Van den Bulck
Euler 0:7b70a3ed96c1 3 *
Euler 0:7b70a3ed96c1 4 * @section LICENSE
Euler 0:7b70a3ed96c1 5 *
Euler 0:7b70a3ed96c1 6 * Copyright (c) 2010 ARM Limited
Euler 0:7b70a3ed96c1 7 *
Euler 0:7b70a3ed96c1 8 * @section DESCRIPTION
Euler 0:7b70a3ed96c1 9 *
Euler 0:7b70a3ed96c1 10 * Pololu MinIMU-9 v2 sensor:
Euler 0:7b70a3ed96c1 11 * L3GD20 three-axis digital output gyroscope.
Euler 0:7b70a3ed96c1 12 * LSM303 three-axis digital accelerometer and magnetometer
Euler 0:7b70a3ed96c1 13 *
Euler 0:7b70a3ed96c1 14 * Information to build this library:
Euler 0:7b70a3ed96c1 15 * 1. The Arduino libraries for this sensor from the Pololu and Adafruit websites, available at gitbub.
Euler 0:7b70a3ed96c1 16 * https://github.com/adafruit/Adafruit_L3GD20
Euler 0:7b70a3ed96c1 17 * https://github.com/pololu/L3G/tree/master/L3G
Euler 0:7b70a3ed96c1 18 * 2. The Rasberry Pi code at https://github.com/idavidstory/goPiCopter/tree/master/io/sensors
Euler 0:7b70a3ed96c1 19 * https://github.com/idavidstory/goPiCopter/tree/master/io/sensors
Euler 0:7b70a3ed96c1 20 * 3. Information on how to write libraries: http://mbed.org/cookbook/Writing-a-Library
Euler 0:7b70a3ed96c1 21 * 4. Information on I2C control: http://mbed.org/users/mbed_official/code/mbed/
Euler 0:7b70a3ed96c1 22 * 5. The Youtube videos from Brian Douglas (3 x 15') at http://www.youtube.com/playlist?list=PLUMWjy5jgHK30fkGrufluENJqZmLZkmqI
Euler 0:7b70a3ed96c1 23 * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
Euler 0:7b70a3ed96c1 24 * Reading an IMU Without Kalman: The Complementary Filter: http://www.pieter-jan.com/node/11
Euler 0:7b70a3ed96c1 25 * setup info on the minIMU-9 v2 on http://forum.pololu.com/viewtopic.php?f=3&t=4801&start=30
Euler 0:7b70a3ed96c1 26 */
Euler 0:7b70a3ed96c1 27
Euler 0:7b70a3ed96c1 28 #include "mbed.h"
Euler 0:7b70a3ed96c1 29 #include "IMU.h"
Euler 0:7b70a3ed96c1 30
Euler 0:7b70a3ed96c1 31 IMU::IMU(PinName sda, PinName scl) : _i2c(sda, scl) {
Euler 0:7b70a3ed96c1 32 _i2c.frequency(400000); /* 400kHz, fast mode. */
Euler 0:7b70a3ed96c1 33 }
Euler 0:7b70a3ed96c1 34
Euler 0:7b70a3ed96c1 35 char IMU::init(void) /* returns error upon a non-zero return */
Euler 0:7b70a3ed96c1 36 {
Euler 0:7b70a3ed96c1 37 char ack, rx, tx[2];
Euler 0:7b70a3ed96c1 38 double pi, a, A;
Euler 0:7b70a3ed96c1 39
Euler 0:7b70a3ed96c1 40 // 1. Initialize selected registers: 2c.read and i2c.write return 0 on success (ack)
Euler 0:7b70a3ed96c1 41 // --------------------------------
Euler 0:7b70a3ed96c1 42 //
Euler 0:7b70a3ed96c1 43 // 1.a Enable L3DG20 gyrosensor and set operational mode:
Euler 0:7b70a3ed96c1 44 // CTRL_REG1: set to 0x1F = 0001-1111 --> enable sensor, DR= 95Hz, LPF-Cut-off-freq=25Hz.
Euler 0:7b70a3ed96c1 45 // CTRL_REG1: set to 0x5F = 0101-1111 --> enable sensor, DR=190Hz, LPF-Cut-off-freq=25Hz.
Euler 0:7b70a3ed96c1 46 // CTRL_REG4: left at default = 0x00 --> Full Scale = 250 degrees/second --> Sensitivity = 0.00875 dps/digit.
Euler 0:7b70a3ed96c1 47 address = L3GD20_ADDR;
Euler 0:7b70a3ed96c1 48 tx[0] = L3GD20_CTRL_REG1; // address contrl_register 1
Euler 0:7b70a3ed96c1 49 tx[1] = 0x1F; // 00-01-1-111 enable sensor and set operational mode.
Euler 0:7b70a3ed96c1 50 ack = _i2c.write(address, tx, 2);
Euler 0:7b70a3ed96c1 51 ack |= _i2c.write(address, tx, 1);
Euler 0:7b70a3ed96c1 52 ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x1F) ack |= 1;
Euler 0:7b70a3ed96c1 53 //
Euler 0:7b70a3ed96c1 54 // 1.b Enable LSM303 accelerometer and set operational mode:
Euler 0:7b70a3ed96c1 55 // CTRL_REG1: set to 0x37 = 0011 0111 --> DR = 25Hz & enable sensor
Euler 0:7b70a3ed96c1 56 // CTRL_REG1: set to 0x47 = 0100 0111 --> DR = 50Hz & enable sensor
Euler 0:7b70a3ed96c1 57 // CTRL_REG1: set to 0x57 = 0101 0111 --> DR = 100Hz & enable sensor
Euler 0:7b70a3ed96c1 58 // CTRL_REG1: set to 0x77 = 0111 0111 --> DR = 200Hz & enable sensor
Euler 0:7b70a3ed96c1 59 // CTRL_REG4: set to 0x08 = 0000 1000 --> Full Scale = +/- 2G & high resolution --> Sensitivity = 0.001G/digit.
Euler 0:7b70a3ed96c1 60 address = LSM303_A_ADDR;
Euler 0:7b70a3ed96c1 61 tx[0] = LSM303_A_CTRL_REG1;
Euler 0:7b70a3ed96c1 62 tx[1] = 0x57; // --> 200 Hz Data rate speed - p.24/42 of datasheet
Euler 0:7b70a3ed96c1 63 ack |= _i2c.write(address, tx, 2);
Euler 0:7b70a3ed96c1 64 ack |= _i2c.write(address, tx, 1);
Euler 0:7b70a3ed96c1 65 ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x57) ack |= 1;
Euler 0:7b70a3ed96c1 66 tx[0] = LSM303_A_CTRL_REG4;
Euler 0:7b70a3ed96c1 67 tx[1] = 0x08; // 0000 1000 enable high resolution mode + selects default 2G scale. p.26/42
Euler 0:7b70a3ed96c1 68 ack |= _i2c.write(address, tx ,2);
Euler 0:7b70a3ed96c1 69 ack |= _i2c.write(address, tx, 1);
Euler 0:7b70a3ed96c1 70 ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x08) ack |= 1;
Euler 0:7b70a3ed96c1 71 //
Euler 0:7b70a3ed96c1 72 // 1.c enable LSM303 magnetometer and set operational mode:
Euler 0:7b70a3ed96c1 73 // CRA_REG is reset from 0x10 to 0x14 = 00010100 --> 30 Hz data output rate.
Euler 0:7b70a3ed96c1 74 // CRA_REG is reset from 0x10 to 0x18 = 00011000 --> 75 Hz data output rate.
Euler 0:7b70a3ed96c1 75 // CRA_REG is reset from 0x10 to 0x1C = 00011100 --> 220 Hz data output rate.
Euler 0:7b70a3ed96c1 76 // CRB_REG is kept at default = 00100000 = 0x20 --> range +/- 1.3 Gauss, Gain = 1100/980(Z) LSB/Gauss.
Euler 0:7b70a3ed96c1 77 // MR_REG is reset from 0x03 to 0x00 -> continuos conversion mode in stead of sleep mode.
Euler 0:7b70a3ed96c1 78 address = LSM303_M_ADDR;
Euler 0:7b70a3ed96c1 79 tx[0] = LSM303_M_CRA_REG;
Euler 0:7b70a3ed96c1 80 tx[1] = 0x18; // --> 75 Hz minimum output rate - p.36/42 of datasheet
Euler 0:7b70a3ed96c1 81 ack |= _i2c.write(address, tx, 2);
Euler 0:7b70a3ed96c1 82 ack |= _i2c.write(address, tx, 1);
Euler 0:7b70a3ed96c1 83 ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x18) ack |= 1;
Euler 0:7b70a3ed96c1 84 tx[0] = LSM303_M_MR_REG;
Euler 0:7b70a3ed96c1 85 tx[1] = 0x00; // 0000 0000 --> continuous-conversion mode 25 Hz Data rate speed - p.24/42 of datasheet
Euler 0:7b70a3ed96c1 86 ack |= _i2c.write(address, tx, 2);
Euler 0:7b70a3ed96c1 87 ack |= _i2c.write(address, tx, 1);
Euler 0:7b70a3ed96c1 88 ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x00) ack |= 1;
Euler 0:7b70a3ed96c1 89
Euler 0:7b70a3ed96c1 90 // 2. Initialize calibration constants with predetermined values.
Euler 0:7b70a3ed96c1 91 // acceleration:
Euler 0:7b70a3ed96c1 92 // My calibration values, vs. the website http://rwsarduino.blogspot.be/2013/01/inertial-orientation-sensing.html
Euler 0:7b70a3ed96c1 93
Euler 0:7b70a3ed96c1 94 /* my predetermined static bias counts */
Euler 0:7b70a3ed96c1 95 L3GD20_biasX = (int16_t) 90; /* digit counts */
Euler 0:7b70a3ed96c1 96 L3GD20_biasY = (int16_t) -182;
Euler 0:7b70a3ed96c1 97 L3GD20_biasZ = (int16_t) -10;
Euler 0:7b70a3ed96c1 98
Euler 0:7b70a3ed96c1 99 /* reference gravity acceleration */
Euler 0:7b70a3ed96c1 100 g_0 = 9.815;
Euler 0:7b70a3ed96c1 101
Euler 0:7b70a3ed96c1 102 /* filter parameters: assume 50 Hz sampling rare and 2nd orcer Butterworth filter with fc = 6Hz. */
Euler 0:7b70a3ed96c1 103 pi = 3.1415926536;
Euler 0:7b70a3ed96c1 104 A = tan(pi*6/50); a = 1 + sqrt(2.0)*A + A*A;
Euler 0:7b70a3ed96c1 105 FF[1] = 2*(A*A-1)/a;
Euler 0:7b70a3ed96c1 106 FF[2] = (1-sqrt(2.0)*A+A*A)/a;
Euler 0:7b70a3ed96c1 107 FF[0] = (1+FF[1]+FF[2])/4;
Euler 0:7b70a3ed96c1 108
Euler 0:7b70a3ed96c1 109 return ack;
Euler 0:7b70a3ed96c1 110 }
Euler 0:7b70a3ed96c1 111
Euler 0:7b70a3ed96c1 112 char IMU::readData(float *d)
Euler 0:7b70a3ed96c1 113 {
Euler 0:7b70a3ed96c1 114 char ack, reg, D[6];
Euler 0:7b70a3ed96c1 115 int16_t W[3];
Euler 0:7b70a3ed96c1 116
Euler 0:7b70a3ed96c1 117 // report the data in rad/s
Euler 0:7b70a3ed96c1 118 // gyro data are 16 bit readings per axis, stored: X_l, X_h, Y_l, Y_h, Z_l, Z_h
Euler 0:7b70a3ed96c1 119 // #define L3GD20_SENSITIVITY_250DPS 0.00875 --- #define L3GD20_DPS_TO_RADS 0.017453293
Euler 0:7b70a3ed96c1 120 address = L3GD20_ADDR;
Euler 0:7b70a3ed96c1 121 reg = L3GD20_OUT_X_L | 0x80; // set address auto-increment bit
Euler 0:7b70a3ed96c1 122 ack = _i2c.write(address,&reg,1); ack |= _i2c.read(address+1,D,6);
Euler 0:7b70a3ed96c1 123 W[0] = (int16_t) (D[1] << 8 | D[0]);
Euler 0:7b70a3ed96c1 124 W[1] = (int16_t) (D[3] << 8 | D[2]);
Euler 0:7b70a3ed96c1 125 W[2] = (int16_t) (D[5] << 8 | D[4]);
Euler 0:7b70a3ed96c1 126 *(d+0) = (float) 0.971*(W[0]-L3GD20_biasX)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
Euler 0:7b70a3ed96c1 127 *(d+1) = (float) 0.998*(W[1]-L3GD20_biasY)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
Euler 0:7b70a3ed96c1 128 *(d+2) = (float) 1.002*(W[2]-L3GD20_biasZ)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
Euler 0:7b70a3ed96c1 129
Euler 0:7b70a3ed96c1 130 // Accelerometer data are stored as 12 bit readings, left justified per axis.
Euler 0:7b70a3ed96c1 131 // The data needs to be shifted 4 digits to the right! This is not general, only for the A measurement.
Euler 0:7b70a3ed96c1 132 address = LSM303_A_ADDR;
Euler 0:7b70a3ed96c1 133 reg = LSM303_A_OUT_X_L | 0x80; // set address auto-increment bit
Euler 0:7b70a3ed96c1 134 ack |= _i2c.write(address,&reg,1); ack |= _i2c.read(address+1,D,6);
Euler 0:7b70a3ed96c1 135 W[0] = ((int16_t) (D[1] << 8 | D[0])) >> 4;
Euler 0:7b70a3ed96c1 136 W[1] = ((int16_t) (D[3] << 8 | D[2])) >> 4;
Euler 0:7b70a3ed96c1 137 W[2] = ((int16_t) (D[5] << 8 | D[4])) >> 4;
Euler 0:7b70a3ed96c1 138 *(d+3) = (float) g_0*0.991*(W[0]+34)/1000;
Euler 0:7b70a3ed96c1 139 *(d+4) = (float) g_0*0.970*(W[1]+2)/1000;
Euler 0:7b70a3ed96c1 140 *(d+5) = (float) g_0*0.983*(W[2]+28)/1000;
Euler 0:7b70a3ed96c1 141 // GN = 001
Euler 0:7b70a3ed96c1 142 // Magnetometer; are stored as 12 bit readings, right justified per axis.
Euler 0:7b70a3ed96c1 143 address = LSM303_M_ADDR;
Euler 0:7b70a3ed96c1 144 reg = LSM303_M_OUT_X_H | 0x80; // set address auto-increment bit
Euler 0:7b70a3ed96c1 145 ack |= _i2c.write(address,&reg,1); ack |= _i2c.read(address+1,D,6);
Euler 0:7b70a3ed96c1 146 W[0] = ((int16_t) (D[0] << 8 | D[1]));
Euler 0:7b70a3ed96c1 147 W[1] = ((int16_t) (D[4] << 8 | D[5]));
Euler 0:7b70a3ed96c1 148 W[2] = ((int16_t) (D[2] << 8 | D[3]));
Euler 0:7b70a3ed96c1 149 *(d+6) = (float) 2.813*(W[0]-264)/1100;
Euler 0:7b70a3ed96c1 150 *(d+7) = (float) 2.822*(W[1]- 98)/1100;
Euler 0:7b70a3ed96c1 151 *(d+8) = (float) 2.880*(W[2]-305)/980;
Euler 0:7b70a3ed96c1 152
Euler 0:7b70a3ed96c1 153 return ack;
Euler 0:7b70a3ed96c1 154 }
Euler 0:7b70a3ed96c1 155
Euler 0:7b70a3ed96c1 156 void IMU::filterData(float *d, double *D)
Euler 0:7b70a3ed96c1 157 // 2nd order Butterworth filter. Filter coefficients FF computed in function init.
Euler 0:7b70a3ed96c1 158 {
Euler 0:7b70a3ed96c1 159 for (int i=0; i<9; ++i) {
Euler 0:7b70a3ed96c1 160 // *(FD+9*i+2) = *(FD+9*i+1); *(FD+9*i+1) = *(FD+9*i); *(FD+9*i) = (double) d[i];
Euler 0:7b70a3ed96c1 161 FD[2][i] = FD[1][i]; FD[1][i] = FD[0][i]; FD[0][i] = (double) d[i];
Euler 0:7b70a3ed96c1 162 FD[5][i] = FD[4][i]; FD[4][i] = FD[3][i];
Euler 0:7b70a3ed96c1 163 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];
Euler 0:7b70a3ed96c1 164 D[i] = FD[3][i];
Euler 0:7b70a3ed96c1 165 }
Euler 0:7b70a3ed96c1 166 // D[0] = FD[0][2]; D[1] = FD[1][2]; D[2] = FD[2][2];
Euler 0:7b70a3ed96c1 167 }