L3G4200D.cpp

00001 
00002 #include "L3G4200D.h"
00003 
00004 L3G4200D::L3G4200D(PinName mosi, PinName miso, PinName sck, PinName cs) : _spi(mosi, miso, sck), _ncs (cs)
00005 {
00006     _spi.frequency(2000000);
00007     _spi.format(8,3);
00008     _ncs = 1;
00009     wait_us(500);
00010     
00011     oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG1), 0x6F);//-> Digital output data rate(ODR)=200Hz and Cut-Off 50Hz
00012     oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG2), 0x00);
00013     oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG3), 0x00);
00014     oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG4), 0x10); // Full Scale selection = 500 dps
00015     oneByteWrite(L3G4200D_SPI_WRITE | (CTRL_REG5), 0x00);
00016     
00017     wait_us(500);
00018 }
00019 
00020 void L3G4200D::setPowerMode(char mode)
00021 {
00022 }
00023 
00024 int L3G4200D::getWhoAmI()
00025 {
00026     return oneByteRead(L3G4200D_SPI_READ | (WHO_AM_I & 0x3F));
00027 }
00028 
00029 char L3G4200D::getTemperature()
00030 {
00031     return (char)oneByteRead(L3G4200D_SPI_READ | (OUT_TEMP & 0x3F));
00032 }
00033 
00034 void L3G4200D::getAxes(int* axes)
00035 {
00036     getData(axes, 3);
00037 }
00038 
00039 int L3G4200D::getAxisX()
00040 {
00041     int axes[3];
00042     
00043     getData(axes, 3);
00044     
00045     return axes[0];
00046 }
00047 
00048 int L3G4200D::getAxisY()
00049 {
00050     int axes[3];
00051     
00052     getData(axes, 3);
00053     
00054     return axes[1];
00055 }
00056 
00057 int L3G4200D::getAxisZ()
00058 {
00059     int axes[3];
00060     
00061     getData(axes, 3);
00062     
00063     return axes[2];
00064 }
00065 
00066 void L3G4200D::getData(int* dataVector, int vectorSize)
00067 {
00068     char* buffer = new char[vectorSize * 2];
00069     
00070     multiByteRead(L3G4200D_SPI_READ | L3G4200D_SPI_MULTI | (OUT_X_L & 0x3F), buffer, vectorSize * 2);
00071     
00072     for(int i = 0, j = 0; i < vectorSize; i++, j += 2)
00073     {
00074         dataVector[i] = (int16_t)((int)buffer[j+1] << 8 | (int)buffer[j]);
00075     }
00076     
00077     free(buffer);
00078 }
00079 
00080 float L3G4200D::getX()
00081 {
00082     signed short byte = 0;    
00083     byte = (oneByteRead(OUT_X_H)&0xFF)<<8;
00084     byte |= (oneByteRead(OUT_X_L)&0xFF);
00085     return (float)byte;
00086 }
00087 
00088 float L3G4200D::getY()
00089 {
00090     signed short byte = 0;    
00091     byte = (oneByteRead(OUT_Y_H)&0xFF)<<8;
00092     byte |= (oneByteRead(OUT_Y_L)&0xFF);
00093     return (float)byte;
00094 }
00095 
00096 float L3G4200D::getZ()
00097 {
00098     signed short byte = 0;    
00099     byte = (oneByteRead(OUT_Z_H)&0xFF)<<8;
00100     byte |= (oneByteRead(OUT_Z_L)&0xFF);
00101     return (float)byte;
00102 }
00103 
00104 
00105 int L3G4200D::oneByteRead(int address) {
00106  
00107     int tx = (L3G4200D_SPI_READ | (address & 0x3F));
00108     int rx = 0;
00109  
00110     _ncs = 0;
00111     //Send address to read from.
00112     _spi.write(tx);
00113     //Read back contents of address.
00114     rx = _spi.write(0x00);
00115     _ncs = 1;
00116  
00117     return rx;
00118  
00119 }
00120  
00121 void L3G4200D::oneByteWrite(int address, char data) {
00122  
00123     int tx = (L3G4200D_SPI_WRITE | (address & 0x3F));
00124  
00125     _ncs = 0;
00126     //Send address to write to.
00127     _spi.write(tx);
00128     //Send data to be written.
00129     _spi.write(data);
00130     _ncs = 1;
00131  
00132 }
00133  
00134 void L3G4200D::multiByteRead(int startAddress, char* buffer, int size) {
00135  
00136     int tx = (L3G4200D_SPI_READ | L3G4200D_SPI_MULTI | (startAddress & 0x3F));
00137  
00138     _ncs = 0;
00139     //Send address to start reading from.
00140     _spi.write(tx);
00141  
00142     for (int i = 0; i < size; i++) {
00143         buffer[i] = _spi.write(0x00);
00144     }
00145  
00146     _ncs = 1;
00147  
00148 }
00149  
00150 void L3G4200D::multiByteWrite(int startAddress, char* buffer, int size) {
00151  
00152     int tx = (L3G4200D_SPI_WRITE | L3G4200D_SPI_MULTI | (startAddress & 0x3F));
00153  
00154     _ncs = 0;
00155     //Send address to start reading from.
00156     _spi.write(tx);
00157  
00158     for (int i = 0; i < size; i++) {
00159         buffer[i] = _spi.write(0x00);
00160     }
00161  
00162     _ncs = 1;
00163  
00164 }