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@18:c8c09a3913ba, 2012-11-03 (annotated)

Committer:
maetugr
Date:
Sat Nov 03 07:44:07 2012 +0000
Revision:
18:c8c09a3913ba
Parent:
17:e096e85f6c36
Child:
19:40c252b4a792
mit allem in der I2C_Sensors klasse, zwischenspeichern, da nichts funktioniert

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 2:93f703d2c4d7 1 #include "ADXL345.h"
maetugr 2:93f703d2c4d7 2
maetugr 17:e096e85f6c36 3 ADXL345::ADXL345(PinName sda, PinName scl) : I2C_Sensor(sda, scl, ADXL345_I2C_ADDRESS)
maetugr 17:e096e85f6c36 4 {
maetugr 2:93f703d2c4d7 5 // initialize the BW data rate
maetugr 17:e096e85f6c36 6 writeRegister(ADXL345_BW_RATE_REG, 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 7
maetugr 2:93f703d2c4d7 8 //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 17:e096e85f6c36 9 writeRegister(ADXL345_DATA_FORMAT_REG, 0x0B); // full res and +_16g
maetugr 2:93f703d2c4d7 10
maetugr 2:93f703d2c4d7 11 // Set Offset - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
maetugr 17:e096e85f6c36 12 writeRegister(ADXL345_OFSX_REG, 0x00); // TODO: 0xFD (sein offset! brauch ich auch?)
maetugr 17:e096e85f6c36 13 writeRegister(ADXL345_OFSY_REG, 0x00); // y[1] = 0x03;
maetugr 17:e096e85f6c36 14 writeRegister(ADXL345_OFSZ_REG, 0x00); // z[1] = 0xFE;
maetugr 2:93f703d2c4d7 15
maetugr 17:e096e85f6c36 16 writeRegister(ADXL345_POWER_CTL_REG, 0x00); // set power control
maetugr 17:e096e85f6c36 17 writeRegister(ADXL345_DATA_FORMAT_REG, 0x0B); // set data format
maetugr 17:e096e85f6c36 18 setDataRate(ADXL345_3200HZ); // set data rate
maetugr 17:e096e85f6c36 19 writeRegister(ADXL345_POWER_CTL_REG, 0x08); // set mode
maetugr 2:93f703d2c4d7 20 }
maetugr 2:93f703d2c4d7 21
maetugr 17:e096e85f6c36 22 void ADXL345::read()
maetugr 17:e096e85f6c36 23 {
maetugr 17:e096e85f6c36 24 char buffer[6]; // 8-Bit pieces of axis data
maetugr 2:93f703d2c4d7 25
maetugr 17:e096e85f6c36 26 readMultiRegister(ADXL345_DATAX0_REG, buffer, 6); // read axis registers using I2C
maetugr 17:e096e85f6c36 27
maetugr 17:e096e85f6c36 28 data[0] = (short) (buffer[1] << 8 | buffer[0]); // join 8-Bit pieces to 16-bit short integers
maetugr 17:e096e85f6c36 29 data[1] = (short) (buffer[3] << 8 | buffer[2]);
maetugr 17:e096e85f6c36 30 data[2] = (short) (buffer[5] << 8 | buffer[4]);
maetugr 10:953afcbcebfc 31
maetugr 10:953afcbcebfc 32 // calculate the angles for roll and pitch (0,1)
maetugr 17:e096e85f6c36 33 float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2)); // calculate the absolute of the magnetic field vector
maetugr 18:c8c09a3913ba 34 angle[0] = -(RAD2DEG * acos((float)data[1] / R)-90); // roll - angle of magnetic field vector in x direction
maetugr 18:c8c09a3913ba 35 angle[1] = RAD2DEG * acos((float)data[0] / R)-90; // pitch - angle of magnetic field vector in y direction
maetugr 17:e096e85f6c36 36 angle[2] = RAD2DEG * acos((float)data[2] / R); // angle from magnetic field vector in direction which it has
maetugr 2:93f703d2c4d7 37 }
maetugr 2:93f703d2c4d7 38
maetugr 17:e096e85f6c36 39 void ADXL345::setDataRate(char rate)
maetugr 17:e096e85f6c36 40 {
maetugr 17:e096e85f6c36 41 char registerContents = readRegister(ADXL345_BW_RATE_REG); // get the current register contents, so we don't clobber the power bit
maetugr 2:93f703d2c4d7 42
maetugr 2:93f703d2c4d7 43 registerContents &= 0x10;
maetugr 2:93f703d2c4d7 44 registerContents |= rate;
maetugr 2:93f703d2c4d7 45
maetugr 17:e096e85f6c36 46 writeRegister(ADXL345_BW_RATE_REG, registerContents);
maetugr 2:93f703d2c4d7 47 }