Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

Sensors/Gyro/L3G4200D.cpp@11:9bf69bc6df45, 2012-10-18 (annotated)

Committer:
maetugr
Date:
Thu Oct 18 20:04:16 2012 +0000
Revision:
11:9bf69bc6df45
Parent:
10:953afcbcebfc
Child:
14:cf260677ecde
Kompass immernoch nicht gut, vor Kalibrierungsberechnung

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:0c4fafa398b4 1 #include "mbed.h"
maetugr 0:0c4fafa398b4 2 #include "L3G4200D.h"
maetugr 0:0c4fafa398b4 3 #include <math.h>
maetugr 0:0c4fafa398b4 4
maetugr 0:0c4fafa398b4 5 #define L3G4200D_I2C_ADDRESS 0xD0
maetugr 0:0c4fafa398b4 6
maetugr 0:0c4fafa398b4 7
maetugr 2:93f703d2c4d7 8 L3G4200D::L3G4200D(PinName sda, PinName scl) : i2c(sda, scl)
maetugr 0:0c4fafa398b4 9 {
maetugr 0:0c4fafa398b4 10 i2c.frequency(400000);
maetugr 0:0c4fafa398b4 11 // Turns on the L3G4200D's gyro and places it in normal mode.
maetugr 0:0c4fafa398b4 12 // Normal power mode, all axes enabled
maetugr 0:0c4fafa398b4 13
maetugr 2:93f703d2c4d7 14 //writeReg(L3G4200D_CTRL_REG2, 0x05); // control filter
maetugr 2:93f703d2c4d7 15 writeReg(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled
maetugr 0:0c4fafa398b4 16 writeReg(L3G4200D_CTRL_REG3, 0x00);
maetugr 2:93f703d2c4d7 17 writeReg(L3G4200D_CTRL_REG4, 0x20); // acuracy 2000 dps
maetugr 0:0c4fafa398b4 18
maetugr 0:0c4fafa398b4 19 writeReg(L3G4200D_REFERENCE, 0x00);
maetugr 0:0c4fafa398b4 20 //writeReg(L3G4200D_STATUS_REG, 0x0F);
maetugr 0:0c4fafa398b4 21 writeReg(L3G4200D_INT1_THS_XH, 0x2C);
maetugr 0:0c4fafa398b4 22 writeReg(L3G4200D_INT1_THS_XL, 0xA4);
maetugr 0:0c4fafa398b4 23 writeReg(L3G4200D_INT1_THS_YH, 0x2C);
maetugr 0:0c4fafa398b4 24 writeReg(L3G4200D_INT1_THS_YL, 0xA4);
maetugr 0:0c4fafa398b4 25 writeReg(L3G4200D_INT1_THS_ZH, 0x2C);
maetugr 0:0c4fafa398b4 26 writeReg(L3G4200D_INT1_THS_ZL, 0xA4);
maetugr 0:0c4fafa398b4 27 //writeReg(L3G4200D_INT1_DURATION, 0x00);
maetugr 2:93f703d2c4d7 28 //writeReg(L3G4200D_CTRL_REG5, 0x12); // Filter einschalten
maetugr 2:93f703d2c4d7 29 //writeReg(L3G4200D_CTRL_REG5, 0x01); // hochpass Filter einschalten
maetugr 2:93f703d2c4d7 30 writeReg(L3G4200D_CTRL_REG5, 0x00); // Filter ausschalten
maetugr 0:0c4fafa398b4 31
maetugr 0:0c4fafa398b4 32 writeReg(L3G4200D_CTRL_REG1, 0x0F); // Gogo
maetugr 2:93f703d2c4d7 33
maetugr 2:93f703d2c4d7 34 // calibrate gyro with an average of count samples (result to offset)
maetugr 2:93f703d2c4d7 35 #define count 50
maetugr 2:93f703d2c4d7 36 for (int j = 0; j < 3; j++)
maetugr 2:93f703d2c4d7 37 offset[j] = 0;
maetugr 2:93f703d2c4d7 38
maetugr 2:93f703d2c4d7 39 float Gyro_calib[3] = {0,0,0}; // temporary to sum up
maetugr 2:93f703d2c4d7 40
maetugr 2:93f703d2c4d7 41 for (int i = 0; i < count; i++) {
maetugr 10:953afcbcebfc 42 read();
maetugr 2:93f703d2c4d7 43 for (int j = 0; j < 3; j++)
maetugr 10:953afcbcebfc 44 Gyro_calib[j] += data[j];
maetugr 2:93f703d2c4d7 45 wait(0.001); // maybe less or no wait !!
maetugr 2:93f703d2c4d7 46 }
maetugr 2:93f703d2c4d7 47
maetugr 2:93f703d2c4d7 48 for (int j = 0; j < 3; j++)
maetugr 2:93f703d2c4d7 49 offset[j] = Gyro_calib[j]/count;
maetugr 0:0c4fafa398b4 50 }
maetugr 0:0c4fafa398b4 51
maetugr 0:0c4fafa398b4 52 // Writes a gyro register
maetugr 1:5a64632b1eb9 53 void L3G4200D::writeReg(byte reg, byte value)
maetugr 0:0c4fafa398b4 54 {
maetugr 2:93f703d2c4d7 55 byte buffer[2];
maetugr 2:93f703d2c4d7 56 buffer[0] = reg;
maetugr 2:93f703d2c4d7 57 buffer[1] = value;
maetugr 0:0c4fafa398b4 58
maetugr 2:93f703d2c4d7 59 i2c.write(L3G4200D_I2C_ADDRESS, buffer, 2);
maetugr 0:0c4fafa398b4 60 }
maetugr 0:0c4fafa398b4 61
maetugr 0:0c4fafa398b4 62 // Reads a gyro register
maetugr 1:5a64632b1eb9 63 byte L3G4200D::readReg(byte reg)
maetugr 0:0c4fafa398b4 64 {
maetugr 0:0c4fafa398b4 65 byte value = 0;
maetugr 0:0c4fafa398b4 66
maetugr 0:0c4fafa398b4 67 i2c.write(L3G4200D_I2C_ADDRESS, &reg, 1);
maetugr 0:0c4fafa398b4 68 i2c.read(L3G4200D_I2C_ADDRESS, &value, 1);
maetugr 0:0c4fafa398b4 69
maetugr 0:0c4fafa398b4 70 return value;
maetugr 0:0c4fafa398b4 71 }
maetugr 0:0c4fafa398b4 72
maetugr 0:0c4fafa398b4 73 // Reads the 3 gyro channels and stores them in vector g
maetugr 10:953afcbcebfc 74 void L3G4200D::read()
maetugr 0:0c4fafa398b4 75 {
maetugr 2:93f703d2c4d7 76 byte buffer[6]; // 8-Bit pieces of axis data
maetugr 0:0c4fafa398b4 77 // assert the MSB of the address to get the gyro
maetugr 0:0c4fafa398b4 78 // to do slave-transmit subaddress updating.
maetugr 2:93f703d2c4d7 79 buffer[0] = L3G4200D_OUT_X_L | (1 << 7);
maetugr 2:93f703d2c4d7 80 i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1);
maetugr 0:0c4fafa398b4 81
maetugr 2:93f703d2c4d7 82 i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6);
maetugr 0:0c4fafa398b4 83
maetugr 11:9bf69bc6df45 84 data[0] = (short) (buffer[1] << 8 | buffer[0]);
maetugr 11:9bf69bc6df45 85 data[1] = (short) (buffer[3] << 8 | buffer[2]);
maetugr 11:9bf69bc6df45 86 data[2] = (short) (buffer[5] << 8 | buffer[4]);
maetugr 2:93f703d2c4d7 87
maetugr 2:93f703d2c4d7 88 //with offset of calibration
maetugr 2:93f703d2c4d7 89 for (int j = 0; j < 3; j++)
maetugr 10:953afcbcebfc 90 data[j] -= offset[j];
maetugr 0:0c4fafa398b4 91 }
maetugr 0:0c4fafa398b4 92
maetugr 0:0c4fafa398b4 93 // Reads the gyros Temperature
maetugr 1:5a64632b1eb9 94 int L3G4200D::readTemp()
maetugr 0:0c4fafa398b4 95 {
maetugr 0:0c4fafa398b4 96 return (short) readReg(L3G4200D_OUT_TEMP);
maetugr 0:0c4fafa398b4 97 }