Mechatronics / 2015_04_17_dydakt

Dydaktyka

Dependencies:   FastPWM mbed-src

Fork of 2015_04_17_quadro_bez_sterowania by Quadrocopter

IMU.cpp@8:dc48ce79ad59, 2015-01-21 (annotated)

Committer:
Michu90
Date:
Wed Jan 21 16:30:33 2015 +0000
Revision:
8:dc48ce79ad59
Parent:
5:c3caf8b83e6b
Child:
10:605b0bfadc2e
2015_01_21

Who changed what in which revision?

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