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?
User | Revision | Line number | New 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,®,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,®,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,®,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 | } |