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/Acc/ADXL345.cpp@15:753c5d6a63b3, 2012-10-29 (annotated)

Committer:
maetugr
Date:
Mon Oct 29 16:43:10 2012 +0000
Revision:
15:753c5d6a63b3
Parent:
11:9bf69bc6df45
Child:
17:e096e85f6c36
I2C/Interrupt-Problem gel?st!! erste Versuche an gespannter schnur, falsche PID werte mitten im umschreiben auf I2C_Sensor mutterklasse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 2:93f703d2c4d7 1 #include "ADXL345.h"
maetugr 2:93f703d2c4d7 2 #include "mbed.h"
maetugr 2:93f703d2c4d7 3
maetugr 2:93f703d2c4d7 4 ADXL345::ADXL345(PinName sda, PinName scl) : i2c(sda, scl) {
maetugr 2:93f703d2c4d7 5
maetugr 2:93f703d2c4d7 6 //400kHz, allowing us to use the fastest data rates.
maetugr 2:93f703d2c4d7 7 //there are other chips on board, sorry
maetugr 2:93f703d2c4d7 8 i2c.frequency(400000);
maetugr 2:93f703d2c4d7 9 // initialize the BW data rate
maetugr 2:93f703d2c4d7 10 char tx[2];
maetugr 2:93f703d2c4d7 11 tx[0] = ADXL345_BW_RATE_REG;
maetugr 2:93f703d2c4d7 12 tx[1] = ADXL345_1600HZ; //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register
maetugr 2:93f703d2c4d7 13 i2c.write( ADXL345_WRITE , tx, 2);
maetugr 2:93f703d2c4d7 14
maetugr 2:93f703d2c4d7 15 //Data format (for +-16g) - This is done by setting Bit D3 of the DATA_FORMAT register (Address 0x31) and writing a value of 0x03 to the range bits (Bit D1 and Bit D0) of the DATA_FORMAT register (Address 0x31).
maetugr 2:93f703d2c4d7 16
maetugr 2:93f703d2c4d7 17 char rx[2];
maetugr 2:93f703d2c4d7 18 rx[0] = ADXL345_DATA_FORMAT_REG;
maetugr 2:93f703d2c4d7 19 rx[1] = 0x0B;
maetugr 2:93f703d2c4d7 20 // full res and +_16g
maetugr 2:93f703d2c4d7 21 i2c.write( ADXL345_WRITE , rx, 2);
maetugr 2:93f703d2c4d7 22
maetugr 2:93f703d2c4d7 23 // Set Offset - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
maetugr 2:93f703d2c4d7 24 char x[2];
maetugr 2:93f703d2c4d7 25 x[0] = ADXL345_OFSX_REG ;
maetugr 2:93f703d2c4d7 26 //x[1] = 0xFD;
maetugr 2:93f703d2c4d7 27 x[1] = 0x00;
maetugr 2:93f703d2c4d7 28 i2c.write( ADXL345_WRITE , x, 2);
maetugr 2:93f703d2c4d7 29 char y[2];
maetugr 2:93f703d2c4d7 30 y[0] = ADXL345_OFSY_REG ;
maetugr 2:93f703d2c4d7 31 //y[1] = 0x03;
maetugr 2:93f703d2c4d7 32 y[1] = 0x00;
maetugr 2:93f703d2c4d7 33 i2c.write( ADXL345_WRITE , y, 2);
maetugr 2:93f703d2c4d7 34 char z[2];
maetugr 2:93f703d2c4d7 35 z[0] = ADXL345_OFSZ_REG ;
maetugr 2:93f703d2c4d7 36 //z[1] = 0xFE;
maetugr 2:93f703d2c4d7 37 z[1] = 0x00;
maetugr 2:93f703d2c4d7 38 i2c.write( ADXL345_WRITE , z, 2);
maetugr 2:93f703d2c4d7 39
maetugr 2:93f703d2c4d7 40 // MY INITIALISATION -------------------------------------------------------
maetugr 2:93f703d2c4d7 41
maetugr 2:93f703d2c4d7 42 writeReg(ADXL345_POWER_CTL_REG, 0x00); // set power control
maetugr 2:93f703d2c4d7 43 writeReg(ADXL345_DATA_FORMAT_REG, 0x0B); // set data format
maetugr 2:93f703d2c4d7 44 setDataRate(ADXL345_3200HZ); // set data rate
maetugr 2:93f703d2c4d7 45 writeReg(ADXL345_POWER_CTL_REG, 0x08); // set mode
maetugr 2:93f703d2c4d7 46 }
maetugr 2:93f703d2c4d7 47
maetugr 10:953afcbcebfc 48 void ADXL345::read(){
maetugr 2:93f703d2c4d7 49 char buffer[6];
maetugr 2:93f703d2c4d7 50 readMultiReg(ADXL345_DATAX0_REG, buffer, 6);
maetugr 2:93f703d2c4d7 51
maetugr 10:953afcbcebfc 52 data[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
maetugr 10:953afcbcebfc 53 data[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
maetugr 10:953afcbcebfc 54 data[2] = (short) ((int)buffer[5] << 8 | (int)buffer[4]);
maetugr 10:953afcbcebfc 55
maetugr 10:953afcbcebfc 56 // calculate the angles for roll and pitch (0,1)
maetugr 10:953afcbcebfc 57 float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2));
maetugr 10:953afcbcebfc 58 angle[0] = -(Rad2Deg * acos((float)data[1] / R)-90);
maetugr 10:953afcbcebfc 59 angle[1] = Rad2Deg * acos((float)data[0] / R)-90;
maetugr 10:953afcbcebfc 60 angle[2] = Rad2Deg * acos((float)data[2] / R);
maetugr 2:93f703d2c4d7 61 }
maetugr 2:93f703d2c4d7 62
maetugr 11:9bf69bc6df45 63 void ADXL345::writeReg(char address, char data){
maetugr 2:93f703d2c4d7 64 char tx[2];
maetugr 2:93f703d2c4d7 65 tx[0] = address;
maetugr 2:93f703d2c4d7 66 tx[1] = data;
maetugr 11:9bf69bc6df45 67 i2c.write(ADXL345_WRITE, tx, 2);
maetugr 2:93f703d2c4d7 68 }
maetugr 2:93f703d2c4d7 69
maetugr 2:93f703d2c4d7 70 char ADXL345::readReg(char address){
maetugr 2:93f703d2c4d7 71 char tx = address;
maetugr 2:93f703d2c4d7 72 char output;
maetugr 2:93f703d2c4d7 73 i2c.write( ADXL345_WRITE , &tx, 1); //tell it what you want to read
maetugr 2:93f703d2c4d7 74 i2c.read( ADXL345_READ , &output, 1); //tell it where to store the data
maetugr 2:93f703d2c4d7 75 return output;
maetugr 2:93f703d2c4d7 76 }
maetugr 2:93f703d2c4d7 77
maetugr 2:93f703d2c4d7 78 void ADXL345::readMultiReg(char address, char* output, int size) {
maetugr 15:753c5d6a63b3 79 i2c.write(ADXL345_WRITE, &address, 1, true); //tell it where to read from
maetugr 15:753c5d6a63b3 80 i2c.read(ADXL345_READ , output, size, true); //tell it where to store the data read
maetugr 2:93f703d2c4d7 81 }
maetugr 2:93f703d2c4d7 82
maetugr 11:9bf69bc6df45 83 void ADXL345::setDataRate(char rate) {
maetugr 2:93f703d2c4d7 84 //Get the current register contents, so we don't clobber the power bit.
maetugr 2:93f703d2c4d7 85 char registerContents = readReg(ADXL345_BW_RATE_REG);
maetugr 2:93f703d2c4d7 86
maetugr 2:93f703d2c4d7 87 registerContents &= 0x10;
maetugr 2:93f703d2c4d7 88 registerContents |= rate;
maetugr 2:93f703d2c4d7 89
maetugr 11:9bf69bc6df45 90 writeReg(ADXL345_BW_RATE_REG, registerContents);
maetugr 2:93f703d2c4d7 91 }