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@10:953afcbcebfc, 2012-10-17 (annotated)

Committer:
maetugr
Date:
Wed Oct 17 08:37:08 2012 +0000
Revision:
10:953afcbcebfc
Parent:
2:93f703d2c4d7
Child:
11:9bf69bc6df45
neue Acc_Winkelberechnung, vor Kompassklassenrevision

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