l3g.cpp

00001 // Code for reading from the L3GD20H gyro with I2C.
00002 
00003 #include <mbed.h>
00004 #include "l3g.h"
00005 #include "pc_serial.h"
00006 
00007 I2C i2c(p9, p10);
00008 
00009 const int address = 0xD6;
00010 
00011 int32_t l3gInit()
00012 {
00013     int whoami = l3gReadReg(0x0F);
00014     if (whoami != 0xD7)
00015     {    
00016         pc.printf("l3g whoami error: %d\n", whoami);
00017         return -1;  // wrong ID or no response
00018     }
00019     
00020     // CTRL1: 800 Hz output data rate,
00021     // low-pass filter cutoff 100 Hz
00022     l3gWriteReg(0x20, 0xFF);
00023 
00024     // CTRL4: 2000 dps full scale
00025     l3gWriteReg(0x23, 0x20);
00026 
00027     // CTRL5: High-pass filter disabled
00028     l3gWriteReg(0x24, 0x00);
00029 
00030     return 0;  // success
00031 }
00032 
00033 int32_t l3gReadReg(char reg)
00034 {
00035     int result = i2c.write(address, &reg, 1); 
00036     if (result != 0)
00037     {
00038         return -1;  // error
00039     }
00040 
00041     char data;
00042     result = i2c.read(address, &data, 1);
00043     if (result != 0)
00044     {
00045         return -2;  // error
00046     }
00047   
00048     return data;
00049 }
00050 
00051 int32_t l3gWriteReg(char reg, char value)
00052 {
00053     char data[2];
00054     data[0] = reg;
00055     data[1] = value;
00056     int result = i2c.write(address, data, 2); 
00057     if (result != 0)
00058     {
00059         return -1;  // error
00060     }
00061     
00062     return 0;
00063 }
00064 
00065 int32_t l3gZAvailable()
00066 {
00067     // Read STATUS
00068     int32_t result = l3gReadReg(0x27);
00069     if (result < 0)
00070     {
00071         return result;
00072     }
00073     
00074     // Read the ZDA bit.
00075     return result >> 2 & 1;
00076 }
00077 
00078 int32_t l3gZRead()
00079 {
00080     char reg = 0x80 | 0x2C;  // OUT_Z_L, with slave address updating
00081     int result = i2c.write(address, &reg, 1);
00082     if (result != 0)
00083     {
00084         return -500001;
00085     }
00086     
00087     char data[2];
00088     result = i2c.read(address, data, 2);
00089     if(result != 0)
00090     {
00091         return -500002;
00092     }
00093     
00094     return (int16_t)(data[1] << 8 | data[0]);
00095 }
00096 
00097 // The stuff below doesn't actually have anything to do with the L3G.
00098 
00099 #define L3G_CAL_COUNT_MAX 1000
00100 int8_t l3gCalBuffer[L3G_CAL_COUNT_MAX];
00101 uint32_t l3gCalCount = 0;
00102 uint32_t l3gCalReplayIndex = 0;
00103 
00104 int32_t l3gCalibrate()
00105 {
00106     int32_t reading = l3gZRead();
00107     
00108     if (l3gCalCount < L3G_CAL_COUNT_MAX)
00109     {
00110         int8_t c;
00111         if (reading > 127) { c = 127; }
00112         else if (reading < -128) { c = -128; }
00113         else { c = reading; }
00114         l3gCalBuffer[l3gCalCount++] = c;
00115     }
00116     
00117     return reading;
00118 }
00119 
00120 bool l3gCalibrateDone()
00121 {
00122     return l3gCalCount >= L3G_CAL_COUNT_MAX;
00123 }
00124 
00125 void l3gCalibrateReset()
00126 {
00127     l3gCalCount = 0;
00128 }
00129 
00130 int32_t l3gZReadCalibrated()
00131 {
00132     int8_t zeroRate = 0;
00133     if (l3gCalCount)
00134     {
00135       if (l3gCalReplayIndex >= l3gCalCount) { l3gCalReplayIndex = 0; }
00136       zeroRate = l3gCalBuffer[l3gCalReplayIndex++];
00137     }
00138     
00139     return l3gZRead() - zeroRate;
00140 }