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@20:e116e596e540, 2012-11-05 (annotated)

Committer:
maetugr
Date:
Mon Nov 05 09:19:01 2012 +0000
Revision:
20:e116e596e540
Parent:
19:40c252b4a792
Child:
21:c2a2e7cbabdd
Eine Achse stabil! bereits abgehoben an Schnur! (erst Gyro im I2C_Sensor)(acos nan abgefangen)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 2:93f703d2c4d7 1 #include "ADXL345.h"
maetugr 20:e116e596e540 2 #include "mbed.h"
maetugr 20:e116e596e540 3
maetugr 20:e116e596e540 4 ADXL345::ADXL345(PinName sda, PinName scl) : i2c(sda, scl) {
maetugr 2:93f703d2c4d7 5
maetugr 20:e116e596e540 6 //400kHz, allowing us to use the fastest data rates.
maetugr 20:e116e596e540 7 //there are other chips on board, sorry
maetugr 20:e116e596e540 8 i2c.frequency(400000);
maetugr 2:93f703d2c4d7 9 // initialize the BW data rate
maetugr 20:e116e596e540 10 char tx[2];
maetugr 20:e116e596e540 11 tx[0] = ADXL345_BW_RATE_REG;
maetugr 20:e116e596e540 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 20:e116e596e540 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 20:e116e596e540 16
maetugr 20:e116e596e540 17 char rx[2];
maetugr 20:e116e596e540 18 rx[0] = ADXL345_DATA_FORMAT_REG;
maetugr 20:e116e596e540 19 rx[1] = 0x0B;
maetugr 20:e116e596e540 20 // full res and +_16g
maetugr 20:e116e596e540 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 20:e116e596e540 24 char x[2];
maetugr 20:e116e596e540 25 x[0] = ADXL345_OFSX_REG ;
maetugr 20:e116e596e540 26 //x[1] = 0xFD;
maetugr 20:e116e596e540 27 x[1] = 0x00;
maetugr 20:e116e596e540 28 i2c.write( ADXL345_WRITE , x, 2);
maetugr 20:e116e596e540 29 char y[2];
maetugr 20:e116e596e540 30 y[0] = ADXL345_OFSY_REG ;
maetugr 20:e116e596e540 31 //y[1] = 0x03;
maetugr 20:e116e596e540 32 y[1] = 0x00;
maetugr 20:e116e596e540 33 i2c.write( ADXL345_WRITE , y, 2);
maetugr 20:e116e596e540 34 char z[2];
maetugr 20:e116e596e540 35 z[0] = ADXL345_OFSZ_REG ;
maetugr 20:e116e596e540 36 //z[1] = 0xFE;
maetugr 20:e116e596e540 37 z[1] = 0x00;
maetugr 20:e116e596e540 38 i2c.write( ADXL345_WRITE , z, 2);
maetugr 2:93f703d2c4d7 39
maetugr 20:e116e596e540 40 // MY INITIALISATION -------------------------------------------------------
maetugr 20:e116e596e540 41
maetugr 20:e116e596e540 42 writeReg(ADXL345_POWER_CTL_REG, 0x00); // set power control
maetugr 20:e116e596e540 43 writeReg(ADXL345_DATA_FORMAT_REG, 0x0B); // set data format
maetugr 20:e116e596e540 44 setDataRate(ADXL345_3200HZ); // set data rate
maetugr 20:e116e596e540 45 writeReg(ADXL345_POWER_CTL_REG, 0x08); // set mode
maetugr 2:93f703d2c4d7 46 }
maetugr 2:93f703d2c4d7 47
maetugr 20:e116e596e540 48 void ADXL345::read(){
maetugr 20:e116e596e540 49 char buffer[6];
maetugr 20:e116e596e540 50 readMultiReg(ADXL345_DATAX0_REG, buffer, 6);
maetugr 2:93f703d2c4d7 51
maetugr 20:e116e596e540 52 data[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
maetugr 20:e116e596e540 53 data[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
maetugr 20:e116e596e540 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 20:e116e596e540 57 float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2));
maetugr 20:e116e596e540 58 float temp[3];
maetugr 20:e116e596e540 59
maetugr 20:e116e596e540 60 temp[0] = -(Rad2Deg * acos((float)data[1] / R)-90);
maetugr 20:e116e596e540 61 temp[1] = Rad2Deg * acos((float)data[0] / R)-90;
maetugr 20:e116e596e540 62 temp[2] = Rad2Deg * acos((float)data[2] / R);
maetugr 20:e116e596e540 63
maetugr 20:e116e596e540 64 for(int i = 0;i < 3; i++)
maetugr 20:e116e596e540 65 if (temp[i] > -360 && temp[i] < 360)
maetugr 20:e116e596e540 66 angle[i] = temp[i];
maetugr 2:93f703d2c4d7 67 }
maetugr 2:93f703d2c4d7 68
maetugr 20:e116e596e540 69 void ADXL345::writeReg(char address, char data){
maetugr 20:e116e596e540 70 char tx[2];
maetugr 20:e116e596e540 71 tx[0] = address;
maetugr 20:e116e596e540 72 tx[1] = data;
maetugr 20:e116e596e540 73 i2c.write(ADXL345_WRITE, tx, 2);
maetugr 20:e116e596e540 74 }
maetugr 20:e116e596e540 75
maetugr 20:e116e596e540 76 char ADXL345::readReg(char address){
maetugr 20:e116e596e540 77 char tx = address;
maetugr 20:e116e596e540 78 char output;
maetugr 20:e116e596e540 79 i2c.write( ADXL345_WRITE , &tx, 1); //tell it what you want to read
maetugr 20:e116e596e540 80 i2c.read( ADXL345_READ , &output, 1); //tell it where to store the data
maetugr 20:e116e596e540 81 return output;
maetugr 20:e116e596e540 82 }
maetugr 20:e116e596e540 83
maetugr 20:e116e596e540 84 void ADXL345::readMultiReg(char address, char* output, int size) {
maetugr 20:e116e596e540 85 i2c.write(ADXL345_WRITE, &address, 1, true); //tell it where to read from
maetugr 20:e116e596e540 86 i2c.read(ADXL345_READ , output, size, true); //tell it where to store the data read
maetugr 20:e116e596e540 87 }
maetugr 20:e116e596e540 88
maetugr 20:e116e596e540 89 void ADXL345::setDataRate(char rate) {
maetugr 20:e116e596e540 90 //Get the current register contents, so we don't clobber the power bit.
maetugr 20:e116e596e540 91 char registerContents = readReg(ADXL345_BW_RATE_REG);
maetugr 2:93f703d2c4d7 92
maetugr 2:93f703d2c4d7 93 registerContents &= 0x10;
maetugr 2:93f703d2c4d7 94 registerContents |= rate;
maetugr 2:93f703d2c4d7 95
maetugr 20:e116e596e540 96 writeReg(ADXL345_BW_RATE_REG, registerContents);
maetugr 2:93f703d2c4d7 97 }